CL关于ORB_SLAM的那些事（Sim3Solver）

Sim3Solver部分的学习总结

刚刚看完关于PnP方面的代码，那就趁热打铁继续阅读关于Sim3Solver部分的代码吧，开始吧！

.h部分的阅读

该头文件里包含了关键帧的头文件维护的变量（protected）有：
两个关键帧指针存放1帧坐标系下的3D点坐标的容器
存放2帧坐标系下的3D点坐标的容器存放1帧内的MapPoints的容器
存放2帧内的MapPoints的容器存放两帧匹配上的MapPoint的容器
存放索引值的容器
存放1帧的Sigma平方数的容器
存放2帧的Sigma平方数的容器
存放1最大误差的容器
存放2最大误差的容器
int整型的N和mN1
Mat类型1到2的旋转矩阵、平移矩阵和尺度因子
Mat类型1到2的变换矩阵和2到1的变换矩阵
存放内点标志位的bool容器
定义int整型的内点数量
定义int整型的迭代次数
定义存放是否是最好内点的标志位的容器
定义int整型的最好内点的数量
定义Mat最好的2到1的变换矩阵
定义Mat最好的旋转矩阵
定义Mat最好的平移矩阵
定义最好的尺度因子
定义是否固定尺度因子的标志位
定义存放随机选择的所有索引的容器
定义存放1帧内的投影坐标的容器
定义存放2帧内的投影坐标的容器
定义Ransac的概率
定义Ransac的最小内点数
定义Ransac的最大迭代次数
定义内外点判断的阈值
定义Sigma的平方数
定义帧1的内参矩阵
定义帧2的内存矩阵

该头文件里中的function有：
构造函数
设置Ransac的参数函数，这边默认的概率为0.99，最小的内点数为6，最大的迭代次数为300
寻找的函数
迭代的函数
获取估计的旋转矩阵的函数
获取估计的平移矩阵的函数
获取估计的尺度因子的函数
#############下面的函数是protected###############
计算中心店坐标函数
计算Sim3的函数
校核内点的函数
投影函数
从相机到图像的函数

function by function

先看一下.cpp文件中除了包含了自己的头文件之后，还有其他哪些头文件：

1. KeyFrame
2. ORBmatcher
3. 词袋DBoW2

构造函数
Sim3Solver()
函数的参数列表：关键帧1、关键帧2、存放匹配的MapPoint的容器、是否固定尺度因子的标志位
用列表初始化的方式，初始化迭代的次数为0，最好的内点数量为0，固定初度因子的标志位为传入的参数
将传入的1和2的关键帧赋值给维护变量
获取1关键帧的能匹配上的MapPoint，将这些MapPoint存入容器内
获取当前两帧关键帧之间匹配上的MapPoint的数量
用这个数量来初始化1和2关键帧中存放MapPoint的容器的大小
将匹配上的MapPoint容器赋值给维护变量
继续用上面的数量初始化1索引容器的大小、1和2的关键帧中存放3D点的容器的大小
分别获取两关键帧的Pose信息（旋转矩阵和平移向量）
继续用上面的数量初始化存放所有索引的容器的大小
定义初始化索引为0
循环遍历匹配上的MapPoint，判断其是否有MapPoint，获取两帧中对于这个MapPoint的指针，如果该点在两帧中都是good情况下，获取其对应的像素坐标的索引，在索引都大于0的情况下，获取两帧中该点对应的像素坐标，并获取该点对应的sigma的平方值，定义最大误差为9.21倍的sigma平方存入最大误差内，存入对应MapPoint和索引值，获取该点在世界坐标系下的坐标，利用之前的Pose信息转换到两个相机坐标系下，存入相机坐标系下的3D坐标容器内，存入定义的索引值，索引值++
获取两帧的内参
相机1和2分别调用FromCameraToImage()函数，传入相机坐标系下的3D点坐标、存放2D像素坐标的容器和内参
调用SetRansacParameters()函数，设置Ransac的参数

设置Ransac的参数
SetRansacParameters()
函数的参数列表：概率值、最小的内点数量和最大迭代次数
将传入的参数，分别赋值给Ransac的概率、Ransac最小的内点数量和Ransac最大的迭代次数
获取匹配上的点对的数量
用这个数量来初始化内点容器的大小
定义一个值epsilon：Ransac最小内点数量/所有的点数
定义迭代的数量变量
如果Ransac最小内点数等于所有的点数，迭代次数为1；
如果不是，迭代次数等于（（1-Ransac的概率）的对数/（1-epsilon的三次方）的对数）的ceil
Ransac的最大迭代次数为：上面计算的迭代次数和传入的最大迭代次数的较小者，选出的较小者与1比的较大者为Ransac的最大迭代次数。
mn的迭代次数赋值为0。

迭代函数
iterate()
函数的参数列表：迭代的次数、不再进行迭代的标志位、存放内点标志位的容器和内点的数量
初始化传入的不再进行迭代的标志为false
初始化存放内点标志位的容器的大小为匹配上的点数，初值都为false
初始化传入的内点数为0
如果匹配上的点数小于Ransac的最小内点数，将不再进行迭代的标志置true，返回空Mat
定义存放可获取索引的容器
定义Mat的三维点矩阵
定义当前迭代的次数为0
while循环（满足mn迭代次数小Ransac的最大迭代次数，且当前迭代次数小于迭代次数），当前迭代次数++，mn迭代次数++，将存放所有索引值的容器赋值给存放可获得索引值的容器，循环三次，随机选取3个MapPoint，将其对应在相机坐标系内的3D坐标分别存放在上面定义的三维点矩阵内，每一个对应一列，挑选过的索引将会被删除，避免抽到同一个点。调用ComputeSim3()函数计算Sim3矩阵，传入3组对应的点。调用CheckInliers()函数，进行内点数的检测。如果内点数不小于最好的内点数，将此时的mvb的内点数赋值给最好的mvb内点数，将此时的mn的内点数赋值给最好的mn内点数，赋值最好的变换矩阵、旋转矩阵和平移矩阵，最重要的是尺度因子也赋值给最好的尺度因子。如果mn内点数大于Ransac最小内点数，将mn内点数赋值给n内点数，循环标记内点的标记位，返回最好的两帧之间的变换矩阵。
如果循环结束后，还是没找到最好的变换矩阵，如果mn迭代的次数不小于Ransac最大的迭代次数，不再迭代的标志位置true，返回空的Mat。

寻找函数
find()
函数的参数列表：12的内点情况的容器和迭代的次数
定义bool的flag标志位
调用刚刚介绍的iterate()函数，返回计算得到的变换矩阵

计算形心坐标
ComputeCentroid()
函数的参数列表：Mat的三维点的矩阵、去形心的三维坐标和形心的坐标
调用opencv中的reduce()函数，按列累加，输出给形心坐标
将形心坐标除上列数（点的数量），得到真正的形心坐标
循环得到去形心的三维坐标

计算Sim3函数
ComputeSim3()
函数的参数列表：两帧内的3D坐标
定义点与点之前的相对坐标
定义形心的坐标
调用刚刚说的ComputeCentroid()，计算得到相对的坐标和形心坐标
构建M矩阵，帧2内的相对坐标乘上帧1内的坐标的转置
定义N矩阵，对称矩阵，如下图所示

定义Mat的特征值和特征向量
调用opencv中的eigen()函数求解N矩阵的特征值和特征向量
将特征向量的矩阵第一行第二列到第4列取出赋值给1行3列的vec变量
下面的操作就是怎么把现有的四元数转换成旋转矩阵，现将四元数转换成欧拉角，然后利用罗德里格斯公式计算得到旋转矩阵。
具体的公式如下图：

计算得到旋转矩阵之后，将2帧中取形心的点集乘上这个旋转矩阵。
如果没有固定尺寸，计算尺度因子，用Mat的dot函数，计算点集1和点集2之间的值

如果是固定尺寸，那么尺度因子为1。
将平移向量创建为1行3列
平移向量直接等于1帧的形心坐标-尺度因子×旋转矩阵×2帧的形心坐标
创建变换矩阵的大小和类型
将计算好的尺度、旋转和平移矩阵填充到变换矩阵中。
另外还要构建一个逆变换矩阵。

校核内点的函数
CheckInliers()
定义存放Mat的类型的容器P1和P2
调用Project()函数进行3D坐标的投影
初始化内点的数量为0
循环投影的点，计算在帧1和帧2中的像素误差，取误差的平方和，如果两边的误差少于最大的误差，该点就是内点，内点的数量++，如果不满足，那就不是内点。

获取估计的旋转矩阵
GetEstimatedRotation()
直接返回最好的旋转矩阵

获取估计的平移矩阵
GetEstimatedTranslation()
直接返回最好的平移矩阵

获取估计的尺度
GetEstimatedScale()
直接返回最好的尺度

投影函数
Project()
函数的参数列表：3D坐标容器、2D坐标容器、变换矩阵和内参矩阵
取出变换矩阵中的旋转和平移矩阵
取出内参矩阵的元素
将2D坐标容器清空
重新设置该容器的大小
循环遍历3D点，利用投影公式计算2D坐标并将其存入容器内。

从相机到图像
FromCameraToImage()
函数的参数列表：3D点坐标容器、2D点坐标容器和内参矩阵
取出内参矩阵中的元素（焦距和光心坐标）
将2D坐标容器清空
重新设置该容器的大小
循环遍历3D点，利用投影公式计算2D坐标并将其存入容器内。

时间：2019年09月15日
作者：hhuchen
机构：河海大学机电工程学院