GeoFlow-SLAM论文导读与算法解析

Mhenwa

557人浏览 · 2026-04-10 11:35:35

Mhenwa · 2026-04-10 11:35:35 发布

前言

GeoFlow-SLAM目标是足式机器人（Legged Robotics），利用紧耦合RGBD、IMU和腿部里程计（Legged Odometry，Legged Encoders），在动态场景下的追踪重建

着重解决高频剧烈运动带来的图像模糊、足端触地冲击导致的IMU数据噪声，以及常见的弱纹理环境（如白墙、长走廊）

开源地址：https://github.com/HorizonRobotics/GeoFlowSlam

请添加图片描述

GeoFlow-SLAM

主要工作

A dual-stream optical flow tracking incorporating map points and pose information，双流光流跟踪
A pose initialization method based on IMU/Legged Odometry, Perspective-n-Point (PnP), and Generalized Iterative Closest Point (GICP)，位姿初始化基于IMU/步态里程计、PnP 与 GICP
A novel optimization framework that tightly integrates depth-to-map and GICP geometric consistency，深度和 GICP 紧耦合
collected datasets using legged robotics，采集了双足机器人数据集

整体架构

在这里插入图片描述
GeoFlow-SLAM 系统由三部分构成：

特征提取与跟踪（Feature Extraction）
IMU/腿式/PnP/GICP 里程计（Odometry）
视觉-深度-IMU 融合建图（Visual-Depth-IMU Integration Mapping）

首先，对RGBD输入提取深度特征与 FAST、ORB 特征，对IMU 和腿部编码器的数据分别进行 IMU预积分、腿部预积分。

随后，IMU预积分、腿部预积分结果进入IMU/腿部里程计确定初始位姿，深度特征进行GICP获得初始位姿估计、ORB特征的进行PnP

接着，采用双流光流并结合上面的结果信息对 ORB 特征进行跟踪。计算深度、重投影以及回环检测残差

最后，在紧耦合的视觉-深度-IMU 因子图中进行单帧、局部关键帧和全局关键帧优化，实现地图构建与位姿估计。

特征提取与跟踪

从深度图中提取深度平面特征，在 GICP 算法中用于获得初始位姿估计；从RGB图像中提取 Oriented FAST 与 Rotated BRIEF(ORB)特征。

采用双流光流与 PnP进行特征跟踪。

IMU 与腿式编码器进行预积分

IMU/腿式/PnP/GICP 里程计

优先用腿式与 IMU 里程计进行位姿预测；否则通过在连续帧间求解 PnP 问题来确定初始位姿

来自 IMU/腿式或 PnP 的初始位姿随后用 GICP 算法进一步精化

采用深度到地图的配准与视觉地图点的重投影（the depth-to-map registration and the reprojection of visual map points）来生成观测残差

视觉-深度-IMU 融合建图

该模块包括单帧优化、局部关键帧优化和全局关键帧优化。

单帧位姿优化与局部BA模块：视觉重投影残差、深度到地图残差以及 IMU/腿部/ GICP 位姿约束紧耦合。

单帧位姿优化仅关注当前相邻帧位姿估计；局部 BA 估计局部关键帧的位姿及相应的视觉地图点

当检测到回环时，对整体关键帧位姿和点云地图进行全局优化

双流光流跟踪

传统光流通常假设运动小且连续。在快速运动时收敛缓慢且计算量大。

为了解决这些问题，提出了利用先验地图点(ORB 特征的对应地图点)和位姿来增强快速运动下的特征跟踪鲁棒性的双流光流跟踪方法

在这里插入图片描述

PipeLine：

对于有效的地图点（绿色的⭐），利用先验的地图点+位姿重投影到当前帧，生成重投影点（🔵）
基于当前帧的重投影点做光流微调（🔵->虚线🟢），即先用几何预测位置，再用光流 refine（3D→2D），以处理 大位移
对上一帧中缺乏合格先验地图点（黄色⭐）的特征点(表示为🟡和🔴)进一步施加普通光流（2D → 2D）

事实上，在3D->2D这一步虽然有先验约束得到更优点位，但这样一个从更优点位开始的普通的光流，仍然没有直接解决模糊的问题，如果普通光流无法解决模糊，为何换个位置的普通光流就可以解决？有些缺乏说服力

3D 到 2D 的光流跟踪

第一路：使用来自里程计模块的 IMU/腿部里程计或 PnP、GICP 提供的初始位姿，将特征点的对应地图点 $P(X,Y,Z)\mathbf{P}\left( {X, Y, Z}\right)$ 投影到当前帧 $p′(x,y){p}^{\prime }\left( {x, y}\right)$ 上的特征点 $p(x,y)p\left( {x, y}\right)$ 。

${p}^{\prime }\left( {x, y}\right) = F\left( {{\mathbf{T}}_{w}^{{c}_{k - 1}}\mathbf{P}}\right)$

$F\left( \mathbf{P}\right) = \left( \begin{array}{l} {f}_{X}\frac{X}{Z} + {c}_{x} \\ {f}_{Y}\frac{Y}{Z} + {c}_{y} \end{array}\right)$
$F(P)F\left( \mathbf{P}\right)$ 表示内参投影； $Twck−1{\mathbf{T}}_{w}^{{c}_{k - 1}}$ 表示时刻 $k - 1$ 的相机位姿。

然后在当前帧上对投影像素 $p′(x,y){p}^{\prime }\left( {x, y}\right)$ 进行 3D 到 2D 的光流跟踪。该方法天然滤除了不稳定特征，如动态不一致的特征。

2D 到 2D 的光流跟踪

不准确的初始位姿和三角化失败，导致ORB 特征常常缺乏有效对应的地图点

为此引入了第二路，在原始特征点上使用 2D 到 2D 的光流跟踪。

还计算了基础矩阵以识别并剔除外点，并配合内点掩码机制，防止在同一区域内不必要地包含跟踪点。

在这里插入图片描述

左图为 ORBSLAM3 ，右图为GeoFlow。绿色点表示特征点，红色点表示重投影到图像上的点。

Legged、PnP 与 GICP 里程计

无纹理、多足机器人的运动模式干扰 IMU ，导致错误估计初始位姿，此处提出了一种融合多足里程计、PnP 与 GICP 的初始位姿估计方法。

若多足里程计数据可用，则优先用于位姿预测
当 IMU 可用且在短时段内精度较高且计算较简单时，我们优先使用 IMU 进行初始位姿预测
不满足IMU 使用条件时，基于成功跟踪的特征点计算本质矩阵以恢复帧间位姿，该方法不提供尺度信息，因此进一步基于深度进行 PnP 以获得更准确的初始位姿
- 若 PnP 失败，则再用 GICP 细化初始位姿。
若上述方法均失败，则基于匀速模型假设估计初始位姿。

非常工程化的方案

GICP 计算中使用 small_gicp，同时，将基于 GICP 的帧间相对位姿约束纳入因子图优化框架以进一步提升精度。

在实际代码中，我们可以找到实现细节，tracking.cc，TrackWithMotionModelICP ：

  ORBmatcher matcher(0.9, true);

  // Update last frame pose according to its reference keyframe
  // Create "visual odometry" points if in Localization Mode
  UpdateLastFrame();
  bool bICP = false;
  int bEnableOdom = mPSettings->enableRobotOdom() && !mlQueueOdomData.empty();
  // 给ICP提供一个初始位姿
  Eigen::Vector3f dpos =
      mpImuPreintegratedFromLastKF->GetUpdatedDeltaPosition();
  if (mpAtlas->isImuInitialized() &&
      (mCurrentFrame.mnId > mnLastRelocFrameId + mnFramesToResetIMU)) {
    PredictStateIMU();
  } else {
    if (!EstimatePoseByOF()) {
      mCurrentFrame.SetPose(mVelocity * mLastFrame.GetPose());
    }
  }

  if (bEnableOdom) {
    PredictStateOdom();
  }
  string icp_method = mPSettings->getICPMethod();
  if (icp_method == "GICP")
    bICP = PredictStateICP();
  else if (icp_method == "NDT")
    bICP = PredictStateNDT();
  else
    bICP = PredictStateICP();
  fill(mCurrentFrame.mvpMapPoints.begin(), mCurrentFrame.mvpMapPoints.end(),
       static_cast<MapPoint*>(NULL));
  int th;

如果 IMU 已初始化，走 PredictStateIMU。
否则先走 EstimatePoseByOF，也就是 LK 光流 + PnP 得到粗位姿。
如果有腿部 odom，再走 PredictStateOdom。
如果开了 ICP/NDT，再走 PredictStateICP/NDT 用深度点云把粗 pose 再修一遍。

这一步的输出是“当前帧一个可用的初始位姿”。

视觉、深度与 IMU 的紧耦合

构建如下因子图以在局部窗口内优化关键帧位姿和视觉特征点，包括：

视觉重投影残差因子（visual reprojection residual factors）
深度到地图的点-面残差因子（depth-to-map point-to-plane residual factors）
GICP 相对位姿残差因子（GICP relative pose residual factors）
IMU 预积分因子（IMU pre-integration factors）

关键帧的深度平面特征点被拼接成局部地图。

对于当前关键帧的每个平面特征点，在将点投影到局部地图后对其 k 个最近点云进行平面拟合，得到点-面残差

随后使用 GICP 约束帧间位姿

若 IMU 预积分与多足里程计或匀速假设显著偏离(表明 IMU 积分发散)，我们会自适应地引入多足里程计约束以改善位姿估计。

在这里插入图片描述
之后，利用优化得到的相机位姿重建更精确的局部深度图

为保证系统实时性，仅在有效视觉观测不足时才将深度到地图约束（depth-to-map constraints）添加到帧节点

在单帧优化中，遵循 ORB-SLAM，通过在相邻帧之间构建约束来进行优化，包括 IMU 预积分、GICP 相对位姿、视觉重投影和深度到地图约束。

${r}_{1} = \mathop{\min }\limits_{X}\mathop{\sum }\limits_{{\left( {i, j}\right) \in F}}\rho \left( {\begin{Vmatrix}{r}_{v}\left( {p}^{{c}_{i}}, X\right) \end{Vmatrix}}_{{\sum }_{v}}^{2}\right)$

${r}_{2} = \mathop{\min }\limits_{X}\mathop{\sum }\limits_{{i \in F}}\rho \left( {\begin{Vmatrix}{r}_{imu}\left( {z}_{{b}_{i}{b}_{i + 1}}, X\right) \end{Vmatrix}}_{{\sum }_{{b}_{i}{b}_{i} + 1}}^{2}\right)$

${r}_{3} = \mathop{\min }\limits_{X}\mathop{\sum }\limits_{{\left( {i, j}\right) \in F}}\rho \left( {\begin{Vmatrix}{r}_{icp}\left( {I}_{{c}_{j}}^{{c}_{i}}, X\right) \end{Vmatrix}}_{{\sum }_{icp}}^{2}\right)$

${r}_{4} = \mathop{\min }\limits_{X}\mathop{\sum }\limits_{{\left( {i, j}\right) \in F}}\rho \left( {\begin{Vmatrix}{r}_{l}\left( {d}^{{c}_{i}}, X\right) \end{Vmatrix}}_{{\sum }_{l}}^{2}\right)$

${r}_{1},{r}_{2}$ 表示视觉重投影与 IMU 预积分残差因子；
${r}_{3},{r}_{4}$ 分别指代 GICP 相对位姿和深度到地图的点到平面残差
${p}^{{c}_{i}}$ 表示对第 $i$ 帧的视觉观测
${z}_{{b}_{i}{b}_{i + 1}}$ 表示预积分观测
${I}_{{c}_{j}}^{{c}_{i}}$ 表示第 $i$ 帧与第 $j$ 帧之间的 GICP 相对位姿
${d}^{{c}_{i}}$ 定义为第 $i$ 帧的深度到地图观测
$ρ\rho$ 是一个用于增强优化步骤稳定性并减轻异常值影响的核心函数
在局部 BA 中， $X$ 表示关键帧的 IMU 状态 $x$ 与视觉地图点 $P$
在单帧优化中， $X$ 仅表示相邻帧的 IMU 状态。

残差	来源	作用
$r_1$	图像	约束像素一致性
$r_2$	IMU	提供运动先验
$r_3$	GICP	提供点云几何位姿
$r_4$	深度	提供结构约束

从深度图中提取特征

从点云中提取平面特征点以节省计算资源

首先，通过计算每个点的垂直角 $αi{\alpha }_{i}$ 与水平角 $θi{\theta }_{i}$ 将点云分割为二维矩阵 $pi=(xi,yi,zi,){p}_{i} = \left( {{x}_{i},{y}_{i},{z}_{i},}\right)$ 。

${\alpha }_{i} = \arctan \left( \frac{{y}_{i}}{{x}_{i}}\right)$

${\theta }_{i} = \arctan \left( \frac{{z}_{i}}{{x}_{i}}\right)$
随后，利用这些角度对检测范围进行等分，将点云划分为 $\times N$ 个单元

在每个单元内，计算所有点的几何中心并定义局部平滑度以提取平面特征，较低的平滑度值意味着平面特征平坦

深度到地图（Depth-to-map）残差

深度残差通过将采样的平面点云匹配到最近的子地图生成。局部建图线程识别出与当前帧在时空上达到指定接近度的关键帧。随后，利用这些关键帧的位姿及其对应的平面特征点构建子地图。

来自平面点到局部地图的点到平面残差 ${r}_{l}$ 构建为:

${r}_{l}\left( {{d}^{{c}_{i}}, X}\right) = {\left( {n}^{w}\right) }^{T}\left( {{P}^{\prime } - {q}_{\text{ plane }}^{w}}\right) = {JX} + \omega$

误差 = 法向量 ⋅ ( 点 − 平面上一点 )，乘法向量将距离投影到固定方向
= 一阶线性化 + 噪声
点到平面的距离作为误差

$\frac{\partial {r}_{l}}{\partial X} = \frac{\partial {r}_{l}}{\partial {P}^{\prime }}\frac{\partial {P}^{\prime }}{\partial X} = {\left( {n}^{w}\right) }^{T}\left\lbrack {I, - {P}^{\prime \land }}\right\rbrack$

残差对“位姿”的导数
${P}^{\prime } = {T}_{c}^{w}{p}_{\text{ plane }}^{c}$
变换到世界坐标系

$nw{\mathbf{n}}^{w}$ 表示世界坐标系中平面的法向量
- $w{q}_{\text{ plane }}^{w}$ 表示世界坐标系中平面的中心点
$c{p}_{\text{ plane }}^{c}$ 表示相机坐标系中的平面特征点
${T}_{c}^{w}$ 表示从相机坐标系到世界坐标系的位姿变换矩阵
$ω\omega$ 是点到平面的观测噪声。

实验

对比实验

在宇树的Go2 四足机器人与 G1 人形机器人上进行了验证

在这里插入图片描述
六个测试序列，其中机器人狗四个，人形机器人两个：

在这里插入图片描述

Seq1 室内办公室
Seq2 无纹理走廊环境
Seq3 地下停车场，光线较暗且有反光。
Seq4 广场，地面纹理重复
Seq5 室内办公室
Seq6 室外

Seq1-4 在 Go2 机器狗上，Seq5-6 在 G1 人形机器人上

与 VINS 系列(VINS-RGBD(RGBD+IMU)和 S-VIO(RGBD+IMU))及 ORB-SLAM3 进行对比：

在这里插入图片描述

性能对比，在比ORB-SLAM3引入额外观测的前提下，每帧处理约 20-30ms

在这里插入图片描述

优势分析

性能归因于三大因素:

鲁棒位姿初始化：受腿式机器人运动特性的影响，IMU 数据常常存在误差。如图所示，Seq1-6 中的原始 IMU 数据严重畸变，四足机器人最大加速度达 $m/s2{40}\mathrm{\;m}/{\mathrm{s}}^{2}$ ，人形机器人为 $m/s2{20}\mathrm{\;m}/{\mathrm{s}}^{2}$ 。

在这里插入图片描述

双流光流跟踪：腿式机器人的独特运动特性常导致图像模糊，严重影响特征跟踪的鲁棒性。但双流光流方法利用先验地图点与位姿信息，即使当前图像模糊也能有效跟踪特征
紧耦合深度约束：在无纹理环境中，视觉特征点稀少，位姿估计困难。通过耦合深度到地图和 GICP 约束，提升了精度与鲁棒性

TABLE III. EVALUATION RESULT ON OPENLOROS DATASET. THE BEST RESULT IS HIGHLIGHTED IN BOLD AND SYMBOL \ INDICATES FAILURE.

	VINS-Mono	VINS-RGBD	ORBSLAM3	S-VIO	VID-SLAM	DDIO-VIO	GeoFlow-SLAM
Home1-1	1.0760	0.4520	0.9290	0.4020	0.3820	0.4060	0.4575
Home1-2	0.5420	0.3790	\	0.3160	0.3620	0.3470	0.3775
Office1-1	0.2310	0.1010	0.0680	0.1010	0.0670	0.0780	0.0577
Office1-2	0.2890	0.1240	0.1230	0.0990	0.1080	0.0700	0.0700
Office1-3	0.1550	0.1540	\	0.1510	0.0220	0.1040	0.0291
Office1-4	0.3920	0.1860	0.2530	0.1640	0.1290	0.1440	0.1554
Office1-5	0.2380	0.2390	\	0.2120	0.2090	0.2140	0.2000