2025.2.27SLAM学习笔记_2025视觉特征点slam-优快云博客

本文链接：https://blog.youkuaiyun.com/2301_76866200/article/details/145908667

一、粗配准

1.原因：
在SLAM中，连续帧之间的相对运动是常见的。通过上一帧和当前帧之间的变换关系（即均匀运动模型），我们可以预测出下一帧的大致位姿。粗配准阶段的目的是快速获取下一帧的初步估计，这有助于减少匹配时的搜索空间并为后续精细优化提供起始点。

2.原因：
将特征点的坐标从像素坐标转换为归一化坐标有助于去除相机内参的影响，使得特征点在不同的帧中具有统一的尺度。这是为了确保在不同视角下的特征点能够进行一致的处理，避免因相机内参的差异造成的误差。

3.原因：
将相机坐标系下的特征点转换为世界坐标系的坐标，可以将其与其他传感器（如IMU）的数据对齐，形成一个统一的空间参考系。这样做的目的是确保特征点在全球坐标系中具有正确的位置，从而为后续的定位和映射提供精确的空间参考。

4.原因：
将世界坐标系下的特征点转换回下一帧的相机坐标系，目的是根据估计的位姿将特征点放置到下一帧的相对位置。这一步使得我们能够将特征点的位置信息应用到下一帧中，从而实现特征点的预测。

5.原因：
将特征点从IMU坐标系转换回相机坐标系是为了进一步对特征点进行精确定位。这一步将IMU和相机的测量数据对齐，从而确保在后续的图像匹配中，特征点在相机坐标系中有一致的表示。

6.估计特征点的位置：最后，我们将特征点从相机坐标系转换回像素平面。通过这种方式，特征点在上一帧中的位置被预测到下一帧中，从而完成了粗配准过程。

6.原因：
通过这些转换步骤，能够得到特征点在下一帧的预测位置，这有助于加速特征点的匹配过程。粗配准阶段的目标是快速且准确地估计特征点的位置，以便为后续的精细配准提供一个良好的初始估计。

总结来说，粗配准阶段的目的是通过估算上一帧和当前帧之间的变换关系，结合相机内参和IMU信息，预测特征点在下一帧中的位置。这个过程为后续的精细配准奠定了基础。

二、Descriptor Matching（描述子匹配）

Descriptor Matching（描述子匹配）是什么？

Descriptor Matching（描述子匹配） 指的是 在两幅图像或两帧视频中，通过计算特征点描述子之间的相似度，找到相对应的匹配点对。

在 计算机视觉（如 SLAM、图像配准、目标识别）中，特征点（Feature Points）通常使用描述子（Descriptor）来表示，而 Descriptor Matching 就是寻找 两个图像中相似的特征点对，用于后续的 位姿估计、地图构建、目标跟踪等任务。

如何进行 Descriptor Matching？

匹配过程通常包括以下步骤：

提取特征点 & 计算描述子
- 在两幅图像中分别检测特征点（例如 ORB、SIFT、SURF 等）。
- 计算这些特征点的描述子（Descriptor），这些描述子是特征点的数值化表示，例如 128 维 SIFT 描述子或 256 维 ORB 描述子。
计算描述子之间的相似度
- 选定一个匹配度量方法，例如：
  - 欧几里得距离（L2 范数） → SIFT/SURF
  - 汉明距离（Hamming 距离） → ORB/BRIEF
- 计算两幅图像中每个特征点的描述子和所有可能匹配点之间的距离。
选择最佳匹配点对
- 最近邻匹配（Nearest Neighbor, NN）
  - 选择距离最近的点作为匹配点。
- 双向最近邻匹配（KNN + 交叉验证）
  - A 图的点匹配到 B 图的点，B 图的点也要匹配回 A 图，确保匹配的可靠性。
- 比值测试（Ratio Test, Lowe’s Ratio）
  - 取最近的两个匹配点，若最近匹配与次近匹配的距离比值低于设定阈值（如 0.7），才认为是有效匹配。

ORB_D 中的 Descriptor Matching

在 ORB_D 描述子（结合 ORB 和深度信息的混合描述子）中，匹配过程使用了 加权欧几里得距离 作为匹配准则：

Dist(diH,djH)=∑k=1N(dik−djk)2+λ(zi)+λ(zj)2(zi−zj)2\text{Dist} ( d^H_i, d^H_j ) = \sqrt{ \sum_{k=1}^{N} (d_{ik} - d_{jk})^2 + \frac{\lambda(z_i) + \lambda(z_j)}{2} (z_i - z_j)^2 }Dist(diH,djH)=k=1∑N(dik−djk)2+2λ(zi)+λ(zj)(zi−zj)2

其中：

di,djd_i, d_jdi,dj 是两个特征点的描述子（包括 ORB 灰度信息 & 深度法向量）。
(zi−zj)2(z_i - z_j)^2(zi−zj)2 是深度信息的约束，用来降低远距离匹配误差。
λ(z)\lambda(z)λ(z) 是深度权重调整函数，控制深度的影响程度。

这一方法结合了 灰度信息 + 深度信息 进行匹配，增强了在动态环境或光照变化下的匹配鲁棒性。

Descriptor Matching 的应用

视觉 SLAM（VSLAM）
- 在连续帧之间匹配特征点，计算相机运动轨迹（位姿估计）。
图像拼接（Image Stitching）
- 在不同视角的图像中匹配特征点，实现图像合成（如全景图）。
目标跟踪（Object Tracking）
- 在视频流中匹配目标的特征点，实现目标跟踪。
3D 重建（Structure from Motion, SfM）
- 通过多视角匹配特征点，估计场景的 3D 结构。

三、ORB_D descriptor

ORB_D descriptor 的设计目的是为了增强特征点匹配的鲁棒性，特别是在动态环境和深度信息不完全或受限的场景下。以下是每个步骤的具体原因和目的：

1. 提取灰度图像特征描述符

原因：ORB（Oriented FAST and Rotated BRIEF）算法是一种高效的特征点描述符，它能够在计算复杂度较低的情况下提供较好的特征表示。通过ORB算法提取每个特征点的描述符，可以高效地捕捉图像中的局部特征。
目的：为每个特征点生成一个描述符 did_idi，用于描述该点周围的局部图像信息。

2. 计算深度图像中的法向量

原因：在动态场景中，深度信息提供了额外的几何信息，使得我们可以对特征点进行更精确的描述。法向量能够反映特征点所在位置的局部几何形状，尤其在表面轮廓变化较大的地方，法向量可以提供有用的深度信息。
目的：通过计算法向量 nin_ini，将深度图像中的信息加入到特征描述中，使得描述符不仅考虑灰度强度，还考虑到深度和几何形态，增强了特征描述符的鲁棒性。

3. 构建混合描述符

原因：单纯依赖灰度信息的描述符在某些情况下，如快速运动、模糊或纹理不明显的区域，可能失效。而深度信息能够有效补充这些灰度图像无法提供的信息。因此，混合描述符（灰度+深度）能够更加全面地描述特征点，从而提高特征匹配的准确性。
目的：通过将ORB描述符与深度图像中的法向量结合，构建一个新的混合描述符 diHd_i^HdiH，不仅考虑了图像的强度信息，还整合了深度的几何信息，提供更强的特征描述能力。

4. 描述符匹配

原因：在匹配特征点时，采用加权欧几里得距离来衡量两个混合描述符的相似度。使用加权距离是为了使深度信息的匹配权重根据深度值的可靠性进行动态调整，避免过远或不准确的深度数据对匹配过程的影响。
目的：通过加权欧几里得距离来确保描述符匹配的准确性，并通过深度加权来动态调整不同深度值对匹配的贡献，从而提高匹配的精度和鲁棒性。

5. 深度加权参数

原因：深度信息的可靠性随着距离的增加而下降，尤其是在深度图像的测量误差较大的远距离区域。因此，采用深度加权参数 λ(z)\lambda(z)λ(z) 来调整远距离的深度信息权重，保证近距离的深度数据对匹配的贡献更大。
目的：通过深度加权参数来调整深度信息的影响力，使得在近距离（较为准确的深度）和远距离（误差较大的深度）之间，能够合理分配匹配的权重，确保匹配过程更加稳定和精确。

6. 描述符加权计算

原因：深度值对特征匹配的影响并非简单的固定权重，而是需要根据每个特征点的深度信息进行适当加权，尤其是当两点深度相差较大时，深度信息的可靠性不同，因此匹配的权重也应不同。
目的：通过计算两个描述符之间的加权平均深度，确保在匹配过程中考虑到深度信息的实际可靠性，避免远距离和不准确的深度影响最终的匹配结果。

总结：

ORB_D descriptor 的设计结合了灰度图像和深度图像的信息，利用法向量和深度信息增强了特征描述符，使得其在动态和复杂环境下能够更好地匹配特征点。
加权欧几里得距离用于根据深度信息的可靠性对描述符进行匹配，提高了匹配的精度和鲁棒性。
深度加权参数确保了远距离深度值对匹配的影响较小，增强了近距离匹配的可靠性，从而优化了特征点的匹配过程。

通过这些方法，ORB_D descriptor 可以有效应对动态场景和深度信息不准确的挑战，提高视觉惯性导航系统中的特征匹配质量。

四、ORB_D Extract

ORB_D Extract 主要指的是从图像中提取混合描述符 ORB_D 的过程。ORB_D 描述符结合了灰度图像中的局部特征描述符（如通过 ORB 算法提取的描述符）和深度图像中的几何特征（例如，法向量），目的是增强特征点匹配的精度和鲁棒性。具体来说，ORB_D 提取的过程包括以下几个步骤：

1. 特征点提取（Keypoint Extraction）

ORB 提取：首先，在灰度图像中使用 ORB 算法提取关键点。ORB（Oriented FAST and Rotated BRIEF）是一种常用的特征点检测和描述算法，能够有效提取图像中的关键特征点并为其计算描述符。
目标：通过 ORB 提取的关键点可以有效表示图像中的局部特征，为后续的匹配和追踪提供基础。

2. 计算灰度描述符（Gray Descriptor Calculation）

ORB 描述符：对于每一个提取出的特征点，使用 ORB 算法计算其灰度图像中的描述符。这个描述符是一个向量，能够捕捉该点周围图像的局部信息，例如纹理和强度变化。
目标：为每个特征点提供一个用于描述该点周围图像信息的描述符。

3. 计算法向量（Normal Vector Calculation）

深度图像中的法向量：除了灰度图像，ORB_D 还结合了深度图像中的信息。对深度图像中的每个特征点，计算其法向量。法向量是通过计算该点周围深度图的梯度来得到的，可以有效表示该点在空间中的几何形状。
目标：法向量提供了深度信息的几何描述，增强了特征描述符的表达能力，使其能够同时利用灰度图和深度图的信息。

4. 构建混合描述符（Hybrid Descriptor Construction）

合成描述符：将灰度图像中提取的 ORB 描述符和深度图像中计算的法向量信息结合，构成一个新的描述符 ORB_D。这个混合描述符的长度为 N+3，其中 N 是灰度描述符的长度，3 代表法向量的三个分量（nx,ny,nzn_x, n_y, n_znx,ny,nz）。
目标：ORB_D 描述符通过结合灰度图像和深度图像的特征，能够提供更丰富的信息，从而提高特征匹配的鲁棒性和准确性。

5. 描述符匹配（Descriptor Matching）

匹配方案：提取并构建了混合描述符之后，需要使用合适的匹配方法对两帧之间的描述符进行匹配。通常，使用加权的欧几里得距离来度量两个描述符之间的相似度。
目标：通过匹配不同帧中的描述符，找到相似的特征点并进行跟踪，从而实现视觉-惯性SLAM系统中的定位和建图。

6. 最终的描述符输出（Final Descriptor Output）

结果：ORB_D 描述符作为最终的特征描述符输出，并用于后续的特征点匹配、姿态估计或地图构建等任务。
目标：通过 ORB_D 描述符，可以提高系统在动态环境和复杂场景中的特征匹配精度，减轻模糊、弱纹理和深度信息不足等问题的影响。

五、Weight Adjustment（权重调整）

Weight Adjustment（权重调整） 指的是在计算某些度量（如匹配误差、优化目标函数等）时，根据不同因素对各个项赋予不同的重要性。

在 ORB_D 描述符匹配 过程中，Weight Adjustment 主要体现在 加权欧几里得距离 计算中。具体来说，它用于调整 深度信息 在特征匹配中的影响程度。

在 ORB_D 描述符匹配中的权重调整

在计算两个描述符之间的距离时，使用了如下公式：

其中：

diH,djHd^H_i, d^H_jdiH,djH 是两个混合描述符。
(dik−djk)2(d_{ik} - d_{jk})^2(dik−djk)2 计算了两个描述符在灰度信息上的差异。
(zi−zj)2(z_i - z_j)^2(zi−zj)2 计算了深度信息的差异。
λ(z)\lambda(z)λ(z) 是 深度信息的权重调整因子，用于平衡灰度描述符和深度信息对最终匹配结果的贡献。

为什么需要权重调整？

深度信息的可靠性随距离变化
- 在 近距离（0.4m-6m） 内，深度数据较为精准，因此可以赋予一个固定较高的权重（例如 0.5）。
- 在 远距离（6m-10m） 处，深度数据误差较大，因此需要 使用对数函数动态调整权重，使远距离的深度信息权重随距离增加而减小，以减少误差的影响。
匹配的准确性
- 如果不进行权重调整，深度信息在远距离处的误差可能会对匹配过程造成较大干扰，导致错误匹配。
- 通过动态调整深度信息的权重，可以在匹配过程中更可靠地融合灰度和深度信息，提高匹配的准确性。

深度信息权重的调整函数

在实际应用中，深度信息的权重 λ(z)\lambda(z)λ(z) 采用以下调整策略：

λ(z)={λ0log⁡(z)+b,6<z<100.5,z<6\lambda (z) = \begin{cases} \lambda_0 \log(z) + b, & 6 < z < 10 \\ 0.5, & z < 6 \end{cases}λ(z)={λ0log(z)+b,0.5,6<z<10z<6

λ0=9.45\lambda_0 = 9.45λ0=9.45，b=−10.98b = -10.98b=−10.98，这两个参数用于控制权重随距离的变化。
对数衰减 机制确保 在 6m 以上，深度权重逐渐降低，减少远距离深度误差对匹配的影响。

总结

Weight Adjustment（权重调整） 在 ORB_D 描述符匹配中主要用于调整 深度信息的影响权重。

通过对深度信息权重的 动态调整，确保近距离时深度信息起主要作用，而远距离时减少其干扰，进而提高特征匹配的可靠性。

六、Depth mask-based corridor scene optimization基于深度掩模的走廊场景优化

在走廊等深度场景中，由于深度相机的有效范围受限，深度信息的可靠范围比办公室等环境要窄。因此，在提取角点特征时，只能通过立体相机的三角测量来获取深度信息。然而，当相机发生晃动时，左右相机之间特征点匹配的成功率降低，这导致深度信息不可靠，进而可能使最终的姿态估计产生偏差。

为了优化此类场景中的特征提取性能，本文引入了基于深度掩膜的优化方法。通过这一方法，可以在相机发生晃动时，优化特征点的提取和匹配，从而有效提高姿态估计的准确性。以下是该方法的具体步骤及其原因：

1. 检测晃动状态（避免因晃动引发的误匹配）

首先，通过IMU（惯性测量单元）信息来判断当前的运动状态。加速度计和陀螺仪数据可以用来评估相机的运动是否出现晃动。这是因为在相机发生晃动时，特征点的匹配精度通常会降低，导致深度信息的不准确，从而影响最终的姿态估计。因此，通过分析加速度和角速度的方差，判断相机是否处于晃动状态，能有效识别并避免因运动不稳定带来的影响。

2. 滤除不可靠的特征点（提高深度信息可靠性）

当检测到晃动状态时，通过深度掩膜只保留那些深度值在有效范围内的特征点。这是因为在晃动过程中，左右相机的特征点匹配可能会受到干扰，导致某些特征点的深度信息不准确或无效。通过设定深度掩膜，只提取那些深度值可信的特征点，可以确保后续的姿态估计依赖于可靠的深度信息，避免误匹配带来的影响。

3. 网格化均匀分布特征点（减少过密或过稀的特征点分布）

为了确保提取的特征点在图像中均匀分布，采用了网格化的方式进行分布优化。这样做的原因是，特征点的均匀分布能够避免在某些区域特征点过密（导致冗余计算）或过稀（导致匹配不充分）的情况。通过将图像划分成均匀的网格，并确保每个网格中都有适量的特征点，可以提高特征点的匹配效果和姿态估计的准确性。均匀分布也有助于避免在深度变化较大的区域（如走廊等）过度集中特征点，从而导致错误匹配。

总结

通过这三步优化，本文采用了基于深度掩膜的方法来增强在深度场景中（如走廊）相机晃动情况下的特征提取和姿态估计的性能：

检测晃动状态：避免相机晃动带来的误匹配和深度信息失真；
筛选有效特征点：确保只使用可靠的深度信息；
均匀分布特征点：避免特征点过密或过稀，优化匹配效果。

这些优化措施能够显著提升在复杂环境（如走廊）下的姿态估计稳定性和准确性，减少由于晃动或不稳定运动带来的潜在问题。

七、基于深度图像的后端优化设计

1. 定义符号与变量

PcamP_{cam}Pcam: 表示特征点在相机坐标系中的位置。
PimuP_{imu}Pimu: 表示特征点在IMU坐标系中的位置。
PworldP_{world}Pworld: 表示特征点在世界坐标系中的位置。
invdepth=1Pworld,zinv_{depth} = \frac{1}{P_{world,z}}invdepth=Pworld,z1: 世界坐标系下的反向深度，即特征点在世界坐标系中的深度的倒数。
ddepth_imgd_{depth\_img}ddepth_img: 特征点由深度相机提供的深度值。
dstereo,svdd_{stereo,svd}dstereo,svd: 由立体相机计算得到的深度信息。

2. 反向深度残差计算

首先，通过 深度残差 来衡量从深度相机和立体相机获取的深度信息之间的差异：

δinvdepth=1ddepth_img−1dstereo,svd(18)\delta inv_{depth} = \frac{1}{d_{depth\_img}} - \frac{1}{d_{stereo,svd}} \quad (18)δinvdepth=ddepth_img1−dstereo,svd1(18)

这一步的目的是计算反向深度残差，描述深度数据的差异。

3. 计算雅可比矩阵

为了在优化过程中更新参数，接下来分别计算关于相机的平移和旋转的雅可比矩阵。

a. 平移部分的雅可比矩阵：

平移部分的雅可比矩阵 ∂invdepth∂tiw\frac{\partial inv_{depth}}{\partial t_i^w}∂tiw∂invdepth 计算如下：

∂invdepth∂tiw=−1(Pworld,z)2Riwe3(19)\frac{\partial inv_{depth}}{\partial t_i^w} = -\frac{1}{(P_{world,z})^2} R_i^w e_3 \quad (19)∂tiw∂invdepth=−(Pworld,z)21Riwe3(19)

其中，tiwt_i^wtiw 是相机的平移向量，RiwR_i^wRiw 是相机的旋转矩阵，e3=[0,0,1]Te_3 = [0, 0, 1]^Te3=[0,0,1]T 是单位向量。

b. 旋转部分的雅可比矩阵：

旋转部分的雅可比矩阵 ∂invdepth∂qiw\frac{\partial inv_{depth}}{\partial q_i^w}∂qiw∂invdepth 计算如下：

∂invdepth∂qiw=−1(Pworld,z)2Riw(e3×Pimu)(20)\frac{\partial inv_{depth}}{\partial q_i^w} = -\frac{1}{(P_{world,z})^2} R_i^w (e_3 \times P_{imu}) \quad (20)∂qiw∂invdepth=−(Pworld,z)21Riw(e3×Pimu)(20)

其中，qiwq_i^wqiw 是描述相机旋转的四元数，×\times× 表示叉积。

4. IMU到相机的变换矩阵的雅可比矩阵

接下来，计算 IMU 到相机的变换矩阵 中的雅可比矩阵。

a. 平移部分的雅可比矩阵：

对于 IMU 到相机的变换矩阵的平移部分，雅可比矩阵计算如下：

∂invdepth∂tic=−1(Pworld,z)2Riwe3(21)\frac{\partial inv_{depth}}{\partial t_i^c} = -\frac{1}{(P_{world,z})^2} R_i^w e_3 \quad (21)∂tic∂invdepth=−(Pworld,z)21Riwe3(21)

其中，tict_i^ctic 表示从 IMU 到相机的平移向量。

b. 旋转部分的雅可比矩阵：

对于旋转部分的雅可比矩阵，计算如下：

∂invdepth∂qic=−1(Pworld,z)2Riw(e3×Pcam)(22)\frac{\partial inv_{depth}}{\partial q_i^c} = -\frac{1}{(P_{world,z})^2} R_i^w (e_3 \times P_{cam}) \quad (22)∂qic∂invdepth=−(Pworld,z)21Riw(e3×Pcam)(22)

其中，qicq_i^cqic 是从 IMU 到相机的旋转四元数。