人脸对齐和关键点计算估计姿态代码

1.人脸对齐代码

1.1定义结构体

#include <iostream>
#include <opencv2/opencv.hpp>
#include <onnxruntime_cxx_api.h>
#include <vector>
#include <cmath>
#include <filesystem>

using namespace cv;

typedef struct FacePts
{
    float x[5], y[5];
} FacePts;

1.2相似变换函数

cv::Mat similarity_transform(std::vector<cv::Point2f>& from_points, std::vector<cv::Point2f>& to_points)
{
    cv::Mat warp_mat(2, 3, CV_64FC1);
    cv::Point2f mean_from, mean_to;
    double sigma_from = 0, sigma_to = 0;
    cv::Mat cov(2, 2, CV_64FC1);
    cov.setTo(0.0);
    for (int i = 0; i < from_points.size(); ++i)
    {
        mean_from.x += from_points[i].x; mean_from.y += from_points[i].y;
        mean_to.x += to_points[i].x; mean_to.y += to_points[i].y;
    }
    mean_from.x /= from_points.size(); mean_from.y /= from_points.size();
    mean_to.x /= from_points.size(); mean_to.y /= to_points.size();
    //std::cout << "cov:" << cov.ptr<double>(0)[0] << std::endl;
    for (int i = 0; i < from_points.size(); ++i)
    {
        sigma_from += (from_points[i].x - mean_from.x) * (from_points[i].x - mean_from.x) + (from_points[i].y - mean_from.y) * (from_points[i].y - mean_from.y);
        sigma_to += (to_points[i].x - mean_to.x) * (to_points[i].x - mean_to.x) + (to_points[i].y - mean_to.y) * (to_points[i].y - mean_to.y);
        cov.ptr<double>(0)[0] += (to_points[i].x - mean_to.x) * (from_points[i].x - mean_from.x);
        cov.ptr<double>(1)[0] += (to_points[i].y - mean_to.y) * (from_points[i].x - mean_from.x);
        cov.ptr<double>(0)[1] += (to_points[i].x - mean_to.x) * (from_points[i].y - mean_from.y);
        cov.ptr<double>(1)[1] += (to_points[i].y - mean_to.y) * (from_points[i].y - mean_from.y);
    }

    sigma_from /= from_points.size();
    sigma_to /= from_points.size();
    cov.ptr<double>(0)[0] /= from_points.size();
    cov.ptr<double>(0)[1] /= from_points.size();
    cov.ptr<double>(1)[0] /= from_points.size();
    cov.ptr<double>(1)[1] /= from_points.size();
    //std::cout << "cov:" << cov.ptr<double>(0)[0] << std::endl;
    cv::Mat u(2, 2, CV_64FC1);
    cv::Mat v(2, 2, CV_64FC1);
    cv::Mat s(2, 2, CV_64FC1);
    cv::Mat d(2, 2, CV_64FC1);
    cv::Mat dd(2, 2, CV_64FC1);
    cv::SVD::compute(cov, d, u, v);
    //s = identity_matrix(cov);
    s.ptr<double>(1)[1] = 1; s.ptr<double>(0)[0] = 1; s.ptr<double>(0)[1] = 0; s.ptr<double>(1)[0] = 0;
    if (cv::determinant(cov) < 0 || cv::determinant(cov) == 0 && cv::determinant(u) * cv::determinant(v) < 0)
    {
        if (d.ptr<double>(1)[1] < d.ptr<double>(0)[0])
            s.ptr<double>(1)[1] = -1;
        else
            s.ptr<double>(0)[0] = -1;;
    }
    cv::Mat r(2, 2, CV_64FC1);
    v = -v;
    r = -u * s * v;
    double c = 1;
    dd.ptr<double>(0)[0] = d.ptr<double>(0)[0];
    dd.ptr<double>(0)[1] = 0;
    dd.ptr<double>(1)[0] = 0;
    dd.ptr<double>(1)[1] = d.ptr<double>(0)[1];
    if (sigma_from != 0)
        c = 1.0 / sigma_from * trace(dd * s).val[0];
    cv::Mat t(2, 1, CV_64FC1), mean_f(2, 1, CV_64FC1), mean_t(2, 1, CV_64FC1);
    mean_f.ptr<double>(0)[0] = mean_from.x;
    mean_f.ptr<double>(1)[0] = mean_from.y;
    mean_t.ptr<double>(0)[0] = mean_to.x;
    mean_t.ptr<double>(1)[0] = mean_to.y;
    mean_t = mean_t - c * r * mean_f;
    warp_mat.ptr<double>(0)[0] = c * r.ptr<double>(0)[0];
    warp_mat.ptr<double>(0)[1] = c * r.ptr<double>(0)[1];
    warp_mat.ptr<double>(0)[2] = mean_t.ptr<double>(0)[0];
    warp_mat.ptr<double>(1)[0] = c * r.ptr<double>(1)[0];
    warp_mat.ptr<double>(1)[1] = c * r.ptr<double>(1)[1];
    warp_mat.ptr<double>(1)[2] = mean_t.ptr<double>(1)[0];
    return warp_mat;
}

 1.3抠图

cv::Mat Align_and_CropFace(cv::Mat& image, FacePts pts, FacePts& outmarks, const int Size=112, const double padding=0.0f)
{
    cv::Point2f srcTri[5];
    cv::Point2f dstTri[5];
    std::vector<cv::Point2f> src;
    std::vector<cv::Point2f> dst;
    // Average positions of face points 0-4
   /* double mean_face_shape_x[] = {
        0.2281715, 0.7520815, 0.490127, 0.25419, 0.726104
    };
    double mean_face_shape_y[] = {
        0.224078, 0.224078, 0.515625, 0.780233, 0.780233
    };*/
    /* arcface_src = np.array([
        [38.2946, 51.6963],
            [73.5318, 51.5014],
            [56.0252, 71.7366],
            [41.5493, 92.3655],
            [70.7299, 92.2041]], dtype = np.float32)
            */
    double mean_face_shape_x[] = {
       0.341916, 0.656534, 0.500225, 0.370976,0.631517
    };
    double mean_face_shape_y[] = {
        0.461574,0.459834, 0.640505, 0.8246919, 0.823251
    }; 

    for (int i = 0; i < 5; i++)
    {
        mean_face_shape_x[i] = (mean_face_shape_x[i] + padding) / (2 * padding + 1);
        mean_face_shape_y[i] = (mean_face_shape_y[i] + padding) / (2 * padding + 1);

        src.push_back(Point2f(pts.x[i], pts.y[i]));
        dst.push_back(Point2f(mean_face_shape_x[i] * Size, mean_face_shape_y[i] * Size));

    }
    Mat det = similarity_transform(src, dst);
    //	Mat warp_mat0(2, 3, CV_64FC1);

    Mat warp_frame(Size, Size, CV_8UC3);
    warpAffine(image, warp_frame, det, warp_frame.size());

    for (int i = 0; i < 5; i++)
    {
        outmarks.x[i] = det.ptr<double>(0)[0] * pts.x[i] + det.ptr<double>(0)[1] * pts.y[i] + det.ptr<double>(0)[2];
        outmarks.y[i] = det.ptr<double>(1)[0] * pts.x[i] + det.ptr<double>(1)[1] * pts.y[i] + det.ptr<double>(1)[2];
    }

    return warp_frame;
}

1.3判断人脸角度是否过大

bool getFacePose(FacePts facePts, const float seg=2.5)
{
    //eye
    //num0++;
    float dis_eye = (facePts.x[1] - facePts.x[0]) / seg;
    float eye_mid = (facePts.x[1] + facePts.x[0]) / 2;

    //mouth
    int th1 = 1000 * fabs(facePts.x[2] - eye_mid) / dis_eye;
    float dis_mouth = (facePts.x[4] - facePts.x[3]) / seg;
    float mouth_mid = (facePts.x[4] + facePts.x[3]) / 2;

    int th2 = 1000 * abs(facePts.x[2] - mouth_mid) / dis_mouth;
    //noise
    float dis_noise = (facePts.y[3] + facePts.y[4]) * 0.5 - (facePts.y[0] + facePts.y[1]) * 0.5;
    float dis_noise_ratio = fabs(((facePts.y[3] + facePts.y[4]) * 0.5 - facePts.y[2]) / (facePts.y[2] - ((facePts.y[0] + facePts.y[1]) * 0.5)));
    int th3 = 1000 * dis_noise_ratio;
    //point_ << name << " " << fabs(facePts.y[3] - facePts.y[4]) << " " << fabs(facePts.x[2] - eye_mid) << " " << dis_eye << " " << fabs(facePts.x[2] - mouth_mid) << " " << dis_mouth << " " << dis_noise_ratio << std::endl;
    //ratio:1.5 0.1 change to 1.35 0.15
    if (/*dis_noise_ratio > 2.55 || dis_noise_ratio < 0.27 ||*/ fabs(facePts.x[2] - eye_mid) > dis_eye || fabs(facePts.x[2] - mouth_mid) > dis_mouth)
    {
        //imwrite("./save/0/" +num2str(num0)+"_" +num2str(th1)+"_"+num2str(th2)+"_"+num2str(th3)+".jpg",temp);
        return false;
    }

    else
    {
        if (dis_mouth < 21)
            return false;
        //imwrite("./save/1/" + num2str(num0) +"_"+ num2str(th1) + "_" + num2str(th2) + "_" + num2str(th3) + ".jpg", temp);
        else
            return true;
    }

}

2.人脸姿态估计代码

// 姿态估计算法，主要用以估计输入目标的三维欧拉角，其大致可分为两类：一类是通过2D标定信息来估计3D姿态信息的算法，如先计算人脸的关键点，然后选取一个参考系（平均正脸的关键点），计算关键点和参考系的变换矩阵，然后通过迭代优化的算法来估计人脸的姿态(可参考opencv中的SolvePnP算法), 另一类就是通过数据驱动的方式训练一个回归器，由该回归器对输入人脸的块进行一个直接的预测。两类方法各有利弊，第一类算法为landmark-based，第二类为landmark-free

//原文链接：https ://blog.csdn.net/minstyrain/article/details/82391052
//计算用人脸关键点坐标所在参考图像
//yaw是double三维旋转之左右旋转角[-90(左), 90(右)]
//pitch是double三维旋转之俯仰角度[-90(上), 90(下)]
//roll是	double平面内旋转角[-180(逆时针), 180(顺时针)]
std::vector<float> getFacePose(std::vector<cv::Point>& landmarks, cv::Mat img_face)
{
    std::vector<float> facePose;//pitch yaw row
    // 3D model points.
    std::vector<cv::Point3d> model_points;
    model_points.push_back(cv::Point3d(0.0f, 0.0f, 0.0f));               // Nose tip
    model_points.push_back(cv::Point3d(-165.0f, 170.0f, -135.0f));
    model_points.push_back(cv::Point3d(165.0f, 170.0f, -135.0f));
    model_points.push_back(cv::Point3d(-150.0f, -150.0f, -125.0f));
    model_points.push_back(cv::Point3d(150.0f, -150.0f, -125.0f));
    //model_points.push_back(cv::Point3d(0.0f, -330.0f, -65.0f));          // Chin
    //model_points.push_back(cv::Point3d(-225.0f, 170.0f, -135.0f));       // Left eye left corner
    //model_points.push_back(cv::Point3d(225.0f, 170.0f, -135.0f));        // Right eye right corner
    //model_points.push_back(cv::Point3d(-150.0f, -150.0f, -125.0f));      // Left Mouth corner
    //model_points.push_back(cv::Point3d(150.0f, -150.0f, -125.0f));       // Right mouth corner

    std::vector<cv::Point2d> image_points;
    //image_points.push_back(cv::Point2d(landmarks[2].x, landmarks[2].y)); 
    float cent_eye = landmarks[0].y / 2 + landmarks[1].y / 2;
    float cent_mouth = landmarks[4].y / 2 + landmarks[3].y / 2;
   // image_points.push_back(cv::Point2d(landmarks[3].x / 2 + landmarks[4].x / 2, cent_mouth + 1.04 * (cent_mouth - landmarks[2].y)));    // Chin 1.03倍嘴巴到鼻子距离
 //   image_points.push_back(cv::Point2d(landmarks[0].x, landmarks[0].y));     // Left eye left corner
  //  image_points.push_back(cv::Point2d(landmarks[1].x, landmarks[1].y));    // Right eye right corner
  //  image_points.push_back(cv::Point2d(landmarks[3].x, landmarks[3].y));    // Left Mouth corner
  //  image_points.push_back(cv::Point2d(landmarks[4].x, landmarks[4].y));    // Right mouth corne
    image_points.push_back(cv::Point2d(landmarks[2].x, landmarks[2].y));
    image_points.push_back(cv::Point2d(landmarks[0].x, landmarks[0].y));
    image_points.push_back(cv::Point2d(landmarks[1].x, landmarks[1].y));
    image_points.push_back(cv::Point2d(landmarks[3].x, landmarks[3].y));
    image_points.push_back(cv::Point2d(landmarks[4].x, landmarks[4].y));
  // Camera internals
    double focal_length = img_face.cols; // Approximate focal length.
    double focal_length_x = img_face.cols; // Approximate focal length.
    double focal_length_y = img_face.rows; // Approximate focal length.
    //focal_length = img_face.cols / tan(60 / 2 * CV_PI / 180);
    focal_length = img_face.cols;
    cv::Point2d center = cv::Point2d(img_face.cols / 2, img_face.rows / 2);
    cv::Mat camera_matrix = (cv::Mat_<double>(3, 3) << focal_length, 0, center.x, 0, focal_length, center.y, 0, 0, 1);
    cv::Mat dist_coeffs = cv::Mat::zeros(4, 1, cv::DataType<double>::type); // Assuming no lens distortion

    cv::Mat rotation_vector; // Rotation in axis-angle form
    cv::Mat translation_vector;

    // Solve for pose
    cv::solvePnP(model_points, image_points, camera_matrix, dist_coeffs, rotation_vector, translation_vector,false, SOLVEPNP_SQPNP);


    //cv2::Rodrigues
    /*
    # 从旋转向量转换为欧拉角
def get_euler_angle(rotation_vector):
    # calculate rotation angles
    theta = cv2.norm(rotation_vector, cv2.NORM_L2)

    # transformed to quaterniond
    w = math.cos(theta / 2)
    x = math.sin(theta / 2)*rotation_vector[0][0] / theta
    y = math.sin(theta / 2)*rotation_vector[1][0] / theta
    z = math.sin(theta / 2)*rotation_vector[2][0] / theta

    ysqr = y * y
    # pitch (x-axis rotation)
    t0 = 2.0 * (w * x + y * z)
    t1 = 1.0 - 2.0 * (x * x + ysqr)
    print('t0:{}, t1:{}'.format(t0, t1))
    pitch = math.atan2(t0, t1)

    # yaw (y-axis rotation)
    t2 = 2.0 * (w * y - z * x)
    if t2 > 1.0:
        t2 = 1.0
    if t2 < -1.0:
        t2 = -1.0
    yaw = math.asin(t2)

    # roll (z-axis rotation)
    t3 = 2.0 * (w * z + x * y)
    t4 = 1.0 - 2.0 * (ysqr + z * z)
    roll = math.atan2(t3, t4)

    print('pitch:{}, yaw:{}, roll:{}'.format(pitch, yaw, roll))

    # 单位转换：将弧度转换为度
    Y = int((pitch/math.pi)*180)
    X = int((yaw/math.pi)*180)
    Z = int((roll/math.pi)*180)

    return 0, Y, X, Z
    //原文链接：https ://blog.csdn.net/u014090429/article/details/100762308
    */
    //2）旋转向量— > 四元数— > 欧拉角
    double theta = cv::norm(rotation_vector,NORM_L2);
    // transformed to quaterniond
    float w = cos(theta / 2);
    float x = sin(theta / 2) * rotation_vector.at<double>(0, 0) / theta;
    float y = sin(theta / 2) * rotation_vector.at<double>(1, 0) / theta;
    float z = sin(theta / 2) * rotation_vector.at<double>(2, 0) / theta;

    float ysqr = y * y;

    // pitch(x - axis rotation)
    float t0 = 2.0 * (w * x + y * z);
    float t1 = 1.0 - 2.0 * (x * x + ysqr);
    printf("t0 = % f, t1 = % f\n", t0, t1);
    float pitch = atan2(t0, t1);

    // yaw(y - axis rotation)
    float t2 = 2.0 * (w * y - z * x);
    if (t2 > 1.0)
    {
        t2 = 1.0;
    }
    if (t2 < -1.0)
    {
        t2 = -1.0;
    }

    float yaw = asin(t2);

    // roll(z - axis rotation)
    float t3 = 2.0 * (w * z + x * y);
    float t4 = 1.0 - 2.0 * (ysqr + z * z);
    float roll = atan2(t3, t4);

    printf("pitch =%f, yaw =%f,roll =%f\n", pitch, yaw, roll);

    // 单位转换：将弧度转换为度
    float Y = float((pitch / CV_PI) * 180);
    float X = float((yaw / CV_PI) * 180);
    float Z = float((roll / CV_PI) * 180);
    if (X > 90)
    {
        X = (X - 180);
    }
    if (X < -90)
    {
        X = (X + 180);
    }
    
    if (Y > 90)
    {
        Y = (Y - 180);
    }
    if (Y < -90)
    {
        Y = (Y + 180);
    }

    if (Z > 90)
    {
        Z = (Z - 180) ;
    }
    if (Z < -90)
    {
        Z = (Z + 180) ;
    }
    facePose.push_back(X);
    facePose.push_back(Y);
    facePose.push_back(Z);
    printf("X =%f, Y =%f,Z =%f\n", X, Y, Z);

     //method2
   /* float X1 = getYaw(landmarks[2].x,0,img_face.cols);
    float Y1 = getPitch(landmarks[2].y,0, img_face.rows);*/
    float X1 = getYaw(landmarks[2].x, 503, 38);
    float Y1 = getPitch(landmarks[2].y, 194, 61); 
    float Z1 = getRoll(landmarks[0].x, landmarks[0].y, landmarks[1].x, landmarks[1].y);
    printf("X1 =%f, Y1 =%f,Z1 =%f\n", X1, Y1, Z1);
   
    //method3
    cv::Mat rotateMat; // 旋转向量
    cv::Rodrigues(rotation_vector, rotateMat); // 转换为旋转矩阵
    cv::Mat proj_matrix;
    cv::hconcat(rotateMat, translation_vector, proj_matrix);
    cv::Mat euler_angle = cv::Mat(3, 1, CV_64FC1);
    //temp buf for decomposeProjectionMatrix()
    cv::Mat out_intrinsics = cv::Mat(3, 3, CV_64FC1);
    cv::Mat out_rotation = cv::Mat(3, 3, CV_64FC1);
    cv::Mat out_translation = cv::Mat(3, 1, CV_64FC1);

    cv::Mat R, t, S;
    cv::decomposeProjectionMatrix(proj_matrix, out_intrinsics, out_rotation, out_translation, cv::noArray(), cv::noArray(), cv::noArray(), euler_angle);
    float X2= euler_angle.at<double>(0);
    float Y2 = euler_angle.at<double>(1);
    float Z2 = euler_angle.at<double>(2);
    if (X2 > 90)
    {
        X2 = (X2 - 180);
    }
    if (X2 < -90)
    {
        X2 = (X2 + 180);
    }

    if (Y2 > 90)
    {
        Y2 = (Y2 - 180);
    }
    if (Y2 < -90)
    {
        Y2 = (Y2 + 180);
    }

    if (Z2 > 90)
    {
        Z2 = (Z2 - 180);
    }
    if (Z2 < -90)
    {
        Z2 = (Z2 + 180);
    }
   // matrix2angle(rotateMat,X2,Y2,Z2);
    printf("X2 =%f, Y2 =%f,Z2 =%f\n", X2, Y2, Z2);
    return facePose;
}
void test_getFacePoseByLandmark()
{
    cv::Mat image = cv2_imread("1.jpg");
   
    std::vector<cv::Point> landmarks;
    //1.jpg
    
    landmarks.push_back(cv::Point(90,318));
    landmarks.push_back(cv::Point(178, 317));
    landmarks.push_back(cv::Point(130, 365));
    landmarks.push_back(cv::Point(97, 407));
    landmarks.push_back(cv::Point(160, 406));
    FacePts pts,outpts;
    pts.x[0] = 90; pts.y[0] = 318;
    pts.x[1] = 178; pts.y[1] = 317;
    pts.x[2] = 130; pts.y[2] = 365;
    pts.x[3] = 97; pts.y[3] = 407;
    pts.x[4] = 160; pts.y[4] = 406;
    bool pose = getFacePose(pts);
    printf("pose:%d\n", pose);
    cv::Mat alignimg = Align_and_CropFace(image, pts, outpts);
    cv::imshow("align", alignimg);
    cv::waitKey(0);
   //11.jpg
    
 /*  landmarks.push_back(cv::Point(50,92));
   landmarks.push_back(cv::Point(138, 93));
   landmarks.push_back(cv::Point(96, 140));
   landmarks.push_back(cv::Point(65, 179));
   landmarks.push_back(cv::Point(124, 180));*/
   
    //33.jpg -1
    
 /*   landmarks.push_back(cv::Point(221, 245));
    landmarks.push_back(cv::Point(229, 233));
    landmarks.push_back(cv::Point(235, 238));
    landmarks.push_back(cv::Point(232, 263));
    landmarks.push_back(cv::Point(240, 252));
    FacePts pts, outpts;
    pts.x[0] = 221; pts.y[0] = 245;
    pts.x[1] = 229; pts.y[1] = 233;
    pts.x[2] = 235; pts.y[2] = 238;
    pts.x[3] = 232; pts.y[3] = 263;
    pts.x[4] = 240; pts.y[4] = 252;
    bool pose = getFacePose(pts);
    printf("pose:%d\n", pose);
    cv::Mat alignimg = Align_and_CropFace(image, pts, outpts);
    cv::imshow("align", alignimg);
    cv::waitKey(0);*/ 
     33.jpg -2
    /*landmarks.push_back(cv::Point(509, 209));
    landmarks.push_back(cv::Point(524, 227));
    landmarks.push_back(cv::Point(511, 227));
    landmarks.push_back(cv::Point(507, 239));
    landmarks.push_back(cv::Point(516, 243));
    FacePts pts, outpts;
    pts.x[0] = 509; pts.y[0] = 209;
    pts.x[1] = 524; pts.y[1] = 227;
    pts.x[2] = 511; pts.y[2] = 227;
    pts.x[3] = 507; pts.y[3] = 239;
    pts.x[4] = 516; pts.y[4] = 243;
    bool pose = getFacePose(pts);
    printf("pose:%d\n", pose);
    cv::Mat alignimg = Align_and_CropFace(image, pts, outpts);
    cv::imshow("align", alignimg);
    cv::waitKey(0);*/
    std::vector<float> facePose;
    facePose = getFacePose(landmarks,image);
}
int main() 
{
    test_getFacePoseByLandmark();
    //run_face_3dkps();
    return 0;
}