ORB-SLAM: A Versatile and Accurate Monocular SLAM Syetem 讲解 3

转载请注明：http://blog.csdn.net/c602273091/article/details/54428693

在ORB-SLAM中，地图初始化设计homography和fundamental matrix。在BA方面涉及cost function、optimization、李群。位姿优化方面有Sim约束。

非线性优化

Bundle Adjustment

首先是定义误差：

把一个点云在经过摄像头的位姿作用下（外参），又通过映射函数进行计算在第i帧第j个特征点的位置（就是点云映射到了图片上，图片的坐标）

首先是摄像头作用：（把点云通过选择和平移映射到了摄像头前面的坐标）

然后是在normilized plane对坐标点做一个归一化。

以上过程可以用下图表示：

论文上的SE(3)和SO(3)关于李群方面的理论之后再解释。

所以整个cost function可以表示为：

这里的ρh 就是Huber robust cost function（？）Ω−1i,j=σ2i,jI2X2是关键点的scale的协方差矩阵。我们的目标就是让上面的cost代价最小化。这个问题就可以用g2o进行解决。里面用得比较多的就是LM算法迭代求最优解。

位姿图优化

在Sim(3)的约束下，然后可以计算出了Sim(3)计算约束可以得到cost：

计算闭环的时候，使用Horn【42】中的方法（待看） 计算logSim3【48】进行tan空间转换。

所以最后关键帧的pose的优化就是：

这里的Λ就是信息矩阵。

相对Sim(3)优化

给n个匹配，（i->j，从关键点到点云）从关键帧1和2优化刚才优化pose的S12，可以计算其误差：

cost function可以表示为：

graph SLAM

要了解g2o以前，需要知道graph slam究竟是什么？
graph SLAM这里的教程当然就是Joan Sola的Course on SLAM

如果要通俗一点的话，可以这么解释：【2】

前面是用闭环检测，这次用观测的路标（landmark）来构建边。如下图所示，假设一个机器人初始起点在0处，并观测到其正前方2m处有一个路标。然后机器人向前移动，通过编码器测得它向前移动了1m，这时观测到路标在其前方0.8m。请问，机器人位姿和路标位姿的最优状态？

在这个图中，我们把路标也当作了一个顶点。构建边的关系如下：

求偏导数：

接下来，将引入了一个重要的概念。我们知道传感器的精度是有差别的，也就是说我们对传感器的相信程度应该不同。比如假设这里编码器信息很精确，测得的路标距离不准，我们应该赋予编码器信息更高的权重，假设是10。重新得到残差平方和如下：

将这个结果和之前对比，可以看到这里的机器人位姿x1更靠近编码器测量的结果。

有了上面的思想之后，我们计算出来的误差就是用计算的点云通过映射到图片的位置与真实的特征点的位置，然后我们要做的就是最小化这个误差。因为同一个点云会在不同的图片中出现，所以需要一起优化。同时前后帧是有一定联系的，所以采用BA进行一个全局优化。
我们需要优化的就是：

所以我们发现，误差的权重矩阵（正式名称为信息矩阵）等于协防差矩阵的逆。由于图优化里每一条边代表一个测量值，如表示相邻位姿关系的编码器测量值或者图像（激光）匹配得到的位姿变换矩阵。所以图优化里每一条边的信息矩阵就是这些测量协防差矩阵的逆。如果协防差越小，表示这次测量越准越值得相信，信息权重就越大。

我们看到图优化问题变成了求解最小二乘问题，然而机器人位姿之间的变化函数不是线性的，所以是个非线性最小二乘问题，得通过迭代法进行求解。如果迭代开始时有一个好的初始假设值，那我们就能用Guass-Newton法或者 Levenberg-Marquardt法了【3】。

g2o

参考半闲居士的文章【4】。

多角度几何学

这个方面的内容看MVG这本神书就可以了。

homography

这个东西在学习标定的时候有说到，具体参考我的博客：【5】我从摄像头标定开始讲起这个问题，可能更加浅显易懂，或者这位美女的博客【6】我会在后续博客中特别说明这个并附上代码解释。

fundamental matrix

参考我的博客【7】将会给出代码以及具体的计算。

【1】Joan Sola，Course on SLAM
【2】http://blog.csdn.net/heyijia0327/article/details/47686523
【3】blog.csdn.net/heyijia0327/article/details/47731631
【4】http://www.cnblogs.com/gaoxiang12/p/5244828.html
【5】http://blog.csdn.net/c602273091/article/details/54572428
【6】http://blog.csdn.net/heyijia0327/article/details/53782094
【7】待更新
。。。