ORB-SLAM2代码阅读笔记：Sim3Solver

• Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector &vpMatched12, const bool bFixScale):
  mnIterations(0), mnBestInliers(0), mbFixScale(bFixScale)
  构造函数

void Sim3Solver::SetRansacParameters(double probability, int minInliers, int maxIterations)
设置进行RANSAC时的参数：和PnPSolver中相同

• void Sim3Solver::ComputeSim3(cv::Mat &P1, cv::Mat &P2)
  根据两组匹配的3D点,计算之间的Sim3变换
  三对匹配点,每个点的坐标都是列向量形式,三个点组成了3x3的矩阵,三对点组成了两个3x3矩阵P1,P2，由于Sim3是用于回环中的，所以P1和P2分布代表当前帧和回环候选帧，

sim3的具体推导可以看论文Closed-form solution of absolute orientataion using unit quaternions，我之前的笔记中也有：http://note.youdao.com/noteshare?id=76a1d9452a3b6371464dd9b2745f5a2a&sub=1CD8BF54C37249D48F6AE82A43FBD534

这里需要计算的是两个坐标系之间的相似变换 相似变换比欧式变换多了一个尺度因子，所以相似变换需要求三个部分：旋转、平移、尺度因子。

image

利用三对匹配点形成左右两个坐标系，

image

image

，求左右两个坐标系相似变换

1. 旋转：这里先求解M矩阵
   image
   ，M矩阵包含了解决旋转最小二乘问题所需的所有信息。可以通过M矩阵求解N矩阵，N矩阵为
   image
   
   M矩阵计算过程见论文或我的sim3笔记。

   cv::Mat M = Pr2*Pr1.t();
   
   // Step 3: Compute N matrix
   
   double N11, N12, N13, N14, N22, N23, N24, N33, N34, N44;
   
   cv::Mat N(4,4,P1.type());
   
   N11 = M.at(0,0)+M.at(1,1)+M.at(2,2);
   N12 = M.at(1,2)-M.at(2,1);
   N13 = M.at(2,0)-M.at(0,2);
   N14 = M.at(0,1)-M.at(1,0);
   N22 = M.at(0,0)-M.at(1,1)-M.at(2,2);
   N23 = M.at(0,1)+M.at(1,0);
   N24 = M.at(2,0)+M.at(0,2);
   N33 = -M.at(0,0)+M.at(1,1)-M.at(2,2);
   N34 = M.at(1,2)+M.at(2,1);
   N44 = -M.at(0,0)-M.at(1,1)+M.at(2,2);
   
   N = (cv::Mat_(4,4) << N11, N12, N13, N14,
                                N12, N22, N23, N24,
                                N13, N23, N33, N34,
                                N14, N24, N34, N44);
   

   然后需要将N进行特征分解，求得最大特征值对应的特征向量为四元数表示的旋转，我们将四元数转换为旋转矩阵的形式，首先求得旋转角和旋转轴，从而得到最终的旋转矩阵。
2. 尺度：
   尺度因子计算公式为:
   

    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(i,j);
           }
       }
       ms12i = nom/den;
   

3. 平移：
   

   平移计算公式：
   image

   mt12i.create(1,3,P1.type());
   // 论文中公式
   mt12i = O1 - ms12i*mR12i*O2;
   
   // Step 8: Transformation
   // 计算双向的位姿变换,目的是在下面的检查的过程中能够进行双向的投影操作
   
   // Step 8.1 T12
   mT12i = 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 T21
   
   mT21i = 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));