「自动驾驶SLAM」 sim3原理

slam2的LoopClosing线程中，做了Loop检测和Loop校正。

Loop是指机器人回到自己见过的场景，即机器人拍摄到了与保存在地图中的的图片B中非常相似的图片A，而对图片B，记录者一个位置姿态，就是在世界坐标系中的平移量和旋转量T0。

此时，机器人的本身也计算了一个位置量和姿态量T1

T1和T0之间，理论上存在着一定的旋转和平移。因为我们是用两张图片之间的ORB特征匹配来发现机器人回到了见过的场景的，只是两张图片在orb特征上比较相似，并不是说两张图片的一摸一样，

既然两张图片不一样了，那么机器人拍这两张图片的位置、姿态必然不同，当然机器人运动过程中同时也积累了误差，即便陪着角度，位置完全相同，也可能因为误差的存在，T1和T0不同。

因此，我们需要根据两张图片上检测到的关键点来计算T1与T0之间的转换关系，这个转换关系里有3个量：s,R,t,

其中s尺度量，R旋转矩阵，t平移向量。

我们需要用这几个量，来纠正一下来计算一下图片A，B上关键点的转换关系，把这个转换关系记录在地图中，当然这个转换关系也可以用来纠正系统的积累误差。

此时，要求取合适的s,R,t,就要使用sim3算法了。

sim3求解方法来自Horn1987,Closed-formsolutionofabsoluteorientationusingunitquaternions.

图片A上的关键点的坐标，是在拍摄A图时的坐标系。

图片B上的关键点的坐标，是在拍摄B图时的坐标系。

也就是说，求解图片A，B上的关键点之间的变换关系，实际上是求解两个坐标系之间的变换关系。

我们定义图片B的坐标系是右坐标系right

我们定义图片A的坐标系是左坐标系left,

则左右坐标系的变换关系可用下式表示：

left,就是左坐标系中的点，right就是右坐标系中的点。

那么现在已知的是：我们在图片A，B上，得到到了很多对的点，也就是说，上面公式中的left和right是一对，我们在图片上获取到了很多对点的坐标。

我们需要求取一组合适的s,R,t能够满足这些已知点的对应变换关系。

求取的过程，先计算R，然后由R计算出s,再由R,s计算出t.

请看以下推导：

1/**2*计算sim33*/4voidSim3Solver::ComputeSim3(cv::MatP1,cv::MatP2)5{6//Customimplementationof:7//Horn1987,Closed-formsolutionofabsoluteorientataionusingunitquaternions89//Step1:Centroidandrelativecoordinates1011cv::MatPr1((),());//Relativecoordinatestocentroid(set1)12cv::MatPr2((),());//Relativecoordinatestocentroid(set2)13cv::MatO1(3,1,());//CentroidofP114cv::MatO2(3,1,());//CentroidofP21516//计算点集的平均值17ComputeCentroid(P1,Pr1,O1);18ComputeCentroid(P2,Pr2,O2);1920//Step2:ComputeMmatrix2122cv::MatM=Pr2*();2324//Step3:ComputeNmatrix2526doubleN11,N12,N13,N14,N22,N23,N24,N33,N34,N44;2728cv::MatN(4,4,());2930N11=(0,0)+(1,1)+(2,2);31N12=(1,2)-(2,1);32N13=(2,0)-(0,2);33N14=(0,1)-(1,0);34N22=(0,0)-(1,1)-(2,2);35N23=(0,1)+(1,0);36N24=(2,0)+(0,2);37N33=-(0,0)+(1,1)-(2,2);38N34=(1,2)+(2,1);39N44=-(0,0)-(1,1)+(2,2);4041N=(cv::Mat_float(4,4)N11,N12,N13,N14,42N12,N22,N23,N24,43N13,N23,N33,N34,44N14,N24,N34,N44);454647//Step4:Eigenvectorofthehighesteigenvalue4849cv::Mateval,evec;5051cv::eigen(N,eval,evec);//evec[0]isthequaternionofthedesiredrotation5253cv::Matvec(1,3,());54((0).colRange(1,4)).copyTo(vec);//extractimaginarypartofthequaternion(sin*axis)5556//,cosistherealpart57doubleang=atan2(norm(vec),(0,0));5859vec=2*ang*vec/norm(vec);//(3,3,());6263cv::Rodrigues(vec,mR12i);//computestherotationmatrixfromangle-axis6465//Step5:Rotateset26667cv::MatP3=mR12i*Pr2;6869//Step6:Scale7071if(!mbFixScale)72{73doublenom=(P3);74cv::Mataux_P3((),());75aux_P3=P3;76cv::pow(P3,2,aux_P3);77doubleden=0;7879for(inti=0;iaux_;i++)80{81for(intj=0;jaux_;j++)82{83den+=aux_(i,j);84}85}8687ms12i=nom/den;88}89else90ms12i=1.0f;9192//Step7:(1,3,());95mt12i=O1-ms12i*mR12i*O2;9697//Step8:Transformation9899//=cv::Mat::eye(4,4,());101102cv::MatsR=ms12i*mR12i;103104((0,3).colRange(0,3));105((0,3).col(3));106107//=cv::Mat::eye(4,4,());110111cv::MatsRinv=(1.0/ms12i)*();112113((0,3).colRange(0,3));114cv::Mattinv=-sRinv*mt12i;115((0,3).col(3));116}

代码中，

11行止14行，P1,P2就是我们的左点，右点。

P1r,P2r对应于公式中的r_ri',r_li'，相对坐标的意思，相对于点集平均点的相对坐标。

O1,O2就是两个点集的中心点了。

63行之前都是用四元组去求解R的，在63行，把R这个旋转矩阵存在了mR12i中，到此，我们就得到R矩阵了，后面就要求取s和t了。

求s:

s的值，我们使用推导中的公式3来求取，如下：

这里，需要求所有r_ri'的模的平方和，这个r_ri'在代码中是Pr1。这里的r_li'对应代码中的Pr2,我们要求一下R*r_li'的模的平方，这主要靠67-90行来实现：

代码里，待求量s为ms12i.

在代码的87行，ms12i=nom/den;其中nom是pr1的元素于P3中元素的点积，

而den是P3中元素的平方和。

公式中的R*r_li'对应P3=mR12i*Pr2;

公式中的R*r_li'的平方，对应于76行:cv::pow(P3,2,aux_P3)，将P3每项单独平方后，放入aux_P3里。

这里与公式3中不太符合。按照公式3，nom应该不是(P3)而是(Pr1),ms12i应是(nom/den)^0.5，这里估计是另外一种推导得到的结果，需要去论文中核对，或者跟进一下sim3方法的改进。

这一点，那个朋友知道，还请留言告知呀

找到了，s是按照这个公式中给的结果：s=D/S_L，如下：

如果我们再一开始的推导中，不对目标函数乘以1/s，我们就会推导得到如上图结果。

无路如何，s就得后，就可以求t了，

t的求取公式为：

代码为：

代码中的t为:mt12i，它的取值为mt12i=O1-ms12i*mR12i*O2;

O1，O2为两个平均点，ms12i为s,mR12i为R矩阵，这个计算过程与公式符合。

至此，R,s,t就计算完成，后面可以根据R,s,t计算出变换矩阵T：

代码第100行，建立了一个4x4的单位矩阵mT12i,

第104行，把3x3的sR矩阵放在了4x4单位矩阵mT12i的前3行，前3列的位置。

第105行，把3x1的t向量mt12i放在了4x4的mT12i的前3行，第4列的位置。

我们就得到了由坐标系1变换到坐标系2的变换矩阵T12,即代码中的mT12i.

那么还可以计算由坐标系2变换到坐标系1的变换矩阵T21,即如下代码中的mT21i:

说明：其中的转换为四元组的部分我没有详细写，旋转矩阵转换为四元组这部分需要单独写一篇做分析。

以上推导过程是我参考了如下文献后整理的，文献中关于四元组求解R的过程写得很详细。读者也可以参考原文进行更加详细的解读

参考：

ORB-SLAM2代码阅读笔记（十）：sim3求解

ORB-SLAM（五）优化