位姿估计(二)：基于PnP的相机位姿估计
PnP (Perspective-n-Point) 是求解3D到2D点运动的方法。他描述了当我们知道n个3D空间点（世界坐标系）以及它们的投影位置时，如何估计相机的姿态。
PnP问题有很多求解方法，例如用三对点估计位姿的P3P，直接线性变换 (DLT)，EPnP (Efficient PnP)，UPnP等。此外，还可以通过非线性优化的方式，构建最小二乘问题并迭代求解，也就是万金油式的Bundle Adjustment
opencv-python Code Example
# https://github.com/xucong-zhang/ETH-XGaze/blob/master/demo.py#L19-L26
def estimateHeadPose(landmarks, face_model, camera, distortion, iterate=True):
ret, rvec, tvec = cv2.solvePnP(face_model, landmarks, camera, distortion, flags=cv2.SOLVEPNP_EPNP)
## further optimize
if iterate:
ret, rvec, tvec = cv2.solvePnP(face_model, landmarks, camera, distortion, rvec, tvec, True)
return rvec, tvec
OpenCV (C++) Code Example
包含头文件：#include <opencv2/calib3d.hpp>
// https://github.com/spmallick/learnopencv/blob/master/HeadPose/headPose.cpp#L31-L42
double focal_length = im.cols; // Approximate focal length.
Point2d center = cv::Point2d(im.cols/2,im.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
cout << "Camera Matrix " << endl << camera_matrix << endl ;
// Output rotation and translation
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);
pose estimation pipeline
- find correspondence (2D-3D点对应)
- solve PnP
- PnP DLT解超定方程 (至少6个点)
- 非线性优化：BA优化
- Rodrigues
cv::Rodrigues(in, out): Converts a rotation matrix to a rotation vector or vice versa. (将旋转向量转为旋转矩阵，或反之)
Blog & Code 🔥🔥🔥
- https://learnopencv.com/head-pose-estimation-using-opencv-and-dlib/
- https://github.com/spmallick/learnopencv/tree/master/HeadPose
Visualize Head Pose
- 首先需要将旋转矩阵转为Euler角，TODO
