求解相機的姿態變化有許多的方法，從視覺的角度出發是很重要的一種，這裡我總結了目前主流的基於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. 覺得本文有幫助的可以左上角點贊，或者留言互動。