《SLAM十四讲》3D-3D位姿估计 pose_estimation_3d3d.cpp（代码详解+原理）

3D-3D的位姿估计的求解需要两张RGB-D图像，经过特征匹配得到配对的特征点，再经过SVD方法求解出相机位姿（R和t）。

P和P’为两组匹配点，pi与p’i分别为两幅RGB-D图像的特征点。

还是先看代码：

void pose_estimation_3d3d(
    const vector<Point3f>& pts1,//第一幅图像3D坐标点
    const vector<Point3f>& pts2,//第二幅图像3D坐标点
    Mat& R, Mat& t
)
{
   
    Point3f p1, p2;//float类型的质心坐标
    int N = pts1.size();
    for ( int i=0; i<N; i++ )
    {
   
        p1 += pts1[i];//pts1[i]为第i对特征点的坐标，对N对特征点坐标进行累加
        p2 += pts2[i];//累加是为了求质心做准备
    }
    p1 /= N; p2 /= N;//累加之后除以N，得到质心坐标
    
    //去质心坐标
    vector<Point3f> q1(N), q2(N);
    for ( int i=0; i<N; i++ )
    {
   
        q1[i] = pts1[i] - p1;