位姿估计(二)：基于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. (将旋转向量转为旋转矩阵，或反之)