SLAM基础技术点之基于计算机视觉求解相机姿态变化的方法汇总

　　求解相机的姿态变化有许多的方法，从视觉的角度出发是很重要的一种，这里我总结了目前主流的基于CV的求解相机姿态变化的方法。基于视觉的方法，一般思路为从两个姿态的图像上选取匹配点，根据数据的不同，对应用不同的方法计算两两匹配点的R|T，也即外参，而这个R|T也就是相机两个姿态的变换关系。这里的的方法大体分为这么三类，2d到2d的匹配点、2d到3d的匹配点、3d到3d的匹配点。

1. 二维到二维的匹配
   
   1. Homography（单应矩阵）
   2. Fundamental（基础矩阵）
   3. Essential（本质矩阵）
2. 二维到三维的匹配
   
   1. PnP
3. 三维到三维的匹配
   
   1. ICP

单应矩阵

基于单应矩阵求解，要求匹配点共面且最少四对。根据匹配点求解两两平面的单应性变换关系，即单应矩阵H，然后分解单应矩阵（分解方法稍后细说）即可得到R|T。

//-- 计算单应矩阵
Mat homography_matrix;
homography_matrix = findHomography ( lastPoints, nextPoints, RANSAC, 3 );
cout<<"homography_matrix is "<<endl<<homography_matrix<<endl;

这里使用OpenCV的findHomography求解单应矩阵，其中lastPoints和nextPoints分别为前后两帧一一对应的匹配点。

//-- 分解单应矩阵
vector<Mat> r, t, n;
double intrinsic[9] = { 525.0,       0,          319.5,
                        0,       525.0,   235.5,
                                      0,              0,          1}; //Xtion内参
Mat mIntrinsic(3, 3, CV_64FC1, intrinsic); //相机内参
decomposeHomographyMat(homography_matrix, K, r, t, n);
cout << "========Homography========" << endl;
for(int i=0; i<r.size(); ++i) {
    cout << "======== " << i << " ========" << endl;
    cout << "rotation" << i << " = " << endl;
    cout << r.at(i) << endl;
    cout << "translation" << i << " = " << endl;
    cout << t.at(i) << endl;
}

使用OpenCV3的decomposeHomographyMat函数分解单应矩阵，这里需要注意，只有OpenCV3才有这个方法，且需要输入相机内参。因为分解后会有四组不确定的值，输入的时候要输入std::vector。这里稍微说一下，四组不确定的值，主要是指针孔模型，物体在相机后面，和相机前面在模型上是等价的，所以，这里两个相机姿态，每个都有前后两种情况，组合就是四组R|T。根据物体平面法向量和物体深度应该可以刷掉两个，剩下两个的方法我还没有想到。

---

本质矩阵和基础矩阵

//-- 计算基础矩阵
Mat fundamental_matrix;
fundamental_matrix = findFundamentalMat ( lastPoints, nextPoints, CV_FM_8POINT );
cout<<"fundamental_matrix is "<<endl<< fundamental_matrix<<endl;

//-- 计算本质矩阵
Point2d principal_point ( 325.1, 249.7 );   //相机光心, TUM dataset标定值
double focal_length = 521;          //相机焦距, TUM dataset标定值
Mat essential_matrix;
essential_matrix = findEssentialMat ( lastPoints, nextPoints, focal_length, principal_point );
cout<<"essential_matrix is "<<endl<< essential_matrix<<endl;

//-- 从本质矩阵中恢复旋转和平移信息.
cv::recoverPose ( essential_matrix, lastPoints, nextPoints, R, t, focal_length, principal_point );
cout<<"R is "<<endl<<R<<endl;
cout<<"t is "<<endl<<t<<endl;

本质矩阵和基础矩阵不需要共面的特征点，但是有一个不足，其不能像单应矩阵一样应对pure rotation的姿态变化。因为这两个矩阵是从对极约束的关系中推导出来的，而对极约束在pure rotation的姿态变化关系中，是不成立的。这一点需要谨记，这也是单应矩阵最大的优点。

---

二维到三维PnP

//-- PnP
Mat R_PnP, T_PnP, r_PnP;
solvePnP(pts1, nextPoints, mIntrinsic, Mat(), R_PnP, T_PnP);
cv::Rodrigues ( R_PnP, r_PnP ); // r为旋转向量形式，用Rodrigues公式转换为矩阵

pts1为目标点在第一个相机姿态的三维坐标，nextPoints为对应的目标点在第二个相机姿态中像平面的投影点。mIntrinsic为内参矩阵。输出为旋转向量，需要用罗德里格斯公式转换成旋转矩阵。对于超过三个点对的情况，可以用solvePnPRansac实现更为精确的求解。

---

三维到三维ICP

三维的一般为求解ICP的方法，有SVD求解，和非线性优化求解。

Point3f p1, p2;     // center of mass
int N = lastPoints.size();
for ( int i=0; i<N; i++ )
{
    p1 += lastPoints[i];
    p2 += nextPoints[i];
}
p1 = Point3f( Vec3f(p1) /  N);
p2 = Point3f( Vec3f(p2) / N);
vector<Point3f>     q1 ( N ), q2 ( N ); // remove the center
for ( int i=0; i<N; i++ )
{
    q1[i] = lastPoints[i] - p1;
    q2[i] = nextPoints[i] - p2;
}

// compute q1*q2^T
Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
for ( int i=0; i<N; i++ )
{
    W += Eigen::Vector3d ( q1[i].x, q1[i].y, q1[i].z ) * Eigen::Vector3d ( q2[i].x, q2[i].y, q2[i].z ).transpose();
}
//    cout<<"W="<<W<<endl;

// SVD on W
Eigen::JacobiSVD<Eigen::Matrix3d> svd ( W, Eigen::ComputeFullU|Eigen::ComputeFullV );
Eigen::Matrix3d U = svd.matrixU();
Eigen::Matrix3d V = svd.matrixV();
//    cout<<"U="<<U<<endl;
//    cout<<"V="<<V<<endl;

Eigen::Matrix3d R_ = U* ( V.transpose() );
Eigen::Vector3d t_ = Eigen::Vector3d ( p1.x, p1.y, p1.z ) - R_ * Eigen::Vector3d ( p2.x, p2.y, p2.z );

// convert to cv::Mat
R = ( Mat_<double> ( 3,3 ) <<
      R_ ( 0,0 ), R_ ( 0,1 ), R_ ( 0,2 ),
      R_ ( 1,0 ), R_ ( 1,1 ), R_ ( 1,2 ),
      R_ ( 2,0 ), R_ ( 2,1 ), R_ ( 2,2 )
    );
T = ( Mat_<double> ( 3,1 ) << t_ ( 0,0 ), t_ ( 1,0 ), t_ ( 2,0 ) );

这里参考的是高博的《视觉SLAM十四讲》参考代码

---

旋转矩阵分解成欧拉角

/**
 * @brief getAnglesformR
 * @param rotation 输入旋转矩阵
 * @return 返回角度
 */
bool getAnglesformR(Mat rotation, double &angleX, double &angleY, double &angleZ)
{
    //      theta_{x} = atan2(r_{32}, r_{33})
    angleX = std::atan2(rotation.at<double>(2,1), rotation.at<double>(2, 2));

    //      theta_{y} = atan2(-r_{31}, sqrt{r_{32}^2 + r_{33}^2})
    double tmp0 = rotation.at<double>(2,0);
    double tmp1 = rotation.at<double>(2, 1) * rotation.at<double>(2, 1);
    double tmp2 = rotation.at<double>(2, 2) * rotation.at<double>(2, 2);
    angleY = std::atan2(-tmp0, sqrt(tmp1 + tmp2));

    //      theta_{z} = atan2(r_{21}, r_{11})
    angleZ = std::atan2(rotation.at<double>(1,0), rotation.at<double>(0, 0));

    angleX *= (180/CV_PI);
    angleY *= (180/CV_PI);
    angleZ *= (180/CV_PI);
}

---

相机坐标系示意图

---

关于分解单应矩阵的方法，这里还有一个另一种可以参考泡泡机器人分享的ORB-SLAM2源码分析，单应矩阵分解并选取最优单应矩阵。
分解H矩阵代码
选最优代码

PS
1. 觉得本文有帮助的可以左上角点赞，或者留言互动。