75、视觉惯性地图的高效对齐

day7

于 2025-09-16 15:36:10 发布

阅读量24

点赞数

CC 4.0 BY-SA版权

分类专栏：机器人前沿：从理论到实践文章标签：视觉惯性地图地图对齐 SLAM

本文链接：https://blog.youkuaiyun.com/day7/article/details/152024025

机器人前沿：从理论到实践专栏收录该内容

83 篇文章 ¥499.90

订阅专栏¥69.90

会员秒杀 ¥9.9 重磅福利

超级会员免费看

视觉惯性地图的高效对齐

1. 引言

在GPS信号缺失的区域创建精确的3D地图，在虚拟现实和增强现实等众多应用中都至关重要。相机和惯性测量单元（IMU）传感器组合在大多数移动设备中都很常见，非常适合此类场景。然而，它们的计算资源有限，限制了使用视觉和惯性测量实时创建地图的大小。

为了减少对预计算地图或云基础设施的依赖，近年来人们开始关注在设备上进行地图绘制。例如ORB - SLAM方法，前端实时跟踪相机的位置和方向，后端计算局部地图并处理闭环。为了限制对大地图进行优化的时间，有人将地图划分为多个重叠的子地图，每个子地图独立优化，然后通过迭代优化仅涉及地图间测量的变量来进行对齐。

但这些方法存在一些问题。新的闭环可能导致更新在整个系统中传播，随着状态大小的增加，会给地图优化带来延迟。早期的方法使用扩展卡尔曼滤波器（EKF）的变体来估计设备的位姿和周围特征的局部地图，但由于存储全局地图的协方差矩阵，内存需求与特征数量呈二次关系，不适合移动设备。其他方法只在探索阶段更新局部地图，推迟应用地图间的闭环校正，但这些更新可能会在所有地图中传播，延迟系统响应。

还有一些方法，如Cunningham等人使用平滑方法利用多设备设置中问题的稀疏结构，但新测量可能需要更新所有地图，导致显著延迟。另一些方法虽然每个地图独立优化，但最后地图合并步骤的成本与地图公共特征数量呈三次方关系，限制了移动设备可绘制区域的大小。

在这些方法中，新测量可能导致所有地图的估计更新，处理成本高。但实际上，地图的局部估计通常不受地图合并和联合优化过程的影响。因此，在已知准确地图和它们的公共特征对应关系的情况下，只需要计算地图之间的变换，就可以实现地图对齐。

2. 技术方法

考虑N个重力对齐的3D特征地图，已知特征对应关系，但地图之间的变换未知。定义要最小化的成本函数为：
[C = \sum_{i,j = 1}^{N}\sum_{f_m\in F_{ij}}\left\lVert ^1p_j - ^1p_i + ^1_jC_jf_m - ^1_iC_if_m\right\rVert^2_{\Omega_{ijm}}]
其中，(F_{ii})为空集，(\Omega_{ijm})是与每个成本项相关的协方差。

在成本函数(C)取最小值时，满足(\frac{\partial C}{\partial ^1p_k}=0)（(k = 2,\cdots,N)），得到：
[\sum_{i = 2}^{N}A_{ki}^1p_i-\sum_{i = 2}^{N}B_{ki}K^1u_i - ^1a_k = 0]
其中：
[A_{ki}\triangleq\begin{cases}-\sum_{f_m\in F_{ik}}\Omega_{ikm}^{-1}&k\neq i\\sum_{i = 1}^{N}\sum_{f_m\in F_{ik}}\Omega_{ikm}^{-1}&k = i\end{cases}]
[B_{ki}\triangleq\begin{cases}\sum_{f_m\in F_{ik}}\Omega_{ikm}^{-1}L(i f_m)&k\neq i\-\sum_{l = 1}^{N}\sum_{f_m\in F_{lk}}\Omega_{lkm}^{-1}L(k f_m)&k = i\end{cases}]
[^1u_i=\begin{bmatrix}\cos(^1\theta_i)\\sin(^1\theta_i)\end{bmatrix},K = \begin{bmatrix}e_1&e_2\end{bmatrix},L\begin{pmatrix}\begin{bmatrix}x\y\z\end{bmatrix}\end{pmatrix}=\begin{bmatrix}x&-y&0\y&x&0\0&0&z\end{bmatrix}]
[^1a_k=\sum_{f_m\in F_{1k}}\Omega_{1km}^{-1}^1f_m+\sum_{i = 2}^{N}B_{ki}e_3]

定义(A)、(B)和(a)为相应的矩阵和向量，可得到：
[^1p_k = ^1t_k+\sum_{i = 2}^{N}T_{ki}^1u_i]
其中(T_{ki})和(^1t_k)分别是(3\times2)矩阵和(3\times1)向量。

将上式代入成本函数(C)，得到仅关于方向的成本函数：
[C=\sum_{i,j = 1}^{N}\sum_{f_m\in F_{ij}}\left\lVert r_{ijm}+\sum_{k = 2}^{N}Q_{ijmk}^1u_k\right\rVert^2_{\Omega_{ijm}}]
进一步展开可得：
[C = C_0+\bar{v}^T\bar{u}+\bar{u}^T\bar{W}\bar{u}]
其中(\bar{u}=\begin{bmatrix}^1u_2^T&\cdots&^1u_N^T\end{bmatrix}^T)，(C_0=\sum_{i,j = 1}^{N}\sum_{f_m\in F_{ij}}\left\lVert r_{ijm}\right\rVert^2_{\Omega_{ijm}})，(\bar{v})和(\bar{W})按相应公式定义。

该式在约束条件(^1u_k^T^1u_k = 1)（(k = 2,\cdots,N)）下的最小化成本与地图数量呈指数关系。因此，采用一阶泰勒级数展开：
(\bar{u}\approx\hat{\bar{u}}+G\tilde{\psi})
其中(\tilde{\psi}=\begin{bmatrix}^1\tilde{\theta}_2&\cdots&^1\tilde{\theta}_N\end{bmatrix}^T)，(G)和(J)按相应公式定义。

代入后得到误差成本函数：
[C’ = C’_0+(\bar{v}+2\bar{W}\hat{\bar{u}})^TG\tilde{\psi}+\tilde{\psi}^TG^T\bar{W}G\tilde{\psi}]
为了最小化(C’)，令(\frac{\partial C’}{\partial\tilde{\psi}} = 0)，得到：
[G^T\bar{W}G\tilde{\psi}=-G^T\left(\bar{W}\hat{\bar{u}}+\frac{1}{2}\bar{v}\right)]
求解(\tilde{\psi})，更新地图方向估计(^1\theta_i^{\oplus}=^1\theta_i^{\ominus}+^1\tilde{\theta}_i)（(i = 2,\cdots,N)），再根据前面的公式计算地图位置，然后开始新的迭代，直到收敛（(\frac{\left\lVert\tilde{\psi}\right\rVert}{N - 1}\leq10^{-5})）。

在特殊情况下，当所有协方差都等于同一个标量乘以单位矩阵（即(\Omega_{ijm}=\sigma^2I_3)）时，成本函数简化为：
[C’=\sum_{i,j = 1}^{N}\sum_{f_m\in F_{ij}}\left\lVert ^1p_j - ^1p_i + ^1_jC_jf_m - ^1_iC_if_m\right\rVert^2]
经过一系列推导得到：
[^1p_k=\sum_{l = 2}^{N}\gamma_{kl}^1a’ l-\sum {i,l = 2}^{N}\gamma_{kl}^1_iC_ib’ l]
进一步简化为：
[^1p_k = ^1t’_k+\sum {i = 2}^{N}^1_iC_it’ k]
将其代入成本函数并进行一些处理后，得到修改后的成本函数：
[C’‘=\sum {i,j = 1}^{N}\sum_{f_m\in F_{ij}}\left\lVert\bar{r} {ijm}+\sum {l = 2}^{N}^1_lR_l\bar{q}_{ijm}\right\rVert^2]
可写成：
[C’’ = C’‘_0+g^T\bar{u}+\bar{u}^T\bar{H}\bar{u}]
同样采用迭代方法，线性化(\bar{u})，得到：
[G^T\bar{H}G\tilde{\psi}=-G^T\left(\bar{H}\hat{\bar{u}}+\frac{1}{2}g\right)]
求解(\tilde{\psi})，更新方向估计，计算位置，迭代直到收敛。

3. 实验结果

实验使用两台Google Pixel手机在线收集和处理视觉和惯性数据。灰度图像分辨率为640×480，以30Hz保存，IMU数据以200Hz保存。数据集分为两类：有VICON地面真值的房间规模数据集和使用最优批量最小二乘（BLS）估计作为地面真值的建筑规模数据集。

数据处理流程如下：
1. 视觉和惯性数据首先通过平方根逆滤波器处理，该滤波器处理IMU数据和从图像中提取的特征轨迹。特征轨迹通过提取FAST角点并匹配其ORB描述符来生成。
2. 使用词袋方法和3pt + 1 - RANSAC进行闭环检测和异常值检测。
3. 滤波器处理的数据，包括特征轨迹、闭环、IMU测量和滤波器的状态估计，传递给BLS过程。
4. 通过将数据集按时间分割成多个等长的小地图来创建多个地图。
5. 计算每个地图的BLS估计，存储IMU - 相机的轨迹和特征位置及其协方差(P_{im})。
6. 通过2pt + 1 - RANSAC确认地图公共特征对中的内点。

为了计算相对偏航角的初始估计，构建一个图，节点表示地图，边的权重等于地图之间的公共特征数量，然后计算该图的最大生成树（MST）。对于每个地图，从其节点遍历MST到根节点（即地图1），仅使用路径中的相对变换计算相对偏航角。

根据特征误差状态的假设，定义成本项的噪声(n_{ijm}=^1p_j - ^1p_i + ^1_jC_jf_m - ^1_iC_if_m)，其协方差(\Omega_{ijm}=^1_iCP_{im}^1_iC^T+^1_jCP_{jm}^1_jC^T)。

使用五个数据集评估方法的准确性和效率，结果如下表所示：
|数据集|WC - RMSE (cm)|WOC RMSE (cm)|GN - RMSE (cm)|WC - time (ms)|WOC - time (ms)|GN - time (ms)|Num. maps|Max common features|Dataset length (m)|BLS time (sec)|Ind. time (sec)|
| ---- | ---- | ---- | ---- | ---- | ---- | ---- | ---- | ---- | ---- | ---- | ---- |
|L1|30.1|38.1|30.1|371.8|201.5|515.2|21|276|752|–|6.59|
|L2|28.3|46.4|28.5|206.5|133.9|289.1|15|243|323|209|3.66|
|S1|13.0|14.6|13.1|0.7|0.6|1.2|5|18|112|82|25.5|
|S2|5.0|5.4|5.2|1.8|0.9|3.2|5|70|104|123|34.7|
|S3|8.0|10.0|7.7|1.2|0.7|1.9|5|28|104|102|18.9|

从结果可以看出，该方法的处理时间与地图数量呈四次方关系，但与公共特征数量呈线性关系。当地图数量增加四倍时，计算变换的时间增加约三百倍，而公共特征数量的变化影响较小。此外，WC方法的时间比GN方法快10% - 60%。并且，该方法在估计地图的6自由度位姿方面通常比GN方法更准确，这是因为地图的重力对齐使得轨迹的滚动和俯仰在视觉惯性BLS中得到准确估计，重新估计可能导致过拟合和不准确的估计。不过，在某些情况下，如数据集S3，不同地图的滚动和俯仰存在小误差，会使4自由度估计的RMSE略高于6自由度估计。

下面是数据处理流程的mermaid流程图：

graph TD;
    A[收集视觉和惯性数据] --> B[平方根逆滤波器处理];
    B --> C[特征轨迹生成];
    C --> D[闭环检测和异常值检测];
    D --> E[数据传递给BLS过程];
    E --> F[创建多个地图];
    F --> G[计算BLS估计];
    G --> H[确认内点];
    H --> I[计算相对偏航角初始估计];
    I --> J[评估公共特征约束协方差];
    J --> K[地图对齐和评估];

视觉惯性地图的高效对齐

4. 关键技术点分析

成本函数的构建与优化
- 成本函数 (C) 是整个算法的核心，它通过最小化不同地图间公共特征的距离来实现地图对齐。在实际应用中，这个函数综合考虑了地图的位置、方向以及特征的协方差，能够更准确地反映地图之间的匹配程度。
- 对成本函数进行优化时，通过求偏导数得到一系列方程，进而求解地图的位置和方向。在特殊情况下，当协方差为特定形式时，成本函数可以进一步简化，降低计算复杂度。
迭代优化过程
- 采用迭代的方式更新地图的方向和位置估计，直到满足收敛条件。这种方法能够逐步逼近最优解，提高地图对齐的准确性。
- 在每次迭代中，根据当前的估计值计算误差成本函数，然后求解误差向量 (\tilde{\psi})，并更新地图的方向和位置。
特殊情况处理
- 当所有协方差都等于同一个标量乘以单位矩阵时，算法可以进行简化。通过一系列推导，得到简化后的成本函数和计算方法，减少了计算量，但可能会牺牲一定的准确性。

5. 方法优势总结

计算效率高
- 算法的处理时间与地图数量呈四次方关系，与公共特征数量呈线性关系。相比其他方法，在处理多个地图时，能够显著减少计算时间。
- 例如，当地图数量增加四倍时，计算变换的时间仅增加约三百倍，而公共特征数量的变化对计算时间影响较小。
准确性高
- 考虑了特征估计的不确定性，通过引入协方差矩阵，提高了地图对齐的准确性。
- 实验结果表明，该方法在估计地图的6自由度位姿方面通常比其他方法更准确，尤其是在地图重力对齐的情况下。
可扩展性强
- 算法可以根据不同的应用场景进行调整，例如在协方差为特定形式时进行简化处理，以适应不同的计算资源和精度要求。

6. 应用场景与展望

应用场景
- 虚拟现实和增强现实 ：在GPS信号缺失的室内环境中，创建精确的3D地图，为用户提供更真实的虚拟体验。
- 机器人导航 ：帮助机器人在复杂环境中进行定位和地图构建，提高导航的准确性和效率。
- 智能物流 ：在仓库等环境中，实现货物的精准定位和路径规划。
未来展望
- 进一步优化算法 ：降低算法的计算复杂度，提高处理速度和准确性，以适应更复杂的应用场景。
- 融合更多传感器数据 ：结合激光雷达、深度相机等其他传感器的数据，提高地图的精度和完整性。
- 拓展应用领域 ：将该方法应用于更多领域，如自动驾驶、无人机测绘等。

7. 总结

本文介绍了一种用于调整多个重力对齐地图的高效方法。通过构建成本函数并进行迭代优化，能够准确计算地图之间的变换，实现地图的对齐。实验结果表明，该方法在计算效率和准确性方面都具有明显优势，适用于多种应用场景。未来，可以进一步优化算法，融合更多传感器数据，拓展应用领域，为相关领域的发展提供有力支持。

下面是方法优势的对比表格：
|优势|描述|
| ---- | ---- |
|计算效率高|处理时间与地图数量和公共特征数量的关系合理，能显著减少计算时间|
|准确性高|考虑特征估计的不确定性，提高地图对齐的准确性|
|可扩展性强|可根据不同应用场景进行调整，适应不同计算资源和精度要求|

下面是应用场景的mermaid流程图：

graph LR;
    A[虚拟现实和增强现实] --> B[创建精确3D地图];
    C[机器人导航] --> D[定位和地图构建];
    E[智能物流] --> F[货物精准定位和路径规划];
    G[自动驾驶] --> H[环境感知和路径规划];
    I[无人机测绘] --> J[地形测绘和数据采集];