「自动驾驶SLAM」 sim3求解过程推导以及代码对应

SLAM什么时候需要使用sim3

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

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

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

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

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

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

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

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

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

sim3

sim3求解方法来自 Horn 1987, Closed-form solution of absolute orientation using unit quaternions.

图片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 * 计算sim3 3 */ 4 void Sim3Solver::ComputeSim3(cv::Mat &P1, cv::Mat &P2) 5 { 6 // Custom implementation of: 7 // Horn 1987, Closed-form solution of absolute orientataion using unit quaternions 8 9 // Step 1: 肖斯塔科维奇第五交响曲Centroid and relative coordinates 10 11 cv::Mat Pr1(P1.size(),P1.type()); // Relative coordinates to centroid (set 1) 12 cv::Mat Pr2(P2.size(),P2.type()); // Relative coordinates to centroid (set 2) 13 cv::Mat O1(3,1,Pr1.type()); // Centroid of P1 14 cv::Mat O2(3,1,Pr2.type()); // Centroid of P2 15 16 // 计算点集的平均值 17 ComputeCentroid(P1,Pr1,O1); 18 ComputeCentroid(P2,Pr2,O2); 19 20 // Step 2: Compute M matrix 21 22 cv::Mat M = Pr2*Pr1.t(); 23 24 // Step 3: Compute N matrix 25 26 double N11, N12, N13, N14, N22, N23, N24, N33, N34, N44; 27 28 cv::Mat N(4,4,P1.type()); 29 30 N11 = M.at<float>(0,0)+M.at<float>(1,1)+M.at<float>(2,2); 31 N12 = M.at<float>(1,2)-M.at<float>(2,1); 32 N13 = M.at<flo生殖器疱疹传染吗at>(2,0)-M.at<float>(0,2); 33 N14 = M.at<float>(0淘宝直通车是什么意思,1)-M.at<float>(1,0); 34 N22 = M.at<float>(0,0)-M.at<float>(1,1)-M.at<float>(2,2); 35 N23 = M.at<float>(0,1)+M.at<float>(1,0); 36 N24 = M.at<float>(2,0)+M.at<float>(0,2); 37 N33 = -M.at<float>(0,0)+M.at<float>(1,1)-M.at<float>(2,2); 38 N34 = M.at<float>(1,2)+M.at<float>(2,1); 39 N44 = -M.at<float>(0,0)-M.at<float>(1,1)+M.at<float>(2,2); 40 41 N = (cv::Mat_<float>(4,4) << N11, N12, N13, N14, 42 N12, N22, N23, N24, 43 N13, N23, N33, N34, 44 N14, N24, N34, N44); 45 46 47 // Step 4: Eigenvector of the highest eigenvalue 48 49 cv::Mat eval, evec; 50 51 cv::eigen(N,eval,evec); //evec[0] is the quaternion of the desired rotation 52 53 cv::Mat vec(1,3,evec.type()); 54 (evec.row(0).colRange(1,4)).copyTo(vec); //extract imaginary part of the quaternion (sin*axis) 55 56 // Rotation angle. sin is the norm of the imaginary part, cos is the real part 57 double ang=atan2(norm(vec),evec.at<float>(0,0)); 58 59 vec = 2*ang*vec/norm(vec); //Angle-axis representation. quaternion angle is the half 60 61 mR12i.create(3,3,P1.type()); 62 63 cv::Rodrigues(vec,mR12i); // computes the rotation matrix from angle-axis 64 65 // Step 5: Rotate set 2 66 67 cv::Mat P3 = mR12i*Pr2; 68 69 // Step 6: Scale 70 71 if(!mbFixScale) 72 { 73 dlv围巾ouble nom = Pr1.dot(P3); 74 cv::Mat aux_P3(P3.size(),P3.小朋友齐打交2type()); 75 aux_P3=P3; 76 cv::pow(P3,2,aux_P3); 77 double den = 0; 78 79 for(int i=0; i<aux_P3.rows; i++) 80 { 81 for(int微生物检查 j=0; j<aux_P3.cols; j++) 82 { 83 den+=aux_P3.at<float>(i,j); 84 } 85 } 86 87 ms12i = nom/den; 88 } 89 else 90 ms12i = 1.0f; 91 92 // Step 7: Translation 93 94 mt12i.create(1,3,P1.type()); 95 mt12i = O1 - ms12i*mR12i*O2; 96 97 // Step 8: Transformation 98 99 女警察被杀// Step 8.1 T12100 mT12i = cv::Mat::eye(4,4,P1.type());101 102 cv::Mat sR = ms12i*mR12i;103 104 sR.copyTo(mT12i.rowRange(0,3).colRange(0,3));105 mt12i.copyTo(mT12i.rowRange(0,3).col(3));106 107 // Step 8.2 T21108 109 mT21i = cv::Mat::eye(4,4,P1.type());110 111 cv::Mat sRinv = (1.0/ms12i)*mR12i.t();112 113 sRinv.copyT数学学习与研究o(mT21i.rowRange(0,3).colRange(0,3));114 cv::Mat tinv = -sRinv*mt12i;115 tinv.copyTo(mT21i.rowRange(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,我们要求audiodg一下R*r_li'的模的平方，这主要靠67-90行来实现：

代码里，待求量s为演艺吧ms12i.

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

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

公式中的R*r_li真皮沙发9;对应P3 = mR12i*Pr2;

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

这里与公式3中不太符合。按照公式3，nom应该不是Pr1.dot(P3)而是Pr1.dot(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求解 blog.csdn/mo张家界攻略yu123456789/article/云宝黛西details/91947539#1.sim3的简单概述ORB-SLAM（五）优化 wwwblogs/luyb/p/5447497.html