对于双目、RGB-D相机，可获得深度，因此不存在尺度问题，因此Sim3中的尺度s=1。
（1）通过词袋加速算法实现当前帧、闭环帧的特征点的匹配，建立闭环帧的路标点和当前帧的特征点间的联系。
（2）使用RANSAC法，随机采取3对点（根据特征点的索引，获得当前帧中的路标点（局部建图时获得）及对应的闭环帧中的路标点（步骤（1）中获得）），计算两帧间的Sim3变换。一共迭代5次，如果有一次，获得的内点数大于20，就认为成功。
（3）根据Sim3变换，将闭环帧的路标点投影至当前帧中进行匹配。随后优化Sim3矩阵。
（4）将闭环帧及其共视帧的路标点投影至当前帧中，如果匹配上的路标点大于40，就认为该闭环帧可靠。
（5）随后就可以进行闭环校正，根据当前帧和闭环帧的Sim3矩阵，去校正当前帧及其共视帧的位姿、路标点坐标。更新共视关系，更新本质图。
设置RANSAC采样参数
主要参数包括迭代次数、迭代成功时，内点的数目、K次迭代可以成功的概率。
void Sim3Solver::SetRansacParameters(double probability, int minInliers, int maxIterations)
{
mRansacProb = probability; // 0.99
mRansacMinInliers = minInliers; // 20
mRansacMaxIts = maxIterations; // 最大迭代次数 300
// 匹配点的数目
N = mvpMapPoints1.size(); // number of correspondences
// 内点标记向量
mvbInliersi.resize(N);
// Adjust Parameters according to number of correspondences
float epsilon = (float)mRansacMinInliers/N;
// Set RANSAC iterations according to probability, epsilon, and max iterations
// 计算迭代次数的理论值，也就是经过这么多次采样，其中至少有一次采样中,三对点都是内点
// epsilon 表示了在这 N 对匹配点中,我随便抽取一对点是内点的概率;
// 为了计算Sim3,我们需要从这N对匹配点中取三对点;那么如果我有放回的从这些点中抽取三对点,取这三对点均为内点的概率是 p0=epsilon^3
// 相应地,如果取三对点中至少存在一对匹配点是外点, 概率为p1=1-p0
// 当我们进行K次采样的时候,其中每一次采样中三对点中都存在至少一对外点的概率就是p2=p1^k
// K次采样中,至少有一次采样中三对点都是内点的概率是p=1-p2
// 候根据 p2=p1^K 我们就可以导出 K 的公式：K=\frac{\log p2}{\log p1}=\frac{\log(1-p)}{\log(1-epsilon^3)}
// 也就是说，我们进行K次采样,其中至少有一次采样中,三对点都是内点; 因此我们就得到了RANSAC迭代次数的理论值
int nIterations;
if(mRansacMinInliers==N)
nIterations=1; // 这种情况的时候最后计算得到的迭代次数的确就是一次
else
nIterations = ceil(log(1-mRansacProb)/log(1-pow(epsilon,3)));
// 外层的max保证RANSAC能够最少迭代一次;
// 内层的min的目的是,如果理论值比给定值要小,那么我们优先选择使用较少的理论值来节省时间(其实也有极大概率得到能够达到的最好结果);
// 如果理论值比给定值要大,那么我们也还是有限选择使用较少的给定值来节省时间
mRansacMaxIts = max(1,min(nIterations,mRansacMaxIts));
// 当前正在进行的迭代次数
mnIterations = 0;
}
RANSAC求解Sim3变换
求解成功的标志：内点数大于20。
/**
* @brief Ransac求解mvX3Dc1和mvX3Dc2之间Sim3，函数返回mvX3Dc2到mvX3Dc1的Sim3变换
*
* @param[in] nIterations 设置的最大迭代次数
* @param[in] bNoMore 为true表示穷尽迭代还没有找到好的结果，说明求解失败
* @param[in] vbInliers 标记是否是内点
* @param[in] nInliers 内点数目
* @return cv::Mat 计算得到的Sim3矩阵
*/
cv::Mat Sim3Solver::iterate(int nIterations, bool &bNoMore, vector<bool> &vbInliers, int &nInliers)
开始循环
循环条件：当前迭代次数小于5，总的迭代次数小于300。当前迭代次数指的是对于当前帧尝试求解Sim3的次数，如果大于5了还不行，就取消。总的迭代次数指的是：在回环检测的时候，会计算所有的候选闭环帧和当前帧的Sim3，在这个过程中的总的迭代次数。
（1）随机取3组点，并将索引在列表中删除
// Get min set of points
// Step 2.1 随机取三组点，取完后从候选索引中删掉
for(short i = 0; i < 3; ++i)
{
// DBoW3中的随机数生成函数
int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size()-1);
int idx = vAvailableIndices[randi];
// P3Dc1i和P3Dc2i中点的排列顺序：
// 相机坐标
// x1 x2 x3 ...
// y1 y2 y3 ...
// z1 z2 z3 ...
mvX3Dc1[idx].copyTo(P3Dc1i.col(i));
mvX3Dc2[idx].copyTo(P3Dc2i.col(i));
// 从"可用索引列表"中删除这个点的索引
vAvailableIndices[randi] = vAvailableIndices.back();
vAvailableIndices.pop_back();
}
（2）根据匹配的3对路标点，计算Sim3变换
ComputeSim3(P3Dc1i,P3Dc2i);
（3）根据Sim3变换检测内点数量
CheckInliers();
（4）更新最优的Sim3参数系数，包括R，t，s，匹配上的内点数等。如果内点数大于20，则计算成功，直接返回Sim3矩阵。
// Step 2.4 记录并更新最多的内点数目及对应的参数
if(mnInliersi>=mnBestInliers)
{
mvbBestInliers = mvbInliersi;
mnBestInliers = mnInliersi;
mBestT12 = mT12i.clone();
mBestRotation = mR12i.clone();
mBestTranslation = mt12i.clone();
mBestScale = ms12i;
if(mnInliersi>mRansacMinInliers) // 只要计算得到一次合格的Sim变换，就直接返回
{
// 返回值,告知得到的内点数目
nInliers = mnInliersi;
for(int i=0; i<N; i++)
if(mvbInliersi[i])
// 标记为内点
vbInliers[mvnIndices1[i]] = true;
return mBestT12;
} // 如果当前次迭代已经合格了,直接返回
} // 更新最多的内点数目
（5）上述过程重复5次还不行，就直接返回FALSE
// Step 3 如果已经达到了最大迭代次数了还没得到满足条件的Sim3，说明失败了，放弃，返回
if(mnIterations>=mRansacMaxIts)
bNoMore=true;
计算三个点的质心以及去质心坐标
/**
* @brief 给出三个点,计算它们的质心以及去质心之后的坐标
*
* @param[in] P 输入的3D点
* @param[in] Pr 去质心后的点
* @param[in] C 质心
*/
void Sim3Solver::ComputeCentroid(cv::Mat &P, cv::Mat &Pr, cv::Mat &C)
{
// 矩阵P每一行求和，结果存在C。这两句也可以使用CV_REDUCE_AVG选项来实现
cv::reduce(P,C,1,CV_REDUCE_SUM);
C = C/P.cols;// 求平均
for(int i=0; i<P.cols; i++)
{
Pr.col(i)=P.col(i)-C;//减去质心
}
}
根据3对匹配的点计算Sim3
需要3对相机1、相机2中的匹配路标点（相机坐标系中）
旋转矩阵------尺度------平移矩阵-------构建Sim3矩阵
计算三个点的质心
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 P1
cv::Mat O2(3,1,Pr2.type()); // Centroid of P2
ComputeCentroid(P1,Pr1,O1);
ComputeCentroid(P2,Pr2,O2);
计算M、N矩阵
// Step 2: 计算论文中三维点数目n>3的 M 矩阵。这里只使用了3个点
// Pr2 对应论文中 r_l,i'，Pr1 对应论文中 r_r,i',计算的是P2到P1的Sim3，论文中是left 到 right的Sim3
cv::Mat M = Pr2*Pr1.t();
// Step 3: 计算论文中的 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+Szz
N12 = M.at<float>(1,2)-M.at<float>(2,1); // Syz-Szy
N13 = M.at<float>(2,0)-M.at<float>(0,2); // Szx-Sxz
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: 特征值分解求最大特征值对应的特征向量，就是我们要求的旋转四元数
cv::Mat eval, evec; // val vec
// 特征值默认是从大到小排列，所以evec[0] 是最大值
// 计算对称矩阵的特征值，特征向量
cv::eigen(N,eval,evec);
// N 矩阵最大特征值（第一个特征值）对应特征向量就是要求的四元数（q0 q1 q2 q3），其中q0 是实部
// 将(q1 q2 q3)放入vec（四元数的虚部）
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
// 四元数虚部模长 norm(vec)=sin(theta/2), 四元数实部 evec.at<float>(0,0)=q0=cos(theta/2)
// 这一步的ang实际是theta/2，theta 是旋转向量中旋转角度
// ? 这里也可以用 arccos(q0)=angle/2 得到旋转角吧
double ang=atan2(norm(vec),evec.at<float>(0,0));
// vec/norm(vec)归一化得到归一化后的旋转向量,然后乘上角度得到包含了旋转轴和旋转角信息的旋转向量vec
// vec：旋转向量
vec = 2*ang*vec/norm(vec); //Angle-axis x. quaternion angle is the half
mR12i.create(3,3,P1.type());
// 旋转向量（轴角）转换为旋转矩阵
cv::Rodrigues(vec,mR12i); // computes the rotation matrix from angle-axis
计算s
双目、RGB-D的s为1。
// Step 6: 计算尺度因子 Scale
if(!mbFixScale)
{
// 论文中有2个求尺度方法。一个是p632右中的位置，考虑了尺度的对称性
// 代码里实际使用的是另一种方法，这个公式对应着论文中p632左中位置的那个
// Pr1 对应论文里的r_r,i',P3对应论文里的 r_l,i',(经过坐标系转换的Pr2), n=3, 剩下的就和论文中都一样了
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;
}
else
ms12i = 1.0f;
计算平移向量
// Step 7: 计算平移Translation
mt12i.create(1,3,P1.type());
// 论文中平移公式
mt12i = O1 - ms12i*mR12i*O2;
计算1–>2,2–>1的Sim3变换矩阵
// Step 8: 计算双向变换矩阵，目的是在后面的检查的过程中能够进行双向的投影操作
// 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));
检测内点
void Sim3Solver::CheckInliers()
投影路标点
根据Sim3变换，将1中的路标点投影至2中，2中的路标点投影至1中。
// 用计算的Sim3 对所有的地图点投影，得到图像点
vector<cv::Mat> vP1im2, vP2im1;
Project(mvX3Dc2,vP2im1,mT12i,mK1);// 把2系中的3D经过Sim3变换(mT12i)到1系中计算重投影坐标
Project(mvX3Dc1,vP1im2,mT21i,mK2);// 把1系中的3D经过Sim3变换(mT21i)到2系中计算重投影坐标
根据重投影误差确定内点
// 对于两帧的每一个匹配点
for(size_t i=0; i<mvP1im1.size(); i++)
{
// 当前关键帧中的地图点直接在当前关键帧图像上的投影坐标mvP1im1，mvP2im2
// 对于这对匹配关系,在两帧上的投影点距离都要进行计算
cv::Mat dist1 = mvP1im1[i]-vP2im1[i];
cv::Mat dist2 = vP1im2[i]-mvP2im2[i];
// 取距离的平方作为误差
const float err1 = dist1.dot(dist1);
const float err2 = dist2.dot(dist2);
// 根据之前确定的这个最大容许误差来确定这对匹配点是否是外点
if(err1<mvnMaxError1[i] && err2<mvnMaxError2[i])
{
mvbInliersi[i]=true;
mnInliersi++;
}
else
mvbInliersi[i]=false;
}// 遍历其中的每一对匹配点
