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可以表示为：

这里的$\rho_h$ 就是Huber robust cost function（？）