Table of Contents

1.sim3的簡單概述

?

2.sim3算法介紹

1)3對匹配的3D點建立坐標系

2)旋轉矩陣計算

3）平移向量計算

4）尺度計算

3.代碼解析

1）sim3求解器構造函數Sim3Solver

2）sim3迭代求解Sim3Solver::iterate

---

---

本篇筆記對 ORB-SLAM2代碼閱讀筆記（八）：LoopClosing線程 中提到的sim3進行了分析，以熟悉使用三對匹配點求解位姿變換的方法。

1.sim3的簡單概述

sim3簡單來說，就是使用3對匹配點來進行相似變換（similarity transformation）的求解，進而解出兩個坐標系之間的旋轉矩陣、平移向量和尺度。ORB-SLAM2中使用的sim3求解方法來自 Horn 1987, Closed-form solution of absolute orientation using unit quaternions 這篇論文。所以，需要了解一下這篇論文中提出的求解思想，以幫助理解代碼。

ORB-SLAM2系統在LoopClosing線程中，當檢測到閉環(huán)候選幀的時候，就需要對當前關鍵幀和對應的閉環(huán)候選幀之間計算其變換關系。這時需要用當前關鍵幀和其對應的閉環(huán)候選幀進行sim3求解，這里的sim3求解是對當前關鍵幀和閉環(huán)候選幀之間匹配的MapPoint進行sim3求解。通過sim3變換解出當前關鍵幀和閉環(huán)候選幀的匹配MapPoint之間的旋轉矩陣R、平移向量t、尺度變換s，也就得到了當前關鍵幀到閉環(huán)關鍵幀之間的sim3變換gScm。使用這個sim3變換gScm乘上閉環(huán)關鍵幀的sim3位姿gSmw，mg2oScw=gScm*gSmw的乘積mg2oScw就是當前關鍵幀的sim3位姿，之后在閉環(huán)校正中就可以使用這個sim3位姿轉換為SE3位姿后對當前關鍵幀進行位姿校正（當然也要對關鍵幀對應的MapPoints以及其共視的關鍵幀進行校正）。當然在實際代碼中，這個過程還要做多次投影和優(yōu)化操作以確保更高的準確率和精度，這個可以閱讀ORB-SLAM2該部分的代碼來體會。

以下圖為例，A為當前關鍵幀，B為候選關鍵幀。

當相機從B處開始運動到A處的時候，檢測到B為A的閉環(huán)候選幀。此時，考慮到相機從B運動到A的過程中不光會產生旋轉和平移的誤差，同時也會產生尺度漂移的累積，需要計算A和B之間的sim3變換，來找到A和B之間的sim3變換（包括旋轉矩陣R、平移向量t、尺度變換s）,有了這些值之后，就可以對關鍵幀A的位姿進行糾正。

?

2.sim3算法介紹

Closed-form solution of absolute orientation using unit quaternions論文提供了一種算法，通過兩個坐標系之間3對匹配點來確定兩個坐標系之間的變換關系。

1)3對匹配的3D點建立坐標系

如上圖所示，在左右兩個坐標系中各有3個點，左側三個點分別是，，，右側三個點為，，。

可以看出，左側的和右側的都處于坐標系的原點處，則我們可以計算出在X軸、Y軸和Z軸上的單位向量如下：

X軸：x軸上的向量為?，對除以其模長得到在x軸方向上的單位向量

? ? ? ? ?圖中這種情況下為原點，在x軸方向上，所以兩個點相減后所得向量在x軸上，對向量除以模長就是x軸上的單位向量。

Y軸：y軸上的向量為，對除以的模長得到y軸方向上的單位向量

? ? ? ? ?所得為從到這兩個點之間的向量，表示向量在x軸方向上的單位長度，表示該向量在x軸方向上的向量的長度。則向量減去其在x軸方向的向量就得到了在y軸方向上的向量。

Z軸：由于z軸垂直與x軸和y軸，所以z軸所在方向也就是x和y軸方向向量叉乘乘積的方向（兩個向量叉乘后的乘積所在的向量方向和這兩個向量垂直），則z軸方向上的單位向量為

同理，也可推出右側坐標系各個軸上的單位向量。

2)旋轉矩陣計算

根據1）中所得的坐標系單位向量，可以得到左右兩側坐標系的各個方向單位向量構成的矩陣：，

此時，如果左側坐標系中有一個向量，則可以計算結果為在三個坐標軸方向上的向量值。左乘后所得結果為變換到右側坐標系中的向量。

則可以推導出從左側坐標系旋轉到右側坐標系中的旋轉矩陣為：

3）平移向量計算

假設左右坐標系中各有各點，他們在左右兩側坐標系中的測量坐標值為和，的取值從1到。

此時，一個向量從左側坐標系到右側坐標系的變換可表示為：?其中，s為尺度變換，為平移偏移量。表示的旋轉。

實際當中，兩個坐標系之間的變換不會那么容易計算出精確的變換向量，和機器學習中采用方法相同，都是采用優(yōu)化的方法（最小化誤差的方法）來求解的，也就是使用最小二乘法來求解。此時，容易看出這里的誤差為：

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ??

那么，求解的最小二乘問題變成了求解：

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ??

那么，怎么來求解這個最小誤差呢？我們先把要求解的表達式放在這里。這里我們要求解的是平移向量，也就是上邊的。看看作者是怎么引入問題求解方法的。

---

計算左側和右側坐標系中所有點的質心（其實也就是所有點的中心）：

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ??? ? ? ? ?，? ? ? ? ? ?

則每一個點距離質心的距離為：

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?? ? ? ? ? ??

則，我們也可以知道：? ? ? ? ??

根據上邊得出的變換公式可得：

? ? ? ? ? ? ? ? ? ? ? ? ? ??? ??????

? ? ? ? ? ? ? ? ? ? ? ? ? ??

? ? ? ? ? ? ? ? ? ? ? ? ? ?

? ? ? ? ? ? ? ? ? ? ? ? ? ?

此時，優(yōu)化函數如下：

? ? ? ? ? ? ? ? ? ? ?

對上邊的式子，容易計算出中間部分

此時剩下第一部分和第三部分，第一部分和沒關系，第三部分不可能為負值。所以，優(yōu)化函數在的情況下取得最小值。

帶入上式：中，可得平移向量:

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?

4）尺度計算

由于，所以可得誤差函數：

? ? ? ? ? ? ? ? ? ? ?

? ? ? ? ? ? ? ? ? ??

我們進一步可以把這個形式寫成如下形式：

令：

? ? ? ? ? ? ? ? ? ? ? ? ? ??

此時，要是誤差取最小值，則必須第一項為0，此時

? ? ? ? ? ? ? ? ? ? ? ? ? ?

根據對稱性可得：? ? ? ? ? ? ??

我們期望計算出來的值為：? ? ? ? ?

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ??? ? ? ? ? ? ? ? ? ?

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?? ?

?但是，此時

當已知兩個系統中的一個系統的坐標比另一個系統的坐標精度高得多時，上述兩個不對稱結果中的一個可能是合適的。

如果兩組測量中的誤差相似，則對誤差項使用對稱表達式更為合理：

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?

則上邊式子變?yōu)?#xff1a;

該式求最小值，可計算得:

這種對稱結果的一個有點是，它允許人們在不需要知道旋轉的情況下確定尺度。重要的是，旋轉的確定不受我們選擇比例因子的三個值之一的影響。在每種情況下，當D盡可能大時，誤差能取得最小值。也就是說，我們必須選擇使盡可能大的旋轉值。

注意：這里的意思是說，只有在取得最大值的情況下，誤差才會最小，所以為了求得最小誤差，我們應該去求最大的.

---

現在，我們把問題變成了求最大的。

旋轉可以使用多種方式進行表達，此處，引入了四元數來表達。

一個四元數表達如下：

其中為實部，并且滿足：

??????????????????????????????????????????????? ?? ??

??????????????????????????????????????????????? ??

此時兩個四元數的點乘積為：

定義的共軛向量為：

則有：

---

此時，

可以用單位四元數表達為如下形式：

??????????????????????????????????????????????

???? 對該式可改寫為： ?????????

此時，假設?

則，可將定義為以下形式

??????????????????????????????????????????????????

定義了M就是為了用其中的元素來表示N,N定義如下：

? ? ??

此時，對N矩陣進行特征值分解求解最大特征值，該特征值對應的特征向量就是待求的四元數。

四元數轉歐拉角:

根據四元數可以求出旋轉矩陣R、平移向量t和尺度s。

3.代碼解析

在Closed-form solution of absolute orientation using unit quaternions這篇論文的結論部分，作者坦言該論文中的算法表達相對復雜一些，但是好處是一些程序庫中已經實現了該算法的sim3求解，所以對我們使用者來說，只要調用庫函數就行了，這樣就簡單了好多。

ORB-SLAM2中sim3求解流程相關代碼位于LoopClosing::ComputeSim3()中。主要的求解函數如下：

1）sim3求解器構造函數Sim3Solver

Sim3Solver函數用于針對每一個和當前關鍵幀存在閉環(huán)關系的閉環(huán)候選關鍵幀之間構造sim3求解器。求解器中，主要是確定兩個關鍵幀中匹配的MapPoint的對應關系，構造好匹配的MapPoint點對列表，并設置Ransac相關參數用于后續(xù)iterate中求解使用。

Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector<MapPoint *> &vpMatched12, const bool bFixScale):mnIterations(0), mnBestInliers(0), mbFixScale(bFixScale) {mpKF1 = pKF1;mpKF2 = pKF2;vector<MapPoint*> vpKeyFrameMP1 = pKF1->GetMapPointMatches();mN1 = vpMatched12.size();mvpMapPoints1.reserve(mN1);mvpMapPoints2.reserve(mN1);mvpMatches12 = vpMatched12;mvnIndices1.reserve(mN1);mvX3Dc1.reserve(mN1);mvX3Dc2.reserve(mN1);cv::Mat Rcw1 = pKF1->GetRotation();cv::Mat tcw1 = pKF1->GetTranslation();cv::Mat Rcw2 = pKF2->GetRotation();cv::Mat tcw2 = pKF2->GetTranslation();mvAllIndices.reserve(mN1);size_t idx=0;for(int i1=0; i1<mN1; i1++){if(vpMatched12[i1]){MapPoint* pMP1 = vpKeyFrameMP1[i1];MapPoint* pMP2 = vpMatched12[i1];if(!pMP1)continue;if(pMP1->isBad() || pMP2->isBad())continue;//獲取MapPoint中記錄的能看到該MapPoint的keyframe中對應的關鍵點的indexint indexKF1 = pMP1->GetIndexInKeyFrame(pKF1);int indexKF2 = pMP2->GetIndexInKeyFrame(pKF2);if(indexKF1<0 || indexKF2<0)continue;const cv::KeyPoint &kp1 = pKF1->mvKeysUn[indexKF1];const cv::KeyPoint &kp2 = pKF2->mvKeysUn[indexKF2];const float sigmaSquare1 = pKF1->mvLevelSigma2[kp1.octave];const float sigmaSquare2 = pKF2->mvLevelSigma2[kp2.octave];mvnMaxError1.push_back(9.210*sigmaSquare1);mvnMaxError2.push_back(9.210*sigmaSquare2);mvpMapPoints1.push_back(pMP1);mvpMapPoints2.push_back(pMP2);mvnIndices1.push_back(i1);//pMP1對應的相機坐標cv::Mat X3D1w = pMP1->GetWorldPos();mvX3Dc1.push_back(Rcw1*X3D1w+tcw1);cv::Mat X3D2w = pMP2->GetWorldPos();//pMP2對應的相機坐標mvX3Dc2.push_back(Rcw2*X3D2w+tcw2);mvAllIndices.push_back(idx);idx++;}}//相機內參mK1 = pKF1->mK;mK2 = pKF2->mK;//FromCameraToImage函數計算從相機坐標到像素坐標的投影點，mvP1im1為投影點列表。FromCameraToImage(mvX3Dc1,mvP1im1,mK1);FromCameraToImage(mvX3Dc2,mvP2im2,mK2);SetRansacParameters(); }

其中調用的函數FromCameraToImage代碼如下：

/*** 計算從相機坐標到像素坐標的投影 */ void Sim3Solver::FromCameraToImage(const vector<cv::Mat> &vP3Dc, vector<cv::Mat> &vP2D, cv::Mat K) {const float &fx = K.at<float>(0,0);const float &fy = K.at<float>(1,1);const float &cx = K.at<float>(0,2);const float &cy = K.at<float>(1,2);vP2D.clear();//vP3Dc為相機坐標，vP2D為要投影到的像素點坐標vP2D.reserve(vP3Dc.size());for(size_t i=0, iend=vP3Dc.size(); i<iend; i++){const float invz = 1/(vP3Dc[i].at<float>(2));const float x = vP3Dc[i].at<float>(0)*invz;const float y = vP3Dc[i].at<float>(1)*invz;vP2D.push_back((cv::Mat_<float>(2,1) << fx*x+cx, fy*y+cy));} }

2）sim3迭代求解Sim3Solver::iterate

iterate函數是開始進行sim3求解的函數。遍歷1）中構造好的每個求解器，調用求解器中的iterate函數進行求解。ORB-SLAM2中迭代5次進行求解。

cv::Mat Sim3Solver::iterate(int nIterations, bool &bNoMore, vector<bool> &vbInliers, int &nInliers) {bNoMore = false;vbInliers = vector<bool>(mN1,false);nInliers=0;if(N<mRansacMinInliers){bNoMore = true;return cv::Mat();}vector<size_t> vAvailableIndices;cv::Mat P3Dc1i(3,3,CV_32F);cv::Mat P3Dc2i(3,3,CV_32F);int nCurrentIterations = 0;while(mnIterations<mRansacMaxIts && nCurrentIterations<nIterations){nCurrentIterations++;mnIterations++;vAvailableIndices = mvAllIndices;// Get min set of points// 每次隨機獲取3對匹配點for(short i = 0; i < 3; ++i){int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size()-1);int idx = vAvailableIndices[randi];mvX3Dc1[idx].copyTo(P3Dc1i.col(i));mvX3Dc2[idx].copyTo(P3Dc2i.col(i));vAvailableIndices[randi] = vAvailableIndices.back();vAvailableIndices.pop_back();}//進行sim3求解ComputeSim3(P3Dc1i,P3Dc2i);CheckInliers();if(mnInliersi>=mnBestInliers){mvbBestInliers = mvbInliersi;mnBestInliers = mnInliersi;mBestT12 = mT12i.clone();mBestRotation = mR12i.clone();mBestTranslation = mt12i.clone();mBestScale = ms12i;if(mnInliersi>mRansacMinInliers){nInliers = mnInliersi;for(int i=0; i<N; i++)if(mvbInliersi[i])vbInliers[mvnIndices1[i]] = true;return mBestT12;}}}if(mnIterations>=mRansacMaxIts)bNoMore=true;return cv::Mat(); } /*** 計算sim3 */ void Sim3Solver::ComputeSim3(cv::Mat &P1, cv::Mat &P2) {// Custom implementation of:// Horn 1987, Closed-form solution of absolute orientataion using unit quaternions// Step 1: Centroid and relative coordinatescv::Mat Pr1(P1.size(),P1.type()); // Relative coordinates to centroid (set 1)cv::Mat Pr2(P2.size(),P2.type()); // Relative coordinates to centroid (set 2)cv::Mat O1(3,1,Pr1.type()); // Centroid of P1cv::Mat O2(3,1,Pr2.type()); // Centroid of P2ComputeCentroid(P1,Pr1,O1);ComputeCentroid(P2,Pr2,O2);// Step 2: Compute M matrixcv::Mat M = Pr2*Pr1.t();// Step 3: Compute N matrixdouble N11, N12, N13, N14, N22, N23, N24, N33, N34, N44;cv::Mat N(4,4,P1.type());N11 = M.at<float>(0,0)+M.at<float>(1,1)+M.at<float>(2,2);N12 = M.at<float>(1,2)-M.at<float>(2,1);N13 = M.at<float>(2,0)-M.at<float>(0,2);N14 = M.at<float>(0,1)-M.at<float>(1,0);N22 = M.at<float>(0,0)-M.at<float>(1,1)-M.at<float>(2,2);N23 = M.at<float>(0,1)+M.at<float>(1,0);N24 = M.at<float>(2,0)+M.at<float>(0,2);N33 = -M.at<float>(0,0)+M.at<float>(1,1)-M.at<float>(2,2);N34 = M.at<float>(1,2)+M.at<float>(2,1);N44 = -M.at<float>(0,0)-M.at<float>(1,1)+M.at<float>(2,2);N = (cv::Mat_<float>(4,4) << N11, N12, N13, N14,N12, N22, N23, N24,N13, N23, N33, N34,N14, N24, N34, N44);// Step 4: Eigenvector of the highest eigenvaluecv::Mat eval, evec;cv::eigen(N,eval,evec); //evec[0] is the quaternion of the desired rotationcv::Mat vec(1,3,evec.type());(evec.row(0).colRange(1,4)).copyTo(vec); //extract imaginary part of the quaternion (sin*axis)// Rotation angle. sin is the norm of the imaginary part, cos is the real partdouble ang=atan2(norm(vec),evec.at<float>(0,0));vec = 2*ang*vec/norm(vec); //Angle-axis representation. quaternion angle is the halfmR12i.create(3,3,P1.type());cv::Rodrigues(vec,mR12i); // computes the rotation matrix from angle-axis// Step 5: Rotate set 2cv::Mat P3 = mR12i*Pr2;// Step 6: Scaleif(!mbFixScale){double nom = Pr1.dot(P3);cv::Mat aux_P3(P3.size(),P3.type());aux_P3=P3;cv::pow(P3,2,aux_P3);double den = 0;for(int i=0; i<aux_P3.rows; i++){for(int j=0; j<aux_P3.cols; j++){den+=aux_P3.at<float>(i,j);}}ms12i = nom/den;}elsems12i = 1.0f;// Step 7: Translationmt12i.create(1,3,P1.type());mt12i = O1 - ms12i*mR12i*O2;// Step 8: Transformation// Step 8.1 T12mT12i = cv::Mat::eye(4,4,P1.type());cv::Mat sR = ms12i*mR12i;sR.copyTo(mT12i.rowRange(0,3).colRange(0,3));mt12i.copyTo(mT12i.rowRange(0,3).col(3));// Step 8.2 T21mT21i = cv::Mat::eye(4,4,P1.type());cv::Mat sRinv = (1.0/ms12i)*mR12i.t();sRinv.copyTo(mT21i.rowRange(0,3).colRange(0,3));cv::Mat tinv = -sRinv*mt12i;tinv.copyTo(mT21i.rowRange(0,3).col(3)); }

?

總結

以上是生活随笔為你收集整理的ORB-SLAM2代码阅读笔记（十）：sim3求解的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。