CL关于ORB_SLAM的那些事（Optimizer）

Optimizer部分的学习总结

之前我一直对优化这一部分的内容是逃避的，因为我觉得优化这部分可能不适合我现在的水平去研究，之前只是了解了一些基本的优化算法，但是现在发现，逃不了，感觉必须要拿出真功夫啃一啃这一部分的东西，先不说用起来，起码自己能看懂代码，知道其中的来龙去脉，话不多说，这就开始吧！

浏览头文件，了解维护变量和函数方法

打开头文件，里面包含了
Map.h
MapPoint.h
KeyFrame.h
LoppClosing.h
Frame.h
还有第三方库里的g2o

Optimizer类内维护的变量：
不好意思，真没有变量

Optimizer类内声明的函数：
所有的函数都是静态函数

1. BA的函数
2. 全局的BA函数
3. 局部的BA函数
4. 位姿优化的函数
5. 优化sim3的函数
   好简洁的头文件，我喜欢

function by function

cpp文件内包含的除本身之外的头文件：
都是g2o的头文件
Eigen的头文件
转换类的头文件

看到目前为止第一个没有构造函数的类
全局BA函数
GlobalBundleAdjustment()
函数的参数列表：地图类型的指针、迭代的次数、bool类型的停止标志的指针、常量的闭环关键帧、常bool类型的鲁棒性
获取地图中的所有的关键帧
获取地图中的所有的MapPoint
调用BA函数，传入所有的关键帧、MapPoint、停止的标志位、闭环的关键帧和鲁棒性的标志位

BA函数
BundleAdjustment()
函数的参数列表：存放所有关键帧的容器、存放所有MapPoint的容器、迭代的次数、是否停止的标志位、闭环检测的关键帧的数量、鲁棒性的标志位
定义不在MapPoint内的标志位容器
根据传入进来的MapPoint的数量给该容器定大小
用g2o的稀疏优化器定义一个变量（定义了图模型）
既然来到了这里，就先对g2o这个优化库进行一定的了解吧，打开《视觉SLAM十四讲从理论到实践》。
矩阵块部分，每个误差项优化变量的维度为6维，误差值维度为3？
定义了线性方程求解器
将求解器指向稠密的增量方程
定义矩阵块求解器
程序里选择了LM的梯度下降方法
设置求解器
判断要求停止的标志位是否为true，如果是，直接调用图中的设置强制停止的标志位，传入要求停止的标志位
循环遍历所有的关键帧，取每一帧关键帧，在关键帧是好的基础上定义SE3的指针映射的优化节点，在这个点里设置估计的初值（在扔进去之前，取出这个关键帧的变换矩阵并通过转换类的转换函数，将SE3转换成？？？），设置这个节点的ID（这边就是该关键帧的帧号），设置是否固定某一帧（固定第一帧关键帧，也就是第一帧），在图中添加这一节点，找到最后一帧关键帧的帧号。
定义二维Huber的阈值为5.99的开方
定义三维Huber的阈值为7.845的开方
循环遍历所有的MapPoint，在该点是好点的基础上，用g2o中的稀疏BA点定义一个点，获取MapPoint在世界坐标系下的坐标，调用转换类中的函数，将点的坐标转换成了三维向量，设置估计初值（传入转换完的三维向量），点设置的ID在最大关键帧帧号的基础上再加自己的ID+1。点设置边缘化为true，在图中添加MapPoint这个节点。取出当前MapPoint的观测的信息，定义初始的边数为0，循环遍历观测信息，取出其中的关键帧，判断其是否是好的或者该关键帧的帧号是否大于目前最大的帧号，只要两个条件中满足一个，直接return。往下就边数直接++，取出该关键帧观测的这个MapPoint在图像上的像素坐标，判断该对应的右图上的值，1）如果小于0（表示说明是单目），就定义Eigen的2行1列的矩阵，存入该MapPoint在此关键帧上的关键点，定义g2o的SE3投影边，这条边连接的MapPoint与能观测到这个点的关键帧，在边内设置的观测就是该MapPoint在此关键帧中的像素坐标（去畸变之后的），根据该关键点所在的金字塔层数取其尺度因子平方的倒数（逆），设置这条边的信息矩阵为2×2的单位阵乘上这个常数，判断闯入的鲁棒性标志位是否为true，如果是，就定义g2o的鲁棒核Huber的指针，指向鲁棒核Huber的内存，将边设置鲁棒核，传入该指针，该指针调用设置Delta，传入上面二维的Huber阈值。取出关键帧内的相机参数，存入边内，在图中添加这条边。2）如果不小于0，观测值就定义为一个3行1列的矩阵，取出右图上的u值，结合上面左图上关键点的像素坐标，组成观测矩阵，这边在定义边时，边的类型就是双目的SE3投影，往下设置边的过程都一样，只是鲁棒性中的阈值换成了三维的Huber阈值。
添加边的大致过程

添加完了一个MapPoint的点之后，判断边数是否为0，如果是0，那么将该MapPoint的节点直接删除，该点对应位置的不在MapPoint中的容器标志位置true；如果不是0，该位置的标志位置false。
初始化优化
传入迭代的次数进行优化
循环遍历关键帧，取出对应关键帧的优化结果（SE3的指数映射），如果闭环的关键帧为0，直接将取出的值转成Mat格式，传给关键帧的设置位姿函数；如果不为0，在此关键帧内全局BA的变换矩阵设置为4行4列，将取出的结果转成Mat后直接拷贝给这个变量，关键帧内的全局BA的关键帧被赋值闭环关键帧。
循环遍历MapPoint，不包括在内的点，直接continue，坏点也是直接continue，获取优化完的点的坐标，如果闭环的关键帧为0，直接将取出的值转成Mat格式，传给MapPoint的设置世界坐标的函数；如果不为0，在此MapPoint内的全局BA的坐标设置成3行1列，将取出的结果转成Mat后直接拷贝给这个变量，MapPoint内的全局BA的关键帧被赋值闭环关键帧。

两帧之间的位姿优化
PoseOptimization()
函数的参数列表：Frame
设置图
矩阵块：每个误差项优化变量的维度为6维，误差值维度为3
用这样的矩阵块定义一个线性求解器
给这个求解器分配稠密的增量方程
定义矩阵块求解器
程序里选择了LM的梯度下降方法
设置求解器
定义初始的匹配为0
定义帧的节点
设置节点的估计（传入帧的变换矩阵，转换成g2o能接收的那种）
设置节点的ID为0
该节点不固定
在图中添加这个节点

获取该帧内的关键点的数量
定义了存放单目边的容器
定义存放边索引的容器
这两个容器的大小为该帧内关键点的数量
定义了存放双目边的容器
定义存放边索引的容器
这两个容器的大小为该帧内关键点的数量
定义单目delta变量为5.991的开方
定义双目delta变量为7.815的开方
循环遍历该帧内关键点对应的MapPoint，判断是否对应有MapPoint，如果有，判断是双目观测还是单目观测，如果右图对应的取值小于0，说明是单目；如果右图对应的取值不小于0，说明是双目。对于单目，先是初始匹配数量自增，该位置对应的外点标志位置false，定义一个2行1列的观测矩阵，取去畸变的关键点像素坐标，在边里添加一个节点，设置观测数据（传入上面定义的观测值），设置这条边的信息矩阵，取出该点所在金字塔的层数的尺度因子的平方的倒数（逆），传入的信息矩阵就是2×2的单位阵乘上这个逆。设置鲁棒性函数，最后传入单目的阈值（卡方自由度为2，内点概率95%对应的临界值）看到这里需要补充一下前面在匹配中，计算的误差其实都是卡方距离，因为不同的关键点是在不同的金字塔的层上提取的，这边就需要进行归一化的处理，所有最后计算的误差都是卡法距离，具体可见下图

再往下就是取出该帧的相机内参，保证MapPoint的坐标是常数，在图内添加这条边，最后将这条边存入单目边的容器内，这条边的id也存入对应的容器内。如果是双目观测的话，连接数量自增，外点的标志位置false，设置边信息，定义3行1列的观测信息矩阵，存入左图的像素坐标和右图的u坐标，定义双目的投影边，连接节点，设置观测矩阵，取出该点所在金字塔层的尺度因子的逆平方，构造信息矩阵，设置信息矩阵，设置鲁棒性函数，传入双目的卡方阈值，将相机的内参传入边内，将该MapPoint设置成常数，将该边添加进图内，最后把边存入双目边的容器内，将当前边的索引存入容器内。
判断在图中建立的边数是否小于3，如果小于3就直接return；
程序内进行了四次优化，所以这边定义了一个float的数组，长度为4，存放卡方平方的阈值，单目一组，双目一组
定义迭代的次数的数组，其长度为4
定义bad的计数变量，初始化为0
循环4次，每次执行：
将该帧的变换矩阵转换成四元数，传入初始估计该帧的初始变换矩阵
初始化优化，传入优化迭代的次数
将bad的计数变量置0
循环遍历单目边，取出其索引值，如果该边不是异常边，调用边内的计算误差函数，获取它的卡方值的平方，如果大于卡方分布的临界值，那么这个MapPoint就是外点，bad点计数自增并设置setLevel为1（即下次不再对该边进行优化）；如果不大于临界值，那么该点标记为内点并设置 setLevel为0（即下次还对该边进行优化）。循环到第三次迭代时，设置不再使用鲁棒核函数。

当在迭代的过程中，减少之后的边数少于10时，将直接跳出迭代循环
取出最后优化的结果
通过转换类转换成Pose
调用Frame的设置Pose函数，传入优化好的变换矩阵
返回最后内点的数量

局部BA函数
LocalBundleAdjustment()
函数的参数列表：关键帧、停止的标志位和地图指针
定义一个存放关键帧的链表
将传入的该关键帧存入此链表内
取该关键帧的帧号拷贝给该关键帧内的局部BA的关键帧帧号
获取和该关键帧连接的所有关键帧
循环取容器内的关键帧，将其内部的局部BA关键帧的帧号都赋值为传入关键帧的帧号，如果该关键帧是好的关键帧，那么就将该帧存入之前定义的关键帧链表。
定义存放MapPoint的局部MapPoint的链表
循环遍历局部关键帧的链表，获取存在匹配关系的MapPoint，循环这些MapPoint，判断是否指向某一个MapPoint，继而判断这个MapPoint是否bad，最后判断其为哪一帧进行的局部BA，如果不是当前优化帧，那么就将这个MapPoint存入局部MapPoint，将该点的进行局部BA的帧赋值当前帧的帧号。
定义存放关键帧的固定相机链表
循环局部MapPoint，获取每个MapPoint的观测信息，遍历观测信息，取观测的关键帧，判断其是否已在本次该帧的优化序列中（局部BA关联的关键帧帧号）再加判断此帧固定的BA帧号是否已在本次该帧的优化序列中，如果都不在，就将这一帧的固定局部BA的帧号赋值当前优化帧的帧号，确定该帧不是bad的情况下，将该帧存入固定相机的链表内。
设置稀疏优化器，定义图对象
矩阵块，优化对象为6维，误差是3维
定义线性求解器，为该线性求解器分配稠密的增量方程
构建的过程和上面一样
如果停止的标志位为true，那么直接调用图里的设置强制停止的标志，传入该标志位。
定义无符号长整型的最大关键帧ID，初始值为0
循环遍历局部关键帧，定义SE3的节点
将每一帧的变换矩阵转换成g2o库能用的类型进行节点估计值的设定
设置节点的ID
设置第一帧关键帧固定
在图中直接添加这个节点
寻找这么多局部关键帧中帧号最大的
往下添加固定的关键帧的节点（过程都差不多，这边就不赘述）
再往下添加MapPoint的节点，预期的MapPoint的数量（（局部关键帧的数量+固定的关键帧的数量）*局部MapPoint的数量）

单目：
定义存放单目边的容器
容器的大小为预期的MapPoint的数量
定义存放关键帧的容器
容器的大小为预期的MapPoint的数量
定义存放MapPoint的容器
容器的大小为预期的MapPoint的数量

双目：
定义存放双目边的容器
容器的大小为预期的MapPoint的数量
定义存放关键帧的容器
容器的大小为预期的MapPoint的数量
定义存放MapPoint的容器
容器的大小为预期的MapPoint的数量

定义了单目和双目不同的卡方临界值
循环遍历局部MapPoint，取出MapPoint，定义其节点，获取MapPoint在世界坐标系里的坐标，转换成优化库的形式，进行初值的设置，设置边的ID、设置进行矩阵边缘化，在图内添加这个节点；获取这个节点对应的MapPoint的观测信息，循环取里面的关键帧，判断这个关键是否是bad，在是不bad的情况下，取其在帧内的像素坐标，根据对应右图中的值来判断是单目观测还是双目观测。右值小于零表明是单目观测，将观测设置成2行1列的矩阵，存入像素坐标，定义SE3的边，进行节点的连接、观测值的设置、取出该点对应的尺度因子平方的逆，构建协方差矩阵，设置鲁棒核函数，最后添加这条边，将这条边存入容器内，将关键帧也存入容器内，将这个MapPoint存入容器内。双目和单目差不太多，只是观测值是一个三维矩阵。
判断传入的停止的标志位是否为true，如果是true，直接return
初始化优化
进行迭代优化，迭代的次数为5
定义计算更多次的标志位，并初始化为true
如果传入的停止的标志位是true，直接将计算更多次的标志位赋值false
如果需要计算多次，循环取出连接节点的边，取出对应位置的MapPoint，判断该点是否是bad，如果是，直接continue；看这条边的卡方距离是否大于5.991或者这条边的深度值是否为正，如果满足其中的一条，说明这是一条异常边，那么设置下一次优化将忽略这条边，由于忽略了这条边，鲁棒核函数也被设置不用。双目也是一样的过程。
继续初始化优化
进行图优化
定义一个存放关键帧和MapPoint成pair的移除容器
容器的大小为（单目边数量+双目边数量）
循环单目边，对应取边和MapPoint，判断这个MapPoint是否是bad，如果是直接continue，取出那些卡方距离大于阈值或者MapPoint的深度值小于0的边和对应的MapPoint，将其pair在一起，存入移除容器内。
如果移除容器内的存在移除的pair，取出其关键帧和MapPoint，在该关键帧中移除这个MapPoint的匹配，在此MapPoint内移除该关键帧的观测。
循环局部关键帧，取出优化的变换矩阵。
循环局部MapPoint，取出优化的世界坐标系下的三维坐标；更新这个MapPoint的法向量和深度值。

优化Essential图
OptimizeEssentialGraph()
函数的参数列表：地图的指针、闭环关键帧的指针、当前关键帧指针、常量闭环时没被修正的Sim3、常量闭环时被修正的Sim3、闭环连接的关键帧以对应的set关键帧和常量固定尺度标志位
定义稀疏优化图
optimizer.setVerbose(false);//设置不要输出调试信息
矩阵块，优化变量的维度为7，误差项的维度为3。
定义线性求解器，赋值为增量式求解器。
定义矩阵块求解器
定义矩阵块求解器的优化方法为LM
设置初始化的系数为1e-16
图设置求解方法
获取地图中的所有关键帧
获取地图中的所有MapPoint
获取地图中的最大帧号
定义存放未修正Sim3的容器
定义存放修正Sim3的容器
定义存放Sim3指数映射的容器
定义最小Feat为100
循环遍历地图内的所有关键帧，在good的基础上，定义Sim3的节点，取关键帧的帧号为ID，在修正过的Sim3中找该关键帧是否被修正过，如果其中有，就取出这个点的S，直接设置这个节点的估计值；如果还没有，取出该关键帧内旋转和平移矩阵，调用g2o中的Sim3，组成一个带尺度的变换矩阵并将其设置成节点的初始值。 如果当前关键帧为闭合处的关键帧，那么就将该节点固定，设置这个节点的ID、不进行矩阵边缘化、节点内的固定尺度赋值传入的那个参数。 在图中添加这个节点，将此节点存放进容器内
定义存放pair<long unsigned int,long unsigned int>的容器
定义了7×7的单位矩阵
循环遍历闭环连接的容器，取出关键帧及其ID，取对应的连接关键帧，取对应的Sim3（世界坐标系到当前），计算Sim3的逆矩阵（当前到世界坐标系）；循环遍历set内存放的关键帧，取出帧号，先判断之前被连接的关键帧是否是那个当前帧或者取连接内的那一帧是否为闭环帧，只有都满足其中的一个要求，或者满足循环内的这一帧的权重要不少于阈值，取出这一帧对应的Sim3，将其右乘上之前对应的单一关键帧的Sim3得到这两关键帧之间的Sim3，定义Sim3的边，用边将这两个节点连接起来，该帧的观测值就是上面计算得到的两帧之间的Sim3，设置的信息矩阵为7×7的单位阵，在图中添加这条边，建立两帧帧号之间的pair，存入插入帧的容器内。
循环遍历所有的关键帧，取出其中的关键帧和帧号，定义g2o中的Sim3（当前到世界坐标系），在未修正的Sim3容器中寻找这个关键帧，如果能找到，就将存储的Sim3取逆赋值给之前定义的那个变量；如果没找到，就直接取矩阵容器内该ID对应的Sim3矩阵；取这个关键帧的父节点，如果存在父节点，取这个父节点的帧号，定义父节点的Sim3，寻找父节点的Sim3变换矩阵，计算子节点和父节点之间的Sim3，定义Sim3的边，将子节点和父节点用边连接起来，边的信息矩阵为7×7的单位矩阵，在图内添加这条边。获取该关键帧的闭环边，添加帧号比自己小的关键帧，其他过程都类似。获取按权重排序的连接关键帧，循环遍历连接的关键帧，在是关键帧且该帧不是这个关键帧的父节点且该帧也不是这一帧的子节点且闭环帧的容器内不包含此帧，另外，这一帧是good的关键帧却其帧号小于当前的关键帧，且不在已经插入边的容器内，定义边进行连接等一些操作，完成边的添加。
初始化优化
迭代优化20次
循环遍历所有的关键帧，取关键帧及其帧号，取出优化完的Sim3，取逆得到当前到世界坐标系的Sim3，获取旋转矩阵、平移矩阵和尺度值。构建新的变换矩阵。设置新的关键帧Pose
循环遍历所有的MapPoint，在该MapPoint是好的情况下，判断该点是在哪一个关键帧内被优化的，如果是当前帧，那就取该点被修正的参考值赋值给参考ID；如果不是当前帧，取该点的参考关键帧，帧号赋值给参考ID。
取存在Sim3容器中内对应的Sim3
取修正容器中对应的Sim3
获取该MapPoint的世界坐标
将该坐标转换成3行1列的矩阵
根据这个矩阵得到矫正完的MapPoint的世界坐标
重新设置该MapPoint的世界坐标
更新其法向量和深度值

优化Sim3
OptimizeSim3()
函数的参数列表：关键帧1、关键帧2、存放MapPoint的匹配容器、2到1的Sim3初值、阈值2和固定尺度的标志位
定义稀疏优化图
定义线性求解器
将线性求解器赋值为增量式求解器
定义矩阵块求解器
选用LM进行优化计算
获取两帧的内参矩阵
获取两帧的位姿，分别取出旋转和平移矩阵
定义Sim3的节点
节点的固定尺度值赋值传入的参数
估计Sim3赋值为传入的参数
设置ID为0
节点内传入两帧的内参元素
在图内添加这个节点
取出两帧匹配的MapPoint
获取关键帧1内的匹配关系
定义存放Sim3边的容器
定义存放逆Sim3边的容器
定义存放边索引的容器
这三个容器的大小为2倍的MapPoint的点数
定义Huber的阈值
死你故意匹配数量为0
循环这些MapPoint，排除没有匹配关系的关键点，分别取出MapPoint的指针，索引值2×i+1和2×（i+1），取MapPoint在帧2中的索引。在两个MapPoint指针都有指向时，判断该MapPoint在各自的帧内是否是good，且在帧2内的索引大于等于0，如果都满足条件，就添加这个MapPoint在各自帧内坐标系下的坐标的节点，且节点固定，节点的ID帧1内为1,3,5,7…，帧2内为2,4,6,8…匹配数量自增，往下就是添加节点和节点之间边了，主要基于以下两个公式：
x1 = S12X2
x2 = S21X1
具体过程如下图：

初始化优化
优化迭代5次
定义bad的数量为0
循环取出对应的一组边，在保证此边存在的情况下，计算重投影误差，两边中只要有一边超过阈值，直接删除匹配关系，图中也移除这两条边，容器内也置空，bad数量++
定义int的更多迭代次数
只要存在bad边，就再迭代优化10次，不存在bad边，就再迭代优化5次
如果边数移除的善于10，直接return 0
初始化优化图
进行更多次数的迭代优化
定义内点数量
循环取出对应边，存在边的情况下，看重投影误差，两边中只要有一边大于阈值，将删除匹配关系；满足阈值要求，内点数量++
获取优化完的Sim3
将优化的Sim3赋值给传入参数g2oS12
最后返回内点的数量

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