Menu
Random Explore

ORB-SLAM3源码阅读笔记12：对系统中前端相关优化函数(视觉IMU联合)的分析

Jan 10,2022   23121 words   83 min

Tags: SLAM

在之前这篇博客中我们介绍了ORB-SLAM2/3前端中使用的纯视觉优化函数：PoseOptimization()和LocalBundleAdjustment()。在之前这篇博客中，我们重点分析了与IMU相关的优化函数。 在本篇博客中，我们继续分析ORB-SLAM3中的优化——前端中视觉IMU联合的优化函数，深入函数内部，看看到底都做了些什么。主要包括三个函数：PoseInertialOptimizationLastKeyFrame()、PoseInertialOptimizationLastFrame()、LocalInertialBA()。

这里不得不说，IMU和相机是天然“互补”的传感器组合。相机作为低频观测，可以提供丰富的视觉信息，但作为一种“外向型”传感器其数据观测的好坏一定程度上依赖于周围的环境条件。如果条件比较恶劣(如大雾、大雪、大雨、弱光照等)，就会严重降低视觉信息的价值。另一方面，IMU观测相对高频，可以在一定程度上“弥合”离散的视觉观测帧，将各帧直接关联起来。同时IMU作为一种“内向型”传感器，其观测值并不会因为外界因素干扰而产生明显变化(一般情况，非极端)，这给视觉传感器提供了非常好的补充。当然IMU也有自身问题，如存在长期漂移、误差累积等。这也注定了无法长期只用IMU进行位姿估计，需要不断用视觉对IMU参数和估计进行校正。

1.PoseInertialOptimizationLastKeyFrame()

根据之前对于该函数的分析，它只在Tracking的TrackLocalMap()中被调用了一次，其和之前介绍过的纯视觉优化函数PoseOptimization()同级、作用相同，都是用来对当前帧位姿进行优化的。不同之处在于它引入了IMU，构造了视觉-IMU联合优化图。

1.1 输入是什么

根据函数的定义，输入其实也十分简单，如下。 一个是待优化位姿的Frame类型影像帧pFrame，另一个是flag变量，默认为false。

当然有了之前的经验也可以预判到，函数中肯定又调用了Frame的成员变量和函数来获取一些信息，这里还是整理列出如下:

  • (1) pFrame->N: 获取输入帧提取的特征点个数
  • (2) pFrame->Nleft: 获取左目影像上提取的特征点个数(双目模式下)
  • (3) pFrame->mpLastKeyFrame: 输入帧关联的上一个关键帧
  • (4) pFrame->mpImuPreintegrated: 当前帧的预积分结果
  • (5) pFrame->mvuRight[i]: 根据索引，获取某个地图点对应的右影像上的横坐标(针对双目情况，该值默认为-1)
  • (6) pFrame->mvKeysUn[i]/mvKeys[i]: 根据索引，获取某个cv::KeyPoint类型的特征点对象
  • (7) pFrame->mvbOutlier[i]: 根据索引，获取某个地图点是否是离群点的flag(该值默认为false)
  • (8) pFrame->mpCamera: 获取输入帧关联的相机模型
  • (9) pFrame->mvInvLevelSigma2[kpUn.octave]: 根据特征点所在金字塔层数获取对应的sigma值
  • (10) pFrame->mvpMapPoints[i]: 根据索引，获取当前帧关联的某个MapPoint类型的地图点

当然了，除了上面的直接访问，还有一些是“拐了很多弯”的间接访问。比如在构造一些优化图节点的时候，又将传入的帧原封不动地传给了节点的构造函数，如下。 在这些构造函数中，又访问了输入帧的其它属性。这里同样列举出来。

  • (11) pF->GetImuPosition(): 获得当前帧IMU的位置
  • (12) pF->GetImuRotation(): 获得当前帧IMU的旋转
  • (13) pF->mTcw: 获得当前帧的位姿
  • (14) pF->mImuCalib: 获得当前帧的IMU参数
  • (15) pF->mbf/mTrl: 获得当前帧相机的双目基线、变换矩阵
  • (16) pF->mVw: 获得当前帧的速度
  • (17) pF->mImuBias: 获得当前帧IMU的偏置

对于从输入帧中获得的MapPoint地图点(通过第10点获得)，也有一些成员函数调用：

  • (18) pMP->GetWorldPos(): 获得某个地图点在世界坐标系下的真实坐标

对于从输入帧中获得的上个KeyFrame关键帧(通过第3点获得)，也有一些成员函数调用：

  • (19) pKF->GetImuPosition(): 获得上一个关键帧IMU的位置
  • (20) pKF->GetImuRotation(): 获得上一个关键帧IMU的旋转
  • (21) pKF->GetVelocity(): 获得上一个关键帧的速度
  • (22) pKF->GetGyroBias(): 获得上一个关键帧的陀螺仪偏置
  • (23) pKF->GetAccBias(): 获得上一个关键帧的加速度计偏置
1.2 执行了什么

与之前类似的，函数遵循使用G2O进行优化的一般流程，下面也简单介绍。

1.2.1 优化器设置

如下所示，是设置优化算法与优化器的步骤。 可以看到，这里我们选择了高斯牛顿算法。

1.2.2 优化图构建

在设置好基本优化器之后，我们就可以着手构建优化图了。当前其实这两个步骤没有必要的依赖关系，顺序不会影响结果。对于视觉-IMU融合的位姿优化，相比于之前介绍的PoseOptimization()要更为复杂一些。遵循节点为优化变量，边为约束的核心思想进行优化图构建，主要是增加了IMU相关的节点与边，下面分别解析。

(a) 节点构建

输入帧位姿节点

如下图所示，将输入帧作为参数，构造了VertexPose类型的输入帧位姿节点VP。 我们把它的ID设为0,并且设为不固定(可优化)。然后就把它添加到优化器中了，没有什么特别好说的。这里需要注意的是，VertexPose类型是ORB-SLAM3中自定义的节点类型，在G2OTypes中被定义。关于这些自定义的节点和边类型，在后面的部分进行介绍。

输入帧速度节点

如下图所示，将输入帧作为参数，构造了VertexVelocity类型的输入帧速度节点VV。 我们将这个节点ID设为1，也是设为可优化，然后添加到优化器中。

输入帧陀螺仪偏置节点

如下图所示，将输入帧作为参数，构造了VertexGyroBias类型的输入帧陀螺仪偏置节点VG。 我们将这个节点ID设为2，也是设为可优化，然后添加到优化器中。

输入帧加速度计偏置节点

如下图所示，将输入帧作为参数，构造了VertexAccBias类型的输入帧加速度计偏置节点VA。 我们将这个节点ID设为3，也是设为可优化，然后添加到优化器中。

关键帧位姿节点

我们首先利用输入帧的成员变量mpLastKeyFrame获得输入帧的上一个关键帧。然后以此关键帧进行关键帧相关节点构建。如下所示。 我们将上一个关键帧作为参数传入VertexPose节点的构造函数，建立了关键帧位姿节点VPk，然后将其ID设为4。这里和上面帧位姿节点不同的是，我们将其设为fixed，也就是说在优化时固定住不可更改。事实上可以发现，所有关键帧相关的节点在这里都被设为fixed。这样的意义是以上一个关键帧为参考/基准(提供已知信息)，来优化、调整当前帧位姿。

关键帧速度节点

如下图所示，将上一个关键帧作为参数，构造了VertexVelocity类型的关键帧速度节点VVk。 我们将这个节点ID设为5，并且设为fixed，然后添加到优化器中。

关键帧陀螺仪偏置节点

如下图所示，将上一个关键帧作为参数，构造了VertexGyroBias类型的关键帧陀螺仪偏置节点VGk。 我们将这个节点ID设为6，并且设为fixed，然后添加到优化器中。

关键帧加速度计偏置节点

如下图所示，将上一个关键帧作为参数，构造了VertexAccBias类型的关键帧加速度计偏置节点VAk。 我们将这个节点ID设为7，并且设为fixed，然后添加到优化器中。

至此，我们一共构建了8个节点，4个由输入帧建立，4个由输入帧的上一个关键帧建立。下面就要着手去构建边了。

(b) 边构建

如下图所示，我们首先通过pFrame->N获取输入帧提取的特征点个数，然后遍历这些特征点，根据不同的条件(单目/双目)添加地图点观测边。

单目地图点观测边

如下图所示，是单目地图观测边的添加步骤。 可以看到，整个流程和PoseOptimization()函数中是一样的。单目地图观测边对应单目影像中的特征点以及双目左影像中没有双目匹配的特征点。我们新建了一个EdgeMonoOnlyPose边，并且将边的节点设为我们一开始建立的输入帧位姿节点VP(ID为0)。而边的观测则是特征点的x、y坐标。然后我们设置边的信息矩阵、鲁棒核等属性。完成之后，将边放到优化图中。可以看到，我们新建的边只连接了一个节点，所以这是一个一元边。

双目地图点观测边

如下图所示，是双目地图观测边的添加步骤。 可以看到双目边和单目观测边有类似的流程。区别在于观测由2个数字变成了3个数字。多了一个右影像的坐标。并且将边的节点设为我们一开始建立的输入帧位姿节点VP(ID为0)。

惯性边

如下图所示，继续向优化图中添加惯性边。 可以看到这是一个多元边，连接了多个节点。具体包括关键帧位姿节点VPk、关键帧速度节点VVk、关键帧陀螺仪偏置节点VGk、关键帧加速度计偏置节点VAk、输入帧位姿节点VP、输入帧速度节点VV。最后，将节点添加到优化图中。

陀螺仪边

如下图所示，继续向优化图中添加陀螺仪边。 可以看到，这是一个二元边，一边连接的是关键帧陀螺仪偏置节点VGk，另一边连接输入帧陀螺仪偏置节点VG。

加速度计边

如下图所示，继续向优化图中添加加速度计边。 与陀螺仪边类似的，这也是一个二元边，一边连接的是关键帧加速度计偏置节点VAk，另一边连接输入帧加速度计偏置节点VA。

至此，我们的优化图就构建好了，一个简单的示意图如下所示。

1.2.3 迭代优化

构造好优化图之后下面就是进行迭代优化，如下图所示。 与PoseOptimization()函数类似的，我们一共会进行4次优化，每次优化会进行10次迭代。整体的运行逻辑也类似。每执行完一次优化以后，就看看有没有离群边，如果有的话就将其level属性设为1，这样下次迭代的时候就不会再优化这些变量了。对于单目边和双目边都会进行这些判断。如果剩下的好的边少于10个，就退出函数，说明优化失败。

如果一切顺利，优化以后就会尝试更新输入帧的位姿、速度以及IMU的偏置，如下图所示。 可以看到，通过调用Frame的SetImuPoseVelocity()成员函数，将优化后的数据重新赋给输入帧。我们可以进一步点开这个函数，如下。 可以看到这个函数将传入的数据经过一系列计算，最终赋给了一些成员变量。 此外，还将优化更新以后的陀螺仪偏置、加速度计偏置重新赋给了Frame的成员变量mImuBias。

最后，遍历单目、双目边，计算海塞矩阵。并用所有最新的数据构造一个ConstraintPoseImu类型的约束，赋给Frame的成员变量mpcpi，如下。 mpcpi这个变量其实是很重要的，它会在PoseInertialOptimizationLastFrame()中被调用，在KeyFrame初始化的时候被赋初值为NULL。在整个系统中，mpcpi只在PoseInertialOptimizationLastKeyFrame()和PoseInertialOptimizationLastFrame()函数中被真正有意义地赋值。如果它为空的话，那么PoseInertialOptimizationLastFrame()函数就会直接退出优化。

1.3 输出是什么

以上便是对PoseInertialOptimizationLastKeyFrame()的分析。从字面上来说，函数的输出为经过过滤以后剩余的内点的个数，如下所示。 当然了事实也并不会这么简单。通过对Frame类成员函数或者变量的调用，修改了很多内容。下面列举：

  • (1) pFrame->mvbOutlier[idx]: 根据条件将某个地图点设置为外点
  • (2) pFrame->SetImuPoseVelocity(): 将优化后的数据赋给Frame类的对应变量，包括位姿(旋转与平移)、速度
  • (3) pFrame->mImuBias: 将优化后的陀螺仪、加速度计偏置赋给Frame对应变量mImuBias
  • (4) pFrame->mpcpi: 将构造好的IMU约束赋给Frame对应变量mpcpi

2.PoseInertialOptimizationLastFrame()

上面我们介绍了PoerInertialOptimizationLastKeyFrame()函数。在ORB-SLAM3中，还有一个“对应”的函数，就是PoseInertialOptimizationLastFrame()。从字面意思来看，一个是针对关键帧的、一个是针对普通帧的，但这两个函数的输入都是普通的Frame类，而非KeyFame类，如下。如何理解呢？ 这里所谓的LastFrame、LastKeyFrame指的并不是输入数据的类型，而是构造优化图时所依赖的帧的类型。在PoerInertialOptimizationLastKeyFrame()函数中，构造优化图时使用了当前帧的上一个关键帧(调用Frame的成员变量mpLastKeyFrame)，而在PoseInertialOptimizationLastFrame()函数中，就只是使用了当前帧的普通上一帧(调用Frame的成员变量mpPrevFrame)。

2.1 输入是什么

函数的输入和PoseOptimizationLastKeyFrame()函数类似，这里也简单列举。

  • (1) pFrame->N: 获取输入帧提取的特征点个数
  • (2) pFrame->Nleft: 获取左目影像上提取的特征点个数(双目模式下)
  • (3) pFrame->mpPrevFrame: 输入帧关联的上一帧
  • (4) pFrame->mpImuPreintegratedFrame: 当前帧的预积分结果
  • (5) pFrame->mvpMapPoints[i]: 根据索引，获取当前帧关联的某个MapPoint类型的地图点
  • (6) pFrame->mvuRight[i]: 根据索引，获取某个地图点对应的右影像上的横坐标(针对双目情况，该值默认为-1)
  • (7) pFrame->mvKeysUn[i]/mvKeys[i]: 根据索引，获取某个cv::KeyPoint类型的特征点对象
  • (8) pFrame->mvbOutlier[i]: 根据索引，获取某个地图点是否是离群点的flag(该值默认为false)
  • (9) pFrame->mpCamera: 获取输入帧关联的相机模型
  • (10) pFrame->mvInvLevelSigma2[kpUn.octave]: 根据特征点所在金字塔层数获取对应的sigma值
  • (11) pFrame->mvKeysRight[i - Nleft]: 获取左目影像上某个特征在右目影像上的对应特征

除此之外，也有一些“间接”的访问，将传入的pFrame直接给顶点的构造函数。相关调用列举如下。

  • (12) pF->GetImuPosition(): 获得当前帧IMU的位置
  • (13) pF->GetImuRotation(): 获得当前帧IMU的旋转
  • (14) pF->mTcw: 获得当前帧的位姿
  • (15) pF->mImuCalib: 获得当前帧的IMU参数
  • (16) pF->mbf/mTrl: 获得当前帧相机的双目基线、变换矩阵
  • (17) pF->mVw: 获得当前帧的速度
  • (18) pF->mImuBias: 获得当前帧IMU的偏置

对于从输入帧中获得的MapPoint地图点(通过第5点获得)，也有一些成员函数调用：

  • (19) pMP->GetWorldPos(): 获得某个地图点在世界坐标系下的真实坐标
2.2 执行了什么

还是类似的，可以分为优化器设置、优化图构建、迭代优化三个主要的步骤，下面简单说明。

2.2.1 优化器设置

在本函数中，我们还是选择高斯牛顿法作为优化方法，如下图所示。

2.2.2 优化图构建

这里的优化图构建同样是类似的，分为节点和边，下面还是分开说明。

(a) 节点构建

输入帧位姿节点

如下所示，构建输入帧的位姿节点VP。 节点ID为0，为可优化状态。

输入帧速度节点

如下所示，构建输入帧速度节点VV。 节点ID为1，为可优化状态。

输入帧陀螺仪偏置节点

如下所示，构建输入帧陀螺仪偏置节点VG。 节点ID为2，为可优化状态。

输入帧加速度计偏置节点

如下所示，构建输入帧加速度计偏置节点VA。 节点ID为3，为可优化状态。

上一帧位姿节点

如下所示，构建输入帧上一帧的位姿节点VPk。 节点ID为4，为可优化状态。这里需要注意，在PoseInertialOptimizationLastKeyFrame()函数中，我们获取输入帧的上一个关键帧，构建上一个关键帧相关节点的时候，我们是把它固定住作为一种参考的。但是这里，我们把上一帧的相关节点也作为可以优化的对象。

上一帧速度节点

如下所示，构建输入帧上一帧的速度节点VVk。 节点ID为5，为可优化状态。

上一帧陀螺仪偏置节点

如下所示，构建输入帧上一帧的陀螺仪偏置节点VGk。 节点ID为6，为可优化状态。

上一帧加速度计偏置节点

如下所示，构建输入帧上一帧的加速度计偏置节点VAk。 节点ID为7，为可优化状态。

可以看到，类似的，我们一共构建了8个节点，4个由当前帧创建、4个由上一帧创建。

(b) 边构建

单目地图点观测边

如下图所示，遍历当前帧关联的地图点，构建单目地图点观测边。 这里构建的还是一元边。边只指向当前帧位姿节点VP。

双目地图点观测边

如下图所示，遍历当前帧关联的地图点，构建双目地图点观测边。 这里构建的还是一元边。边只指向当前帧位姿节点VP。

惯性边

如下图所示，根据当前帧的预积分结果构建惯性边ei。 可以看到，这里构建的依然是一个连接多个节点的多元边，连接了上一帧位姿节点VPk、上一帧速度节点VVk、上一帧陀螺仪偏置节点VGk、上一帧加速度计偏置节点VAk、当前帧位姿节点VP、当前帧速度节点VV。

陀螺仪边

如下图所示，构建了陀螺仪边egr。 该边连接了上一帧陀螺仪偏置节点VGk和当前帧陀螺仪偏置节点VG。

加速度计边

如下图所示，构建了加速度计边ear。 该边连接了上一帧加速度计偏置节点VAk和当前帧加速度计偏置节点VA。

IMU先验位姿边

如下图所示，利用当前帧上一帧的成员变量mpcpi构建了IMU先验位姿边。 该边连接了上一帧位姿节点VPk、上一帧速度节点VVk、上一帧陀螺仪偏置节点VGk、上一帧加速度计偏置节点VAk。

至此，所有的边就构建完成了。相比于PoseInertialOptimizationLastKeyFrame()，增加了IMU先验位姿边。

并且，所有的边和节点也都构建完成了，可以用一个简单的示意图表示。 可以看到，相比于PoseInertialOptimizationLastKeyFrame()构建的优化图，上一帧的节点之间多了一条专门的多元边进行连接。并且，这些节点不再固定。

2.2.3 迭代优化

迭代优化的流程还是一样的，这里就不再赘述。 我们会执行4次优化，每次有10次迭代。

执行完成以后，就会将优化后的数据更新到输入帧中，如下图所示。 主要更新了输入帧的位姿、速度、IMU偏置、IMU约束。

2.3 输出是什么

函数的输出其实和PoseInertialOptimizationLastKeyFrame()是一模一样的。

  • (1) pFrame->mvbOutlier[idx]: 根据条件将某个地图点设置为外点
  • (2) pFrame->SetImuPoseVelocity(): 将优化后的数据赋给Frame类的对应变量，包括位姿(旋转与平移)、速度
  • (3) pFrame->mImuBias: 将优化后的陀螺仪、加速度计偏置赋给Frame对应变量mImuBias
  • (4) pFrame->mpcpi: 将构造好的IMU约束赋给Frame对应变量mpcpi

3.LocalInertialBA()

上面，我们分析了针对单帧的两个视觉IMU联合优化函数，与之前PoseOptimization()函数一样的，它们也是只针优化帧位姿，不会优化地图点。在前端中，纯视觉还有个LocalBundleAdjustment()函数用于进行局部BA。那么对于视觉IMU联合的情况也不例外，有LocalInertialBA()函数。遵循上面的结构，对这个函数进行简单的分析。由于整个函数是在LocalBundleAdjustment()函数上扩展得到的，所以如果能这个函数有一定的了解，再看LocalInertialBA()函数就会更容易一些。LocalBundleAdjustment()函数见之前的这篇博客。

3.1 输入是什么

看函数的定义如下: 可以看到有较多的参数，但主要的输入还是当前关键帧、是否停止的flag变量以及当前地图的指针。后面几个num_开头的则是函数输出的一些统计结果，如固定关键帧的个数num_fixedKF、优化关键帧的个数num_OptKF、地图点个数num_MPs、边的个数num_edges。最后还有两个flag类型的变量，一个是bLarge。它用于指示当前需要优化的局部地图是否比较大，如果是的话，bLarge就为true。在函数内部，就会根据不同状态设置不同优化参数，如下。 另一个问题是，怎样才算“大”？我们可以从调用LocalInertialBA()的LocalMapping::Run()函数中找到答案，如下。 可以看到，对于单目而言，如果跟踪的地图点超过75个、双目跟踪的地图点超过100个，就认为是比较大的。

遵循同样的套路，我们查找函数中对于输入变量的相关调用，如下。

  • (1) pKF->GetMap(): 获取传入关键帧所在的地图
  • (2) pKF->mnId: 获取传入关键帧ID
  • (3) pKF->mnBALocalForKF: 获取执行当前局部BA的关键帧ID(也就是输入函数的那个关键帧ID)
  • (4) pKFi->GetMapPointMatches(): 迭代vpNeighsKFs，依次获取关键帧关联的地图点
  • (5) pKFi->mnBAFixedForKF: 获取执行局部BA的固定关键帧ID
  • (6) pKFi->isBad(): 获取关键帧的状态是不是好的
  • (7) pKFi->bImu: 获取关键帧是否使用了IMU
  • (8) pKFi->mPrevKF: 获取迭代的某个关键帧的上一个关键帧
  • (9) pKFi->mpImuPreintegrated: 获取迭代某一帧的预积分结果
  • (10) pKFi->mvuRight[leftIndex]: 获取某个左影像的特征点在右影像上的对应
  • (11) pKFi->mvKeysUn[leftIndex]: 获取对应的特征点
  • (12) pKFi->mpCamera: 获取相机模型
  • (13) pKFi->mvInvLevelSigma2[kpUn.octave]: 获取某个特征点所在金字塔层级的尺度信息
  • (14) pKFi->NLeft: 获取某个关键帧中提取的特征点个数

除此之外，在构造节点时也传入了一些关键帧，在这些节点的构造函数中也“间接使用”了一些关键帧的成员函数和变量，如下。

  • (15) pKF->GetImuPosition(): 获得当前关键帧IMU的位置
  • (16) pKF->GetImuRotation(): 获得当前关键帧IMU的旋转
  • (17) pKF->GetRotation(): 获得当前关键帧的旋转
  • (18) pKF->mImuCalib: 获得当前关键帧的IMU参数
  • (19) pKF->GetVelocity(): 获得当前关键帧的速度
  • (20) pKF->GetGyroBias(): 获得当前关键帧的陀螺仪偏置
  • (21) pKF->GetAccBias(): 获得当前关键帧的加速度计偏置

地图点相关的调用也列举如下。

  • (22) pMP->GetWorldPos(): 获得某个地图点在世界坐标系下的真实坐标
  • (23) pMP->isBad(): 返回某个地图点的状态，是否是坏的
  • (24) pMP->mnId: 获取某个地图点的ID
  • (25) pMP->GetObservations(): 获取某个地图点所对应的观测，简单来说就是关键帧+该地图点对应的特征点在该关键帧中的索引。
  • (26) pMP->mTrackDepth: 地图点的跟踪深度，在Frame::isInFrustum()函数中被计算
3.2 执行了什么

整个函数还是基于G2O优化的基本流程在走，下面简单介绍。

3.2.1 输入数据预处理

其实在ORB-SLAM系统中，对数据的输入与处理流程基本是相同的，如下：

  • 输入关键帧
  • 根据关键帧找关联帧(基于共视或者时序关系，如GetVectorCovisibleKeyFrames())
  • 根据帧找地图点(利用GetMapPointMatches())
  • 根据地图点找观测(利用GetObservations())

下面分别进行介绍。和之前LocalBundleAdjustment()基本相同的，“万恶之源”都是传入的关键帧pKF，如下图所示。 首先，我们根据传入的关键帧pKF的成员函数GetVectorCovisibleKeyFrames()获取与当前关键帧共视的其它关键帧，放到vpNeighsKFs中，然后暂时就先放在一边，如下图所示。 之后，我们建立了一个新的vector叫vpOptimizableKFs，这里面用于存放可优化的关键帧。首先，我们先将传入的关键帧放进来。然后我们遍历这个vector，每次都取出它的最后一个元素，并且获得这个元素的上一个关键帧。如果它存在的话，我们就把它也给添加到vector中来(末尾)。然后，又一次遍历这个vector，又会获得最后一个元素，重复上面的步骤。这样，我们就可以依次获得多个关键帧。而且根据遍历范围，可以看到，至多获得Nd个关键帧(包含输入关键帧)。而这个Nd是如上图中第一个红框里的步骤算出来的。简单来说就是获取当前地图中的关键帧个数-2，再将其和maxOpt比较，两者取最小值。当然了，根据bLarge的不同，maxOpt也会不同：如果bLarge为false，maxOpt为10，如果bLarge为true，maxOpt为25。举个例子，某一时刻，当前关键帧跟踪到的地图点个数大于100(双目模式)，所以bLarge为true，maxOpt为25。此时，当前地图中已有100个关键帧，据此，可以得到Nd为25和98两者的最小值，也就是25。所以接下来，我们至多会遍历25次，追踪前序关键帧放到vpOptimizableKFs。如果追溯到第10个关键帧，它没有前序关键帧了(mPrevKF为NULL)，那么就到此为止了。

我们输入了某个关键帧，函数首先将其添加到vpOptimizableKFs中。

在获取了关联的关键帧以后，我们依次遍历vpOptimizableKFs，对于每个关键帧，调用关键帧类成员函数GetMapPointMatches()，获取对应的地图点，如下图所示。 对于每个地图点，如果地图点状态OK、并且地图点的mnBALocalForKF属性不等于当前关键帧的ID，那就把这个地图点添加到lLocalMapPoints中。需要注意的是，到目前为止，我们添加的地图点都来自于输入关键帧的前序关键帧，并不是当前关键帧的共视关键帧。那前序关键帧和共视关键帧有没有可能有重叠呢？答案是肯定的。有些关键帧离输入关键帧比较近，那么就很可能既是共视关键帧，也是前序关键帧。当然如果离当前关键帧比较远，那就可能不是共视关键帧了。

然后再获取vpOptimizableKFs的最后一个元素，添加到lFixedKeyFrames，如下。 如果最后一个关键帧的上一个关键帧不为空的话，那就添加这个上一关键帧。如果没有的话，只好添加当前vpOptimizableKFs的最后一个元素。虽然lFixedKeyFrames是个list，但按照现有逻辑，它其实只会包含一个元素。当然，这里有一个非常细节但是又非常重要的操作。有这样一种情况：vpOptimizableKFs的最后一个元素确实没有上一关键帧，那么这最后一个元素就会被添加到lFixedKeyFrames。自然是没问题。但是这样岂不表示这个关键帧被添加了两遍？这样之后在遍历vpOptimizableKFs创建时序关键帧节点、lFixedKeyFrames创建固定关键帧节点的时候不久会重复吗？这是个好问题，说明看的很仔细。答案是不会。因为如果vpOptimizableKFs的最后一个元素没有关键帧，它确实会被放到lFixedKeyFrames中。但是别忘了，还有一行代码vpOptimizableKFs.pop_back()。这一行就解释了为什么上面提到的问题不会出现。因为如果添加了，那么就会把vpOptimizableKFs的最后一个元素删除，所以不会创建两个一样的对象。而且这样做了以后，vpOptimizableKFs的最后一个元素的前序关键帧就是lFixedKeyFrames中的元素(因为它们本来都在vpOptimizableKFs中，只是其中一个被挪了个位置，放到了lFixedKeyFrames)。这样的结果就是，不管你vpOptimizableKFs中有多少个关键帧，经过这种处理都可以保证vpOptimizableKFs的最后一个关键帧都有前序关键帧。因为如果你本身就有前序关键帧，没问题。如果你没有，我就强行将最后一个元素拿走，这样，原本倒数第二的元素就变成了倒数第一，但是它和原先前序关键帧之间的联系还在。所以，也有了前序关键帧。

然后，我们再添加共视关键帧相关联的地图点，如下图所示。 依次遍历共视关键帧，如果某个共视关键帧的mnBALocalForKF或者mnBAFixedForKF已经是输入关键帧ID的话，就跳过它。否则就将这个共视关键帧的mnBALocalForKF设为当前帧ID。然后进一步判断共视关键帧的状态，如果状态OK且属于当前地图(因为有多地图)的话，就将迭代的这个共视关键帧添加到lpOptVisKFs中(没办法，多地图策略就会导致逻辑更复杂一些)。并且获得这个关键帧的所有关联地图点，如果地图点状态OK且之前没有被添加过，就添加到lLocalMapPoints中。所以到这里也就可以看出vpNeighsKFs和lpOptVisKFs的区别了。lpOptVisKFs存储的关键帧是在vpNeighsKFs基础上筛选过的。

至此，基本上所有的局部地图点都添加完成了。最后一步，我们遍历添加好的所有局部地图点，调用MapPoint的成员函数GetObservations()获取地图点的观测，如下图所示。 迭代地图点，检查地图点的每个观测关键帧。如果它在之前没有添加到局部关键帧中过(根据关键帧的mnBALocalForKF和mnBAFixedForKF属性判断，因为只要是前面处理过的，肯定会把这两个属性设为输入关键帧ID)，我们就把它添加到lFixedKeyFrames中，作为优化时的参考。这些关键帧其实就是之前博客里说的“间接关键帧”。简单来说就是它和输入关键帧以及输入关键帧的共视关键帧之间没有直接联系，但是它又确实和局部地图点有一定的观测关系。这种关键帧我们只作为优化时的一种参考。

至此，我们就将所有相关的数据都进行了预处理。可以看到，相比于LocalBundleAdjustment()中的数据处理复杂了一些，大体上分为6个步骤，围绕时序和共视两条主线展开，如下：

  • (1) 将输入关键帧的共视关键帧添加到vpNeighsKFs中
  • (2) 将输入关键帧加入vpOptimizableKFs并迭代追踪上一个关键帧添加到vpOptimizableKFs
  • (3) 遍历vpOptimizableKFs中的每一个关键帧，添加关联地图点到lLocalMapPoints
  • (4) 将vpOptimizableKFs中最后一个元素或最后一个元素的前一个关键帧(如果有的话)添加到lFixedKeyFrames
  • (5) 遍历共视关键帧列表vpNeighsKFs，如果关键帧状态OK，就将这个关键帧添加到lpOptVisKFs中，并且将其对应的符合条件的地图点添加到lLocalMapPoints
  • (6) 遍历局部地图点列表lLocalMapPoints，获取每个地图点的观测，如果观测关键帧的mnBALocalForKF或mnBAFixedForKF属性不等于输入关键帧ID，就将其添加到lFixedKeyFrames

在函数中，主要有5个变量存储输入的数据：vpNeighsKFs和lpOptVisKFs用于存储共视关键帧(区别是vpNeighsKFs中可能包含非当前地图的共视关键帧，而lpOptVisKFs不会)、vpOptimizableKFs用于存储输入关键帧之前一段时间内可被用于优化的时序关键帧(包括输入关键帧)、lLocalMapPoints用于存储局部地图点、lFixedKeyFrames用于存储优化中固定(不改变)的关键帧。 这里还是简单说明一下，由于有多地图策略的存在，获得的共视关键帧可能不在同一个地图中。所以vpNeighsKFs存储的共视关键帧可能来自多个地图。而lpOptVisKFs里存储的则是经过筛选的、当前地图中的共视关键帧。事实上在优化的时候，我们更需要的是当前地图中的共视关键帧，所以用的是lpOptVisKFs。

3.2.2 优化算法选择

如下图所示，我们选择了Levenberg优化算法。 可以看到根据待优化地图点的规模，设置了不同的参数。

3.2.3 优化图构建

在数据输入完成后，我们就可以构建优化图了，如下图所示。 还是类似的，构造分为节点与边，下面分开介绍。

(a) 节点构建

时序关键帧位姿节点

如下图所示，为构建时序关键帧位姿节点VP的步骤： 可以看到，我们遍历时序关键帧列表vpOptimizableKFs进行时序关键帧位姿节点建立。节点的ID设置为关键帧的ID，状态设置为可优化(非固定)。

时序关键帧IMU相关节点

不仅如此，如果某个时序关键帧有可用的IMU数据，我们再继续创建IMU相关节点，包括速度节点VV、陀螺仪偏置节点VG、加速度计偏置节点VA，如上图中红色框框所示，它们都可被优化。它们的ID分别被设为maxKFid+3×(pKFi->mnId)+1、maxKFid+3×(pKFi->mnId)+2、maxKFid+3×(pKFi->mnId)+3。

局部视觉关键帧位姿节点

如下图所示，遍历共视关键帧列表lpOptVisKFs，建立局部视觉关键帧位姿节点。 类似的，节点ID设为关键帧ID，且设为可优化。其实这里所谓的局部视觉关键帧位姿节点就是共视关键帧位姿节点。而vpNeighsKFs和lpOptVisKFs的区别在上面也已经说过了。

固定关键帧位姿节点

如下图所示，遍历固定关键帧列表lFixedKeyFrames，建立固定关键帧位姿节点。 节点ID为关键帧ID，但设为固定(fixed)，不可优化。

固定关键帧IMU相关节点

如果某个固定关键帧有可用的IMU数据，我们再继续创建IMU相关节点，包括速度节点VV、陀螺仪偏置节点VG、加速度计偏置节点VA，如上图中红色框框所示。但它们都是固定的，不可以被优化。它们的ID分别被设为maxKFid+3×(pKFi->mnId)+1、maxKFid+3×(pKFi->mnId)+2、maxKFid+3×(pKFi->mnId)+3。

地图点节点

如下图所示，遍历局部地图点列表lLocalMapPoints，创建地图点节点。 地图点ID为pMP->mnId+iniMPid+1。当然，这里可以简单回顾一下，lLocalMapPoints中的地图点都来自哪里。除了输入关键帧自己关联的地图点外，简单来说自两个部分，一是时序关键帧(vpOptimizableKFs，根据前面的逻辑，至多25个)，二是共视关键帧(更准确的说是当前地图中的共视关键帧lpOptVisKFs)。这两部分共同构成了局部地图点列表lLocalMapPoints。

至此，节点就基本创建完了。

(b) 边构建

下面构建节点之间的边。

单目地图点观测边

如下图所示，构建单目地图点观测边。这个操作是在遍历lLocalMapPoints构建地图节点之后做的。所以这里的id指的就是地图节点的ID。 构建了一个二元边。一边连接当前地图节点，另一边连接关键帧。单目边的观测就是一个二维向量。这里有一个需要注意的地方：如果说某一个观测关键帧的mnBALocalForKF和mnBAFixedForKF都不等于输入关键帧的ID，那么我就不会添加这条边。那么mnBALocalForKF和mnBAFixedForKF在哪里被修改的呢？答案是上面构造时序关键帧位姿节点、视觉关键帧位姿节点、固定关键帧位姿节点的时候。所以简单点说就是如果某个关键帧和输入关键帧一点关系都没有，即使它和当前帧有共视地图点，我们依旧不建立观测边。

那么这些地图点是哪里来的呢？根据上面介绍的6个步骤，这些地图点主要是在第三步、第五步从时序关键帧列表vpOptimizableKFs和共视关键帧列表vpNeighsKFs中添加进来的。所以说添加边的关键帧至多不会超过vpOptimizableKFs和vpNeighsKFs的总和。比如有个地图点它被这两个列表之外的关键帧观测到了。由于前面说了添不添加边一个前提条件就是观测关键帧的mnBALocalForKF和mnBAFixedForKF等不等于输入关键帧的ID。而这两个属性又是在构造节点时修改的。所以对于没有经过处理的关键帧，其这两个属性肯定不和输入帧ID相等。因此，这个关键帧就不会被添加边。

双目地图点观测边

如下图所示，双目地图点观测边构建流程如下。这个操作同样是在遍历lLocalMapPoints构建地图节点之后做的。 构建了一个二元边。一边连接当前地图节点，另一边连接关键帧。单目边的观测就是一个三维向量。这里有个需要注意的点，可以看到无论是单目边还是双目边，添加过程都是遵循“从地图点到关键帧”这个方向(虽然从结果上来说没什么区别，因为构造的图本来也就是个无向图，但是这种思想还是要有的)。

惯性边

如下图所示，遍历vpOptimizableKFs构建惯性边。 可以看到，我们首先根据索引获得了某个关键帧以及它上一个关键帧有关的8个节点：分别是某个关键帧的位姿节点VP2、速度节点VV2、陀螺仪偏置节点VG2、加速度计偏置节点VA2，输入关键帧上一关键帧的位姿节点VP1、速度节点VV1、陀螺仪偏置节点VG1、加速度计偏置节点VA1。如果这8个顶点有任何一个有问题，就不会添加这条边。

如果一切顺利，我们会根据当前迭代的关键帧的IMU预积分结果mpImuPreintegrated新建一个惯性边，然后设置它的顶点。它是个多元边，它连接了6个顶点，包括：上一关键帧的位姿节点VP1、速度节点VV1、陀螺仪偏置节点VG1、加速度计偏置节点VA1、当前关键帧的位姿节点VP2、速度节点VV2。这里有个很重要的点：我们这里建立的惯性边连接了当前关键帧和上一关键帧。这是在ORB-SLAM3里特有的。因为在前面介绍纯视觉LocalBundleAdjustment()函数的博客里也说了在ORB-SLAM2中各个关键帧之间是没有边(约束)进行连接的。但在ORB-SLAM3中，通过IMU“弥补”了这种时间上的“割裂”。最后将边放到优化图中，同时我们也新建了个vector类型的vei存储这些边。

这里需要注意的是惯性边构造时的遍历和之前建立的前序关键帧节点和固定关键帧节点之间的关系。前面说了，对于vpOptimizableKFs而言，如果跟踪的地图点数量较多，那么bLarge就为true，vpOptimizableKFs最多包含25个关键帧。第一种情况是25帧满了，但是最后一个关键帧仍然有前序关键帧。这个前序关键帧会被添加到lFixedKeyFrames中。这时，我们建立的惯性边就把vpOptimizableKFs的最后一个关键帧和它的前序关键帧连起来了。第二种情况是25帧没满。如果出现了这种情况，只可能说明vpOptimizableKFs的最后一个关键帧一定是没有前序关键帧的(根据前面的逻辑，不然不会停止添加)。此时，vpOptimizableKFs的最后一个关键帧被作为固定帧添加到了lFixedKeyFrames中，变成了固定帧节点。此时开始遍历vpOptimizableKFs的时候，会依次添加惯性边，直到添加到倒数第二个元素，都是正常流程。这次迭代连接的倒数第二个元素和最后一个元素。而迭代到最后一个元素的时候，因为它没有前序关键帧了，所以直接跳过本次迭代，结束。

这里另外需要注意一下。前面说了，如果vpOptimizableKFs最后一个元素没有前序关键帧，那么这最后一个元素就会被放到lFixedKeyFrames中，并且将vpOptimizableKFs的最后一个元素删除。这样操作之后，vpOptimizableKFs最后一个元素也就变成有前序关键帧的了，就是被放到lFixedKeyFrames的原本倒数第一的关键帧。这样就可以顺利地迭代vpOptimizableKFs构造关键帧之间的惯性边了。

陀螺仪偏置边

如下图所示，构建陀螺仪偏置边。 可以看到，陀螺仪偏置边连接了上一个关键帧的陀螺仪偏置节点VG1和当前迭代关键帧陀螺仪偏置节点VG2。这条边也在时间上连接了上一关键帧和当前迭代帧。这也是ORB-SLAM2中没有的。

加速度计偏置边

如下图所示，构建加速度计偏置边。 同理可以看到，加速度计偏置边同样连接了上一个关键帧的加速度计偏置节点VA1和当前迭代关键帧的加速度计偏置节点VA2。把离散的观测在时间上“关联”了起来。

至此，优化图的边就构建完成了。我们可以简单可视化示意图如下所示。 另外可以发现的是，对于G2O中构建的边，其作为一种约束，或者说是一种两个节点之间的变换关系，要么直接通过setMeasurement()函数设置观测，或通过setInformation()函数设置信息矩阵来表达两个节点之间的变换关系。二者至少有一个。

3.2.4 迭代优化

构建好优化图以后，下面就是进行迭代优化，如下图所示。 其实非常简单。这里的优化次数就是在根据传入的bLarge参数设置的。opt_it默认为10，如果bLarge为true，就为4。当然了，这里也有对是否停止这个flag的判断，如果为true，就停止优化。

优化完成以后，系统就会删除一些粗差观测，如下图所示。 如果某个观测边的误差大于阈值，就将把它记录下来，放到vToErase中。之后，遍历这个vector，调用KeyFrame的成员函数EraseMapPointMatch()删除地图点，调用MapPoint的成员函数EraseObservation()删除观测，如下。

最后，恢复优化后的关键帧位姿、地图点坐标。具体会分为三部分，如下。 对于时序关键帧，直接调用关键帧的成员函数SetPose()将更新优化后的位姿，并且将mnBALocalForKF属性设为0。如果有IMU的话，再调用成员函数SetVelocity()更新优化后的速度、SetNewBias()更新优化后的IMU偏置。

对于局部共视关键帧也是类似的，调用关键帧的成员函数SetPose()将更新优化后的位姿，并且将mnBALocalForKF属性设为0。

最后，对于地图点，调用MapPoint的成员函数SetWorldPos()更新位置坐标，并调用UpdateNormalAndDepth()进行地图点内部变量的更新。

3.3 输出是什么

从函数的字面意思上来说，函数一个返回值都没有。 但显然不是这样的。通过类的成员函数实现了对于变量的赋值。

3.3.1 位姿相关

列举如下。

  • (1) pKFi->SetPose(Tcw): 更新关键帧位姿，在函数内部修改了Rcw、tcw、Rwc等一系列变量
  • (2) pKFi->mnBALocalForKF: 设置关键帧的mnBALocalForKF属性为0(恢复默认值)
  • (3) pKFi->SetVelocity(): 设置关键帧的速度Vw
  • (4) pKFi->SetNewBias(): 设置关键帧的IMU偏置
3.3.2 地图点相关

列举如下。

  • (5) pMP->SetWorldPos(): 设置地图点的坐标mWorldPos
  • (6) pMP->UpdateNormalAndDepth(): 根据更新后的位置计算法向与深度

4.构造的优化图的对比

在上面，我们详细分析了PoseInertialOptimizationLastKeyFrame()、PoseInertialOptimizationLastFrame()、LocalInertialBA()中优化的相关内容。在这篇博客中，我们也详细分析了PoseOptimization()、LocalBundleAdjustment()的内容。这一部分我们就对这几个函数构造的优化图进行一个简单的比较。

4.1 纯视觉与视觉IMU融合单帧位姿优化图比较

我们比较PoseOptimization()、PoseInertialOptimizationLastKeyFrame()、PoseInertialOptimizationLastFrame()构造的优化图如下。 可以看到，PoseOptimization()构造的优化图是最简单的(莫名的有点像让人讨厌的新冠病毒)，图中绿色箭头表示双目边、蓝色箭头表示单目边。可以看到，无论是PoseInertialOptimizationLastKeyFrame()还是PoseInertialOptimizationLastFrame()都继承了PoseOptimization()优化图的基础构造，只是在上面增加了IMU的观测节点。而LastKeyFrame和LastFrame的区别在于，在LastKeyFrame中，上一帧的观测是固定的，而LastFrame中，上一帧的观测不是固定的，而且上一帧的观测之间还多了个多元边连接。

4.2 纯视觉与视觉IMU融合局部BA优化图比较

我们比较LocalBundleAdjustment()和LocalInertialBA()构造的优化图。

下图是根据LocalBundleAdjustment()函数中添加节点以及边的步骤可视化的一个示意动图。 图中基本反应了纯视觉局部BA的优化图构建过程。根据输入帧获得共视关键帧，根据关键帧列表找到所有局部地图点。再遍历局部地图点，根据观测反推相关关键帧，并加入优化图。

上图则表达了LocalInertialBA()构造的优化图。可以看到，相比于纯视觉BA优化图，最显著的区别就是增加了时序相邻关键帧的IMU观测。前面也说了，对于视觉IMU联合优化而言，寻找关键帧主要有两条主线，一个是共视、二是时序。另外这其实分别就是对应了视觉和IMU对当前关键帧进行的优化。也就是如上图中蓝色区域和绿色区域所示。通过IMU和视觉，同时对同一个位姿节点进行优化。这也就是视觉、IMU紧耦合最本质的提现(而不是有两个位姿节点，视觉优化一个，IMU优化一个)。

4.3 与论文中优化图的比较

我们也可以将我们画的图和ORB-SLAM3论文里的优化图比较一下，如下： 上图出自《Visual-Inertial Monocular SLAM With Map Reuse》的图2。 上图出自《ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM》的图2。 可以看到，整体是基本一致的。只是论文图中为了简洁，把陀螺仪、加速度计的偏置写成了一个节点b。

5.IMU相关类型

最后简单介绍一下ORB-SLAM3中IMU相关的自定义类型，主要在ImuTypes.h文件中。

5.1 IMU观测

如下图所示，在ORB-SLAM3中，IMU的观测通过自定义的Point类型来表示。 可以看到，这个类里包含了一个float三维向量a用于存放加速度计观测、一个float类型三维向量w用于存放陀螺仪观测，以及一个double类型的t用于存放观测时间戳。系统也重载了构造函数，可以很方便的进行初始化。

5.2 IMU偏置

如下图所示，在ORB-SLAM3中，IMU偏置用Bias类型表示。 bax、bay、baz分别是加速度计在x、y、z方向上的偏置，bwx、bwy、bwz分别是陀螺仪在x、y、z方向上的偏置。在ORB-SLAM3系统中，偏置通过Tracking::ComputeGyroBias()、Tracking::ComputeVelocitiesAccBias()计算。

5.3 IMU校正参数

如下图所示，在ORB-SLAM3中，对于IMU的校正参数用Calib类型表示。 何为校正参数，从成员变量就可以看出，主要是IMU的外参(和相机之间的相互变换关系Tcb、Tbc)以及IMU的噪声相关参数Cov、CovWalk。你可能会好奇这两个噪声相关参数是怎么得到的。别着急。我们到Tracking::ParseIMUParamFile()中，在这里，我们就新建了一个Calib类型对象mpImuCalib，如下。 Tbc是IMU外参、sf是IMU帧率的开方、Ng为陀螺仪噪声、Na为加速度计噪声、Ngw为陀螺仪游走噪声、Gaw为加速度计游走噪声。而这些参数在配置文件里都有的，比如EuRoC数据集的IMU参数如下。 那么将Calib构造函数的参数列表与上面的输入参数进行比对，就可以得到对应关系，而且可以看到，重载的这个构造函数其实直接把传进来的数据又传给了Set()函数，如下。 所以，我们不妨到Set()函数里看一下，如下。 可以清晰地看到，Cov和CovWalk就是根据陀螺仪和加速度计的噪声和游走噪声算出来的。至此便成功“破案”，回答了前面的问题。

5.4 IMU预积分

如下图所示，在ORB-SLAM3中，对于预积分的结果用Preintegrated类型表示。 可以看到，它包含非常多的成员变量和成员函数。由于篇幅关系，这里就不展开介绍如何进行IMU预积分了。之后如果有时间，会接着这里详细介绍一下预积分的流程以及IMU相关的基本知识。

5.5 其它问题

什么是IMU偏置，和校正参数有什么关系？

在ORB-SLAM3中，Frame类中有一个专门的IMU::Bias类型成员变量mImuBias来保存IMU的偏置，同时也有一个IMU::Calib类型的mImuCalib来保存IMU的参数，如下。 如上面的介绍，IMU::Bias中存储了陀螺仪和加速度计在x、y、z方向上的偏置，而IMU::Calib存储了IMU的外参(和相机之间的相互变换关系)以及IMU的噪声相关参数。这就是两者的差别。

当然你可能还是会问，所以到底什么是偏置？所谓偏置(或者说是零偏)，可以理解为一种缓慢变化(长时间)或不变(短时间)的系统性误差。当陀螺仪或加速度计静止不动的时候，理论上观测应该都为0。但实际上由于各种因素存在，肯定不为0。一段时间内这种观测和0之间的差异的均值，就可以认为是偏置(The bias of a rate gyro is the average output from the gyroscope when it is not undergoing any rotation)。举个例子，某段时间内，陀螺仪静止不动，但其观测的角速度分别为-1、3、2、2、1、-2、1、0、1，理论上观测应该为0，但实际上有误差。我们对这些观测求平均，就可以得到约为0.78。也就是说这个陀螺仪在这段时间内的偏置就为0.78。偏置会随着时间漂移，但和观测频率相比要低很多，所以在一段时间内可以看作是常量。

感兴趣的话可以参考这个网页。

什么是IMU游走噪声，和噪声又有什么关系？

首先我们需要明确，通常IMU有两类噪声：一类是剧烈波动的测量白噪声(高频)，另一类则是变化相对缓慢的偏置(低频)。测量噪声可以看做是一种测量过程中引入的外部噪声，而偏置则是由传感器内部机械、温度等各种物理因素变化而产生的传感器内部误差综合参数。

对于单次测量而言，其测量值主要就是高斯白噪声，你也可以理解为问题中默认的“噪声”。但IMU作为一种天然需要积分的传感器，单次测量值很多时候意义不大。必须要在先前观测的基础上不断积分，才能得到我们想要的结果。所以多次观测之间这种噪声的变化趋势就是我们需要关注的问题。最好的情况是既没有噪声，也不会随时间变化，这样我们直接拿IMU的观测进行积分就可以得到完全没有误差的结果。次一点的情况是有噪声，但是其不会随时间变化，这样我们先估计出这个噪声，然后在积分过程中消除它的影响，也可以得到不错的结果。最坏的情况是有噪声，而且噪声会随着时间而不断变化。这时候我们不得不尝试估计出这个噪声随时间变化的规律，然后在积分的过程中尝试弥补。

更具体来说，这种随时间而缓慢变化的噪声就是第一个问题提到的“偏置”。为了能够建模这个过程，根据过去的观测估计出未来一段时间内的偏置，随机游走噪声模型就很自然的被引入了。所以准确的讲，问题里的“IMU游走噪声”并不是一个噪声，而是一个用于描述噪声随时间变化的模型，这个被描述的对象就是IMU的偏置。另外，随机游走模型其实是一个离散模型，其连续模型被称为维纳过程(Wiener Process)，维纳模型是高斯白噪声的积分。对IMU噪声模型感兴趣可以参考这个网页。

6.G2OTypes

根据这篇博客中的介绍，G2O的使用需要自定义节点，事实也是如此。通过前面的分析可以看到，ORB-SLAM3中自定义了很多节点或者边的类型，比如VertexPose、VertexVelocity、VertexGyroBias、VertexAccBias、EdgeInertial等。这些类型都是在G2OTypes.h中构造的，如下图所示。 可以看到，首先定义了两个基本类ImuCamPose、InvDepthPoint。然后基于这个类和G2O中提供的模板类定义了VertexXXX类型。然后又基于VertexPose等定义了EdgeMono、EdgeMonoOnlyPose、EdgeStereo、EdgeStereoOnlyPose等边。我们可以整理出如下的继承关系。 如图所示是各种类之间的继承关系。

7.参考资料

  • [1] https://www.zhihu.com/question/268227830
  • [2] https://zhuanlan.zhihu.com/p/82210442
  • [3] https://blog.csdn.net/qq_27251141/article/details/98748872
  • [4] https://www.zhihu.com/question/352293998/answer/874374903
  • [5] https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model
  • [6] https://zhuanlan.zhihu.com/p/37843131

本文作者原创,未经许可不得转载,谢谢配合

返回顶部
鼓励一下