一、sim3的應(yīng)用場(chǎng)景

1.1 slam中sim3的應(yīng)用

sim3簡(jiǎn)單來(lái)說(shuō)，就是使用3對(duì)匹配點(diǎn)來(lái)進(jìn)行相似變換（similarity transformation）的求解，進(jìn)而解出兩個(gè)坐標(biāo)系之間的旋轉(zhuǎn)矩陣、平移向量和尺度。ORB-SLAM2中使用的sim3求解方法來(lái)自 Horn 1987, Closed-form solution of absolute orientation using unit quaternions 這篇論文。
?

ORB-SLAM2系統(tǒng)在LoopClosing線程中，當(dāng)檢測(cè)到閉環(huán)候選幀的時(shí)候，就需要對(duì)當(dāng)前關(guān)鍵幀和對(duì)應(yīng)的閉環(huán)候選幀之間計(jì)算其變換關(guān)系。意思就是說(shuō)，這些關(guān)鍵幀本來(lái)應(yīng)該是連續(xù)的，但由于累計(jì)誤差的存在，現(xiàn)在不連續(xù)了，需求求解出他們的偏差出來(lái)，需要計(jì)算出來(lái)他們之間的旋轉(zhuǎn)和平移，然后使他們連續(xù)。

這時(shí)需要用當(dāng)前關(guān)鍵幀和其對(duì)應(yīng)的閉環(huán)候選幀進(jìn)行sim3求解，這里的sim3求解是對(duì)當(dāng)前關(guān)鍵幀和閉環(huán)候選幀之間匹配的MapPoint進(jìn)行sim3求解。

通過(guò)sim3變換解出當(dāng)前關(guān)鍵幀和閉環(huán)候選幀的匹配MapPoint之間的旋轉(zhuǎn)矩陣R、平移向量t、尺度變換s，也就得到了當(dāng)前關(guān)鍵幀到閉環(huán)關(guān)鍵幀之間的sim3變換 gScm。

使用這個(gè)sim3變換gScm乘上閉環(huán)關(guān)鍵幀的sim3位姿 gSmw，

mg2oScw=gScm*gSmw

的乘積mg2oScw就是當(dāng)前關(guān)鍵幀的sim3位姿，之后在閉環(huán)校正中就可以使用這個(gè)sim3位姿轉(zhuǎn)換為SE3位姿后對(duì)當(dāng)前關(guān)鍵幀進(jìn)行位姿校正

（當(dāng)然也要對(duì)關(guān)鍵幀對(duì)應(yīng)的MapPoints以及其共視的關(guān)鍵幀進(jìn)行校正）。

當(dāng)然在實(shí)際代碼中，這個(gè)過(guò)程還要做多次投影和優(yōu)化操作以確保更高的準(zhǔn)確率和精度，這個(gè)可以閱讀ORB-SLAM2該部分的代碼來(lái)體會(huì)。

上邊是在其他博客摘錄的，覺(jué)得寫的很好。

??????(15條消息) ORB-SLAM2代碼閱讀筆記（十）：sim3求解_文科升的博客-CSDN博客_sim3

1.2 gis中sim3的應(yīng)用

在考慮下另外一個(gè)應(yīng)用的場(chǎng)景，就是slam領(lǐng)域的問(wèn)題，應(yīng)該屬于gis領(lǐng)域的問(wèn)題了。現(xiàn)在我們有個(gè)模型，默認(rèn)模型有模型的原點(diǎn)，在三維場(chǎng)景中擺放模型時(shí)候，模型原點(diǎn)就擺放在對(duì)應(yīng)的世界坐標(biāo)處。而模型的擺放姿態(tài)一般就和制作模型時(shí)候3dmax之類的建模軟件建模又關(guān)系了，可能理解為模型制作完成之后，就帶有初始姿態(tài)了。那么現(xiàn)在有這樣的需求，就是需要我在場(chǎng)景（地圖）點(diǎn)選好模型擺放的對(duì)應(yīng)點(diǎn)，比如點(diǎn)4個(gè)點(diǎn)對(duì)應(yīng)房子模型的四個(gè)角。除了這四個(gè)點(diǎn)分別對(duì)應(yīng)哪四個(gè)角，另外的就是要計(jì)算如何計(jì)算出合適的姿態(tài)講模型擺放到點(diǎn)擊的四個(gè)點(diǎn)位處。

分解下問(wèn)題：

模型原始坐標(biāo)---(相對(duì)于模型原點(diǎn)的偏移+需要擺放的世界坐標(biāo))--->

模型在世界坐標(biāo)系下的初始狀態(tài)?---(根據(jù)點(diǎn)選了四個(gè)點(diǎn)以及四個(gè)點(diǎn)的房子的匹配關(guān)系計(jì)算R、T)--->

模型的新位置的坐標(biāo)與姿態(tài)（求解從初始位置到目前位置的旋轉(zhuǎn)和平移）

實(shí)現(xiàn)原理和過(guò)程可以看這篇博客。

（備注：這里的變換是歐式變換，當(dāng)前也可以理解為尺度為1的相似變換，下邊會(huì)說(shuō)到orbslam2單目閉環(huán)檢測(cè)環(huán)節(jié)使用的是相似變換）

(14條消息) 3對(duì)對(duì)應(yīng)點(diǎn)求解選擇和平移_謝大旭的博客-CSDN博客

二、sim3原理

2.1?sim3來(lái)由來(lái)

Closed-form solution of absolute orientation using unit quaternions論文提供了一種算法，通過(guò)兩個(gè)坐標(biāo)系之間3對(duì)匹配點(diǎn)來(lái)確定兩個(gè)坐標(biāo)系之間的變換關(guān)系。如圖

文章地址：

??????Absolute_Orientation.pdf (mit.edu)

2.2 相似變換與歐式變換

歐式變換

相似變換

總結(jié)：

相似變換的特點(diǎn)是改變?cè)疚矬w的尺度比例(保形狀)，所以這個(gè)S就是要乘在R上，如果在t上，那只是對(duì)改變物體的平移量，并不改變形狀的尺度比例。從公式可以看出，相似變換和歐式變換只是差了一個(gè)尺度，這就解釋了為什么orbslam2在單目攝像頭閉環(huán)檢測(cè)環(huán)節(jié)使用的是使用的是相似變換，因?yàn)閱文繘](méi)有深度信息，所對(duì)應(yīng)的變換是相似變換。

除此之外，還有別的變換。比如透視投影變換、仿射變換。這里就不展開(kāi)講先。

2.3 orbslam2中為什么需要使用到相似變換（sim3）

常見(jiàn)的歐式變換由6個(gè)自由度(R,TR,T各三個(gè))，在SLAM問(wèn)題中，可能會(huì)存在尺度的改變，因此自由度就變成了7個(gè)，見(jiàn)下圖：

Sim3(Similarity Transformation)的提出就是為了解決兩個(gè)坐標(biāo)系之間的相似變換問(wèn)題，只要我們能得到3對(duì)匹配好的點(diǎn)在兩個(gè)坐標(biāo)系下的坐標(biāo)，我們就能解出相似變換。

從幾何上理解：在歐式變換中，我們只需要兩對(duì)匹配的點(diǎn)就可以求解，因?yàn)閮蓚€(gè)點(diǎn)構(gòu)成向量，通過(guò)比較向量在兩個(gè)空間的平移量和旋轉(zhuǎn)量，就可以得到歐式變換的結(jié)果。而在相似變換中，通過(guò)增加一個(gè)點(diǎn)，三個(gè)點(diǎn)構(gòu)成三角形，通過(guò)法向量能計(jì)算旋轉(zhuǎn)量，通過(guò)面積相似性能計(jì)算尺度變化，利用R,sR,s將兩個(gè)面平行，通過(guò)計(jì)算面距離得到平移量。（總計(jì)下這里：這里回答的是，相似變換下怎么求解旋轉(zhuǎn)和平移問(wèn)題？）

從代數(shù)上理解：兩組三維點(diǎn)，能構(gòu)成6個(gè)方程，解6個(gè)未知數(shù)；三組三維點(diǎn)，構(gòu)成9個(gè)方程，超定方程解7個(gè)未知數(shù)。

在SLAM中主要用于回環(huán)檢測(cè)，因?yàn)榛丨h(huán)過(guò)程中會(huì)產(chǎn)生尺度的漂移，需要計(jì)算當(dāng)前幀和候選幀之間的sim3變換

?Sim3變換 | JR's Blog | 直掛云帆濟(jì)滄海 (jiangren.work)

三、orbslam2中的相似變化求解器實(shí)現(xiàn)

首先是在閉環(huán)檢測(cè)的主線程中進(jìn)行sim3計(jì)算，這里計(jì)算方法出處是

Horn 1987, Closed-form solution of absolute orientataion using unit quaternions

后邊再找時(shí)間詳細(xì)閱讀，目前在整理代碼框架階段。

/*** @brief 根據(jù)兩組匹配的3D點(diǎn),計(jì)算P2到P1的Sim3變換* @param[in] P1 匹配的3D點(diǎn)(三個(gè),每個(gè)的坐標(biāo)都是列向量形式,三個(gè)點(diǎn)組成了3x3的矩陣)(當(dāng)前關(guān)鍵幀)* @param[in] P2 匹配的3D點(diǎn)(閉環(huán)關(guān)鍵幀)*/ void Sim3Solver::ComputeSim3(cv::Mat &P1, cv::Mat &P2) {// Sim3計(jì)算過(guò)程參考論文:// Horn 1987, Closed-form solution of absolute orientataion using unit quaternions// Step 1: 定義3D點(diǎn)質(zhì)心及去質(zhì)心后的點(diǎn)// O1和O2分別為P1和P2矩陣中3D點(diǎn)的質(zhì)心// Pr1和Pr2為減去質(zhì)心后的3D點(diǎn)cv::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: 計(jì)算論文中三維點(diǎn)數(shù)目n>3的 M 矩陣。這里只使用了3個(gè)點(diǎn)// Pr2 對(duì)應(yīng)論文中 r_l,i'，Pr1 對(duì)應(yīng)論文中 r_r,i',計(jì)算的是P2到P1的Sim3，論文中是left 到 right的Sim3cv::Mat M = Pr2*Pr1.t();// Step 3: 計(jì)算論文中的 N 矩陣double 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); // Sxx+Syy+SzzN12 = M.at<float>(1,2)-M.at<float>(2,1); // Syz-SzyN13 = M.at<float>(2,0)-M.at<float>(0,2); // Szx-SxzN14 = 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: 特征值分解求最大特征值對(duì)應(yīng)的特征向量，就是我們要求的旋轉(zhuǎn)四元數(shù)cv::Mat eval, evec; // val vec// 特征值默認(rèn)是從大到小排列，所以evec[0] 是最大值cv::eigen(N,eval,evec); // N 矩陣最大特征值（第一個(gè)特征值）對(duì)應(yīng)特征向量就是要求的四元數(shù)（q0 q1 q2 q3），其中q0 是實(shí)部// 將(q1 q2 q3)放入vec（四元數(shù)的虛部）cv::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 part// 四元數(shù)虛部模長(zhǎng) norm(vec)=sin(theta/2), 四元數(shù)實(shí)部 evec.at<float>(0,0)=q0=cos(theta/2)// 這一步的ang實(shí)際是theta/2，theta 是旋轉(zhuǎn)向量中旋轉(zhuǎn)角度// ? 這里也可以用 arccos(q0)=angle/2 得到旋轉(zhuǎn)角吧double ang=atan2(norm(vec),evec.at<float>(0,0));// vec/norm(vec)歸一化得到歸一化后的旋轉(zhuǎn)向量,然后乘上角度得到包含了旋轉(zhuǎn)軸和旋轉(zhuǎn)角信息的旋轉(zhuǎn)向量vecvec = 2*ang*vec/norm(vec); //Angle-axis x. quaternion angle is the halfmR12i.create(3,3,P1.type());// 旋轉(zhuǎn)向量（軸角）轉(zhuǎn)換為旋轉(zhuǎn)矩陣cv::Rodrigues(vec,mR12i); // computes the rotation matrix from angle-axis// Step 5: Rotate set 2// 利用剛計(jì)算出來(lái)的旋轉(zhuǎn)將三維點(diǎn)旋轉(zhuǎn)到同一個(gè)坐標(biāo)系，P3對(duì)應(yīng)論文里的 r_l,i', Pr1 對(duì)應(yīng)論文里的r_r,i'cv::Mat P3 = mR12i*Pr2;// Step 6: 計(jì)算尺度因子 Scaleif(!mbFixScale){// 論文中有2個(gè)求尺度方法。一個(gè)是p632右中的位置，考慮了尺度的對(duì)稱性// 代碼里實(shí)際使用的是另一種方法，這個(gè)公式對(duì)應(yīng)著論文中p632左中位置的那個(gè)// Pr1 對(duì)應(yīng)論文里的r_r,i',P3對(duì)應(yīng)論文里的 r_l,i',(經(jīng)過(guò)坐標(biāo)系轉(zhuǎn)換的Pr2), n=3, 剩下的就和論文中都一樣了double nom = Pr1.dot(P3);// 準(zhǔn)備計(jì)算分母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: 計(jì)算平移Translationmt12i.create(1,3,P1.type());// 論文中平移公式mt12i = O1 - ms12i*mR12i*O2;// Step 8: 計(jì)算雙向變換矩陣，目的是在后面的檢查的過(guò)程中能夠進(jìn)行雙向的投影操作// Step 8.1 用尺度，旋轉(zhuǎn)，平移構(gòu)建變換矩陣 T12mT12i = cv::Mat::eye(4,4,P1.type());cv::Mat sR = ms12i*mR12i;// |sR t|// mT12i = | 0 1|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)); }

四、其他相似實(shí)現(xiàn)

Finding optimal rotation and translation between corresponding 3D points | Nghia Ho

原理：

另外找到了另外一份源碼，一開(kāi)始以為能夠是這篇文章的實(shí)現(xiàn)，但是實(shí)際是針對(duì)世界坐標(biāo)系轉(zhuǎn)相機(jī)坐標(biāo)的轉(zhuǎn)換。

詳細(xì)可以看這篇文章：

(14條消息) 3對(duì)對(duì)應(yīng)點(diǎn)求解選擇和平移_謝大旭的博客-CSDN博客

五、參考資料

sim3相較于se3的好處 - 古月居 (guyuehome.com)

(14條消息) ORB_SLAM2中的Sim3變換_囂張的叉燒包的博客-CSDN博客_sim3變換

???(15條消息) 3、ORBSLAM閉環(huán)檢測(cè)終局之戰(zhàn)之sim3計(jì)算流程，確定是否檢測(cè)到閉環(huán)_宛如新生的博客-CSDN博客

g2o 代碼學(xué)習(xí)—— exp map and log map for SE(3), SIM(3) - JingeTU - 博客園 (cnblogs.com)

什么是單目SLAM的尺度不確定性，為什么會(huì)出現(xiàn)尺度漂移？ - 知乎 (zhihu.com)

總結(jié)

以上是生活随笔為你收集整理的sim3在orbslam2与gis中的应用的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。