优化

如何优化

1 优化的目标函数在SLAM问题中,常见的几种约束条件为: 1. 三维点到二维特征的映射关系(通过投影矩阵);2. 位姿和位姿之间的变换关系(通过三维刚体变换);3. 二维特征到二维特征的匹配关系(通过F矩阵);5. 其它关系(比如单目中有相似变换关系)。如果我们能够知道其中的某些关系是准确的,那么可以在g2o中定义这样的关系及其对应的残差,通过不断迭代优化位姿来逐步减小残差和,从而达到优化位姿的目标。

2 局部优化

当新的关键帧加入到convisibility graph时,作者在关键帧附近进行一次局部优化,如下图所示。Pos3是新加入的关键帧,其初始估计位姿已经得到。此时,Pos2是和Pos3相连的关键帧,X2是Pos3看到的三维点,X1是Pos2看到的三维点,这些都属于局部信息,共同参与Bundle Adjustment。同时,Pos1也可以看到X1,但它和Pos3没有直接的联系,属于Pos3关联的局部信息,参与Bundle Adjustment,但取值保持不变。Pos0和X0不参与Bundle Adjustment。

因此,参与优化的是下图中红色椭圆圈出的部分,其中红色代表取值会被优化,灰色代表取值保持不变。(u,v)是X在Pos下的二维投影点,即X在Pos下的测量(measurement)。优化的目标是让投影误差最小。

优化

3 全局优化

在全局优化中,所有的关键帧(除了第一帧)和三维点都参与优化。

 

优化

4 闭环处的Sim3位姿优化

当检测到闭环时,闭环连接的两个关键帧的位姿需要通过Sim3优化(以使得其尺度一致)。优化求解两帧之间的相似变换矩阵,使得二维对应点(feature)的投影误差最小。

如下图所示,Pos6和Pos2为一个可能的闭环。通过(u4,2,v4,2)和(u4,6,v4,6)之间的投影误差来优化S6,2

优化

5 Sim3上的位姿优化

单目SLAM一般都会发生尺度(scale)漂移,因此Sim3上的优化是必要的。相对于SE3,Sim3的*度要多一个,而且优化的目标是矫正尺度因子,因此优化并没有加入更多的变量(如三维点)。

作者在检测到闭环时在Sim3上对所有的位姿进行一次优化。定义Sim3上的残差如下:

优化

优化

 

 优化

 

上一篇:Deploy != Release(第一部分):部署与发布的区别,以及为什么这很重要


下一篇:matlab三体运动