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

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

二维到二维的匹配 Homography（单应矩阵） Fundamental（基础矩阵）Essential（本质矩阵）二维到三维的匹配 PnP三维到三维的匹配 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. 觉得本文有帮助的可以左上角点赞，或者留言互动。