openface(四) GetPose

刘咚咚的记事本

已于 2022-04-15 11:11:22 修改

阅读量683

点赞数

分类专栏： openface 文章标签： c++ opencv

于 2021-09-07 15:37:08 首次发布

本文链接：https://blog.csdn.net/weixin_41386168/article/details/120159169

版权

openface 专栏收录该内容

3 篇文章

订阅专栏

本文解析了如何利用CLNF模型的参数计算人脸的3D姿态，包括估计平移(Tx, Ty, Tz)和旋转(Rx, Ry, Rz)。步骤涉及从local参数推导3D点、相机投影、PnP求解和最终姿态转换。关键点在于理解landmark_3D的计算和solvePnP算法的应用。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

cv::Vec6f LandmarkDetector::GetPose(const CLNF& clnf_model, float fx, float fy, float cx, float cy)
{
	//params_global:描述人脸缩放旋转和平移的刚性形状参数(scale,rotx,roty,rotz,tx,ty)
	if (!clnf_model.detected_landmarks.empty() && clnf_model.params_global[0] != 0)
	{
		// This is used as an initial estimate for the iterative PnP algorithm
		float Z = fx / clnf_model.params_global[0];
		float X = ((clnf_model.params_global[4] - cx) * (1.0 / fx)) * Z;
		float Y = ((clnf_model.params_global[5] - cy) * (1.0 / fy)) * Z;

		// Correction for orientation
		// 2D points
		cv::Mat_<float> landmarks_2D = clnf_model.detected_landmarks;
		landmarks_2D = landmarks_2D.reshape(1, 2).t();
		// 3D points
		cv::Mat_<float> landmarks_3D;
		clnf_model.pdm.CalcShape3D(landmarks_3D, clnf_model.params_local);
		landmarks_3D = landmarks_3D.reshape(1, 3).t();

		// Solving the PNP model
		// The camera matrix
		cv::Matx33f camera_matrix(fx, 0, cx, 0, fy, cy, 0, 0, 1);
		cv::Vec3f vec_trans(X, Y, Z);
		cv::Vec3f vec_rot(clnf_model.params_global[1], clnf_model.params_global[2], clnf_model.params_global[3]);
		cv::solvePnP(landmarks_3D, landmarks_2D, camera_matrix, cv::Mat(), vec_rot, vec_trans, true);

		cv::Vec3f euler = Utilities::AxisAngle2Euler(vec_rot);

		return cv::Vec6f(vec_trans[0], vec_trans[1], vec_trans[2], euler[0], euler[1], euler[2]);
	}
	else
	{
		return cv::Vec6f(0, 0, 0, 0, 0, 0);
	}
}

1、估算Tx Ty Tz Rx Ry Rz

2、clnf_model.pdm.CalcShape3D(landmarks_3D, clnf_model.params_local);

根据clnf_model.params_local 求landmarks_3D

3、根据landmarks_3D 与 clnf_model.params_global 求landmarks_2D（投影）

4、计算cv::solvePnP(landmarks_3D, landmarks_2D, camera_matrix, cv::Mat(), vec_rot, vec_trans, true);

求vec_rot（Tx Ty Tz），vec_trans（Rx Ry Rz）

也就是3D人脸模型所在世界坐标系到相机坐标系的旋转平移向量

Tz 就是3D人脸模型所在世界坐标系原点到相机坐标系平行于主轴的距离

float array[68][3] = {

       -74.620308, -21.38689, 84.615593,
        -74.001801, -2.5352249, 82.85746,
        -71.760422, 16.344358, 81.790642,
        -68.076843, 34.488819, 80.089172,
        -61.016972, 50.978931, 75.583359,
        -49.595757, 64.895622, 67.698685,
        -35.347885, 75.66153, 55.404514,
        -19.102194, 83.471436, 40.557095,
        -1.128034, 85.47583, 36.06636,
        16.250248, 83.172989, 42.129261,
        
        31.422182, 75.343559, 56.124622,
        45.145576, 64.725929, 67.718628,
        56.116695, 50.833668, 74.576691,
        63.161697, 33.870422, 77.834366,
        66.985252, 15.405346, 79.229507,
        69.259621, -3.251652, 81.09063,
        70.14904, -21.950649, 84.088486,
        -62.346188, -40.94706, 51.20248,
        -52.51437, -50.355255, 44.216206,
        -39.031582, -53.581615, 37.39011,
        
        -25.249537, -52.618858, 30.341558,
        -12.862496, -48.272217, 24.980661,
        10.829853, -48.976494, 24.896856,
        23.879473, -53.487644, 29.632961,
        37.111805, -54.363171, 35.925106,
        49.964226, -50.887806, 42.297497,
        58.827068, -41.775715, 48.563805,
        -0.57284302, -33.779247, 23.567225,
        -0.421974, -22.57918, 15.797207,
        -0.234579, -11.530055, 7.6640239,
        
        0, 0, 0,
        -15.999255, 11.012796, 16.816057,
        -8.4070225, 13.16613, 13.411376,
        -0.67086297, 14.977441, 11.003612,
        7.0457158, 13.075546, 13.252319,
        13.987568, 11.057587, 16.089903,
        -47.274075, -29.05687, 43.98605,
        -38.90147, -34.315968, 39.969276,
        -29.11064, -34.296974, 38.301689,
        -20.875051, -28.3402, 36.836971,
        
        -29.499748, -26.719952, 36.800789,
        -39.3092, -26.504501, 38.424671,
        18.039085, -28.617765, 36.282314,
        26.667408, -34.927902, 37.195721,
        36.210747, -34.696281, 38.644493,
        43.944023, -29.671974, 41.842224,
        36.969669, -27.117483, 37.231022,
        27.538206, -27.069748, 35.775387,
        -30.143049, 37.027256, 34.707748,
        -18.759977, 30.586727, 21.013725,
        
        -7.9113731, 27.443592, 14.336705,
        -0.84578198, 29.135658, 13.199623,
        7.1486602, 27.450001, 14.226065,
        17.649836, 30.808649, 21.337381,
        27.567629, 36.494465, 33.730667,
        17.830791, 44.712788, 21.960064,
        7.7295918, 48.049114, 14.393815,
        -0.84523398, 48.810188, 13.356434,
        -8.6556778, 48.250946, 14.541954,
        -19.387417, 45.092442, 21.826153,
        
        -25.604273, 37.092312, 32.162376,
        -8.1244164, 33.890518, 16.054317,
        -0.88612002, 34.42881, 14.727581,
        7.2179389, 33.740738, 15.92254,
        23.24769, 36.737549, 31.235285,
        7.222383, 39.010757, 16.276571,
        -1.021461, 39.82328, 15.04439,
        -8.4250488, 39.259418, 16.620037
}


cv::Vec3f vec_trans_test(0, 0, fl);
cv::Vec3f vec_rot_test(0, 0, 0);
cv::Matx33f camera_matrix(fl , 0, dx, 0, fl, dy, 0, 0, 1); 
cv::Matx51f distCoeffs;

vector<Point2f> landmark2d;
for (auto i = 0; i < 68; i++)
    landmark2d.emplace_back(Point2f(lmks[i].x, lmks[i].y));

vector<Point3f> landmark3d;
for (auto i = 0; i < 68; i++)
    landmark3d.emplace_back(Point3f(array[i][0], array[i][1], array[i][2]));


cv::solvePnP(landmark3d, landmark2d, camera_matrix, distCoeffs, vec_rot_test, vec_trans_test, true);

pitch_yaw_roll = AxisAngle2Euler(vec_rot_test);
x_y_z = vec_trans_test;             //pi
distance = int(vec_trans_test[2]);  //mm

AxisAngle2Euler:

cv::Vec3f AxisAngle2Euler(const cv::Vec3f& axis_angle)
{
    cv::Matx33f rotation_matrix;
    cv::Rodrigues(axis_angle, rotation_matrix);
    return RotationMatrix2Euler(rotation_matrix);
}