FAST-LIVO2 测试

1. Sophus

/home/h/programs/Sophus/sophus/so2.cpp: In constructor ‘Sophus::SO2::SO2()’:
/home/h/programs/Sophus/sophus/so2.cpp:32:26: error: lvalue required as left operand of assignment
   32 |   unit_complex_.real() = 1.;
      |                          ^~
/home/h/programs/Sophus/sophus/so2.cpp:33:26: error: lvalue required as left operand of assignment
   33 |   unit_complex_.imag() = 0.;

code /home/h/programs/Sophus/sophus/so2.cpp

SO2::SO2()
{
  // unit_complex_.real() = 1.;
  // unit_complex_.imag() = 0.;
  unit_complex_.real(1.);
  unit_complex_.imag(0.);
}

source  /workspace/FAST-LIVO2_ws/devel/setup.zsh

在 FAST-LIVO2（以及更广义的激光 SLAM/LO）中说的 “LiDAR 退化”（LiDAR Degeneracy） 指的是：

✅ 传感器在某些场景或运动下，几何信息不足，导致激光点云无法确定全部 6 自由度（6DoF）的位姿。

换句话说，就是点云对位姿的约束不足，造成某些方向上的定位变得不可靠、甚至完全不可观测。

🔍 一句话解释

退化就是：激光雷达给不出足够的几何信息 → 优化无法解出完整位姿 → 会导致漂移、发散、震荡、跳变。

📌 为什么会退化？

主要是因为 点云形状/分布太“单调”，导致某些方向的位姿变化无法从点云中被观测到。

FAST-LIVO2 论文中重点提到几类典型退化情况：

🧊 1. 平面场景（Plane-only）

例如：

地板 + 天花板（室内）
平坦的公路
墙面

如果环境只有一个大平面，那么：

沿平面方向的平移不可观测
沿平面法向的旋转不可观测

➡️ 例如在平直路上行驶，激光雷达难以测出前进的位移量。

🧱 2. 平行结构（Parallel Structures）

如：

两侧都是平行的墙（长走廊）
隧道
机库/仓库里的货架通道

只有平行特征会造成：

沿特征平行方向的位移不可观测
某些旋转也不可靠

➡️ 典型表现：走廊里会飘，高度和左右正常但是前后漂移大。

🌀 3. 旋转场景（旋转但缺少平移）

车辆/无人机原地旋转时：

激光扫描到的点几乎没变化
只靠旋转运动几乎无法确定位置

➡️ 会导致 yaw 漂移，常见于无人机悬停旋转。

FAST-LIVO2 使用 IMU 来补偿这个弱点。

🌲 4. 重复结构、几何重复（Repetitive Structure）

例如：

森林树干密集且相似
立柱阵列
地下停车场的立柱阵列

重复几何导致：

优化会匹配到错误的点
→ 造成跳变或者漂移

📦 FAST-LIVO2 如何处理退化？

FAST-LIVO2 的核心改进之一就是：

⭐ 在退化场景中，IMU + LiDAR 特征点的联合优化提高可观测性

方式包括：

（1）使用高频 IMU 维持系统可观测性

IMU 在纯旋转/缺乏平移时仍能提供约束。

（2）对具有退化方向的特征，自动进行权重调节

在退化方向上点云残差会被降低权重，不把系统“拉偏”。

（3）基于 Law of the Wall（LOTW）特征、线特征等多类型约束

让点云在弱纹理场景也能保持观测量。

（4）多尺度 FAST 特征提取

尽量保留更多具有约束意义的特征。

📌 举例总结（非常直观）

场景	为什么退化？	影响
平坦路面，开阔无障碍	只有一个地面平面	前进方向漂移
走廊、隧道	平行墙面限制约束	前后方向不可靠
原地旋转	点云几乎不变	yaw 飘
森林/重复结构	匹配易混淆	跳变、轨迹偏移

📎 简单比喻

LiDAR 退化就像你在一张白纸上找定位点：

如果纸上只有一条直线 → 你只能沿直线移动，不可观测
如果纸上只有一个点 → 你旋转位置不清楚
如果纸上啥都没有 → 完全定位不了

如果你需要，我可以提供：

✓ FAST-LIVO2 对退化场景的数学描述（Jacobian Rank 分析）
✓ 如何在无人机上避免/缓解 LiDAR 退化
✓ 是否可以结合 mmWave 雷达 / 视觉来强化退化方向
✓ 退化场景可观测性例子 + 仿真代码

你想继续了解哪一部分？

好 — 我把“可立刻使用”的方法按检测 → 主动缓解 → 被动/融合缓解 → 系统级措施四类列清楚，直接拿去在无人机上试验与调参。每一项我都给出为什么可行、如何实现、常用阈值/技巧，以及小片段伪代码或命令行提示，方便你马上落地。

1) 先检测：怎么判断 LiDAR 退化在发生

思路：用观测信息矩阵 / 雅可比矩阵的奇异值（SVD）来检测可观测性丢失；也可以基于残差分布或法向量方差做简单判据。

简单方法（最常用）

将当前帧多对特征点的残差对位姿的雅可比 J 拼成一个矩阵（或直接用线性化的 information matrix H = J^T W J）。
做 SVD：s = svd(H)。若最小奇异值 s_min 和次大奇异值的比值 s_min / s_max < thresh（典型 thresh 范围 1e-3 ~ 1e-5），则判定在退化或接近退化。
也可观察对某一自由度（例如前后方向）的奇异值，若某方向奇异值远小于其他方向 → 该方向退化（可只约束旋转或某个平移分量）。

快速启发式（不用构造大矩阵）

计算点云局部法向量的方差（或主方向协方差矩阵的最小特征值）。若某帧法向量主要集中在一个面（最小特征值很小），说明平面占主导 → 可能前后/侧向不可观测。
如果 ICP / 配准收敛但匹配残差变化非常小且位移估计波动大，也可能是退化。

2) 主动航迹与控制策略（最有效、也最直接）

原理：通过让无人机执行能“激发”所有自由度的运动（尤其是与退化方向正交的运动），提高观测可行性。

具体动作建议（容易执行且低成本）：

短距离横向/侧移（±0.5–2 m）：在平坦、开阔场景对前向退化最有效。
带小幅俯仰/侧滚的平移轨迹（yaw + 平移）：使激光扫描覆盖更多高度层次和几何变化。
不只纯旋转或纯匀速直线：避免长时间保持纯平面运动或只原地旋转。
在走廊/隧道出现退化时：做短促的横向摆动或左右扫航线（类似锯齿航线）。
定期小距上升/下降（0.2–1 m）：对分层特征缺乏时很有帮助（比如地面 + 低矮物体）。

实施建议：当检测到退化（见第1步），控制器自动触发一段短时“激励机动”然后恢复任务航线。

3) 传感器布置与硬件策略

选更合适的 LiDAR 类型/视角：向下倾斜一点的 LiDAR（比如向下 10–30°）能在地面/地物上获得高度变化信息，减少纯平面退化。
多传感器（双 LiDAR 或 360° + 前视）：不同视角互补，减少单视角退化。
添加简单的高度传感器（激光测高 / 超声 / 雷达）：对高度（z）不可观测时提供直接测量。
搭配立体摄像头或单目 + scale 初始/深度先验：视觉能提供纹理方向、色彩特征，和 LiDAR 互补。

4) 传感器融合（IMU、视觉、气压计、毫米波）

IMU（必须项，尤其是机载）：IMU 在纯旋转或短时运动中提供角速度/线加速度补偿。用 IMU 预积分（pre-integration）与 LiDAR 约束联合优化（例如滑动窗口或因子图）。
视觉（单目/双目）：在结构单一处，视觉纹理有时比点云更富信息（尤其是远景平面时）。
毫米波 / 雷达 / 激光测高：在能见度差或重复结构时作为补偿量。
融合细节：使用因子图/滑动窗口优化（VIO + LIO）；在检测到某个方向退化时临时提升 IMU/视觉因子的权重或依赖程度。

5) 算法层面的缓解（软件/匹配/优化技巧）

多类型特征融合：点特征（点到平面）、线特征（点到线）、面特征都一起用。线/面特征在走廊/隧道里能补偿点特征的不可观测方向。
自适应残差权重：当检测到某方向退化时，降低该方向点到平面残差的权重，避免“错误拉偏”。FAST-LIVO2 就有类似权重调节策略。
退化方向投影约束（Degeneracy-aware projection）：把优化问题在观测子空间和退化子空间投影开来，只在观测子空间做更新。
鲁棒匹配与去噪：使用更严格的匹配阈值（法向夹角、最近邻距离、局部几何一致性），减少在重复/相似结构下的错误匹配。
用地图先验/Loop closure（回环校正）：长期航线或有回环时，回环检测能修正累积漂移。
稀疏关键帧策略：只把信息量大的帧作为关键帧入图，减少冗余“平面”帧影响。

6) 实作细节：退化检测 + 自动策略伪代码

# 伪代码：用信息矩阵奇异值检测退化并触发激励机动
# J: n x 6 雅可比矩阵（n 个残差），W: n x n 权重对角矩阵
H = J.T @ W @ J
s = svd(H, compute_uv=False)   # s sorted descending
degeneracy_ratio = s[-1] / s[0]
if degeneracy_ratio < 1e-4:
    trigger_active_maneuver()   # 侧移、微俯仰或短暂横向摆动
    increase_imu_weight()       # 临时增加 IMU 在优化中的权重
    mark_frame_as_degenerate()

阈值建议：

degeneracy_ratio 阈值可从 1e-3 调整到 1e-5，先选 1e-4 做实验；
法向量最小特征值比 λ_min / (λ1+λ2+λ3) < 1e-3 可判定主要为平面分布。

7) 经验性调参建议（在无人机场景）

在室外开阔平地测试：优先验证 横向位移激励 是否能快速恢复可观测性。
在走廊/隧道场景：实现自动“横摆”或“锯齿”航线方案。
将 IMU 预积分误差协方差设得稍小（对 IMU 更信任）可以在短时退化时稳住估计，但不要过度，否则视觉/雷达信息无法被用好。
对法向量、匹配距离、残差阈值做在线统计（滑动窗口均值+方差），避免只用静态阈值。

8) 系统级冗余与安全策略

如果 LiDAR 退化且短时间内无法恢复：切换到 降级模式（用 IMU+高度传感器维持航线，减缓速度并寻求多样化视角或返回安全点）。
记录“退化事件日志”（时间、位置、触发动作、恢复时间）用于离线分析与策略改进。
若是在任务关键期（低空，障碍密集）发生退化，优先安全：减速、悬停、并执行激励动作或返航。

9) 常见场景与对应对策速查表

平坦农田 / 跑道：加横向位移 + 下倾 LiDAR 或高度传感器。
长走廊 / 隧道：横摆 / 锯齿航线 + 使用线特征（墙边缘）。
重复立柱/森林：加强鲁棒匹配、使用视觉或毫米波互补。
原地旋转：增加平移分量或使用 IMU 辅助。

10) 小结（实操优先）

先实现检测（SVD 或法向量方差），能在系统运行时自动发现退化。
在检测到退化时自动执行激励机动（短时横移/俯仰/上升）并提高 IMU 权重。
增加传感器冗余（下倾 LiDAR / 摄像头 / 高度计 / 毫米波）并在优化中融合。
在优化层面做退化感知（投影约束、自适应权重）和更鲁棒的匹配策略。
记录并离线分析退化日志，逐步调优阈值与运动策略。

如果你希望，我可以：

给你一段可直接运行的 Python 测试脚本（对点云帧计算主方向、法向方差与信息矩阵奇异值），方便你在飞控或地面站上实时检测退化；
或者根据你现在的 传感器配置（LiDAR 型号、是否向下倾斜、是否有相机/IMU/高度计），为你定制一套自动激励动作策略 + 参数建议（例如横移距离、触发阈值、激励持续时间）。

你想先要哪个？

可以 — 且这是非常有效的做法。把 mmWave（毫米波雷达） 与 视觉（摄像头） 和 LiDAR 结合，可以在 LiDAR 退化的方向上补强可观测性、提高鲁棒性与冗余性。下面把原理、优缺点、融合策略、实现细节、调参建议与注意事项都列清楚，方便你直接落地实现。

一、为什么可行（直观动机）

mmWave 雷达：穿透雾霾、灰尘、太阳眩光等恶劣环境，能稳定测距（相对粗但可靠），并能提供多目标距离与径向速度（Doppler）。对 尺度/深度 和 运动信息 有用，尤其能补偿 LiDAR 在远距或重复结构下的退化。
视觉（Camera）：提供丰富的纹理/结构信息，对几何特征（角点、纹理）敏感，能在 LiDAR 面状、平面占主导时提供横向或远距特征；但受光照、运动模糊影响。
互补性：三者互补 —— LiDAR 精确（点云稠密、几何准确），视觉丰富（方向/纹理），毫米波稳健（恶劣条件/速度信息）。组合后可显著提高所有自由度的观测能力。

二、融合架构选择（总体思路）

按紧耦合/松耦合分两类，建议在无人机上优先选择 紧耦合因子图 / 滑动窗口融合（LIO + VIO + Radar），因为紧耦合能在退化检测时立刻利用其他传感器约束：

紧耦合滑窗优化（推荐）
- 状态变量：位姿、速度、IMU偏置、关键帧外参（相机、radar 相对 IMU）
- 因子：IMU 预积分因子、LiDAR 点到面/点到线残差因子、视觉重投影因子、radar 距离/速度因子、回环因子。
- 优点：统计一致、能直接处理协方差、退化时能自动用其它因子补偿。
松耦合 / 级联融合（备用）
- 例如先运行 VIO，再用 VIO 输出辅助 LIO，或把 radar 做独立障碍/高度提供器。实现快但精度/一致性差一点。适合资源受限场景。

三、如何把 mmWave / 视觉具体接入（要点）

mmWave 雷达接入

输出：目标距离（range）、角度（解模糊后可得 azimuth/elevation 或只得扇区）、径向速度（Doppler），可能还有反射强度。
因子设计：
- 点到-range 因子：将雷达测得的某一回波对应到估计状态上，残差为 r = measured_range - predicted_range(state, object_position)。
- Doppler 因子（可选）：把径向速度作为对速度分量的约束（对移动平台/静态环境区分要做好）。
- 扇区/角度不精确：如果雷达角度分辨率差，把角度当作弱约束或只用 range + Doppler。
协方差/权重：radar 的测量噪声通常比 LiDAR 大，初始协方差设大一些；但在 LiDAR 退化时可动态降低协方差（增加信任）。

视觉接入

视觉类型：单目 / 双目 / RGB-D。双目或 RGB-D 更易给出尺度，单目需和 LiDAR/radar/IMU 联合恢复尺度。
因子设计：
- 重投影因子（Reprojection factor）：经典 VIO 做法（视觉特征与相机pose/landmark 之间残差）。
- 直接/半直接方法：在纹理极其丰富或需要低延迟时可选，但调参更麻烦。
对退化有用的点：纹理点、角点、线特征（走廊墙边线）都很有价值。
在弱光或模糊时：降低视觉权重或停用视觉因子，改由 radar / LiDAR/IMU。

四、退化情景下的自适应策略（核心：动态权重与切换）

实现关键是检测退化 → 调整因子权重/策略。示例流程：

在线退化检测（使用 LiDAR 的信息矩阵奇异值等）。
若检测到某方向退化：
- 提升 mmWave 因子 与 视觉因子 在对应自由度上的权重（降低 LiDAR 在退化方向的权重）。
- 如果视觉受光照影响，则优先信任 mmWave + IMU。
- 触发主动机动（小幅侧移/俯仰）以创造新的视觉/雷达观测。
若 mmWave 观测到稳定的高度/range，可在滑窗中把该方向的协方差显著缩小，直接补偿 LiDAR 的缺陷。

示例伪代码（伪）：

if lidar_degenerate_in_x:
    increase_weight(radar_range_factor, factor=10)
    increase_weight(vision_reproj_factor, factor=5)
    decrease_weight(lidar_point_factor, direction='x', factor=0.1)
    trigger_small_lateral_maneuver()

五、同步、外参与标定（务必注意）

时间同步：IMU、LiDAR、相机、mmWave 必须尽量精确对齐；时间偏差会导致融合错误（尤其高速飞行）。使用硬件时间戳或网络时间同步（PPS）优先。
外参标定：相机-IMU、LiDAR-IMU、radar-IMU 的位姿外参精确度直接影响优化。建议：
- 先做离线标定（Kalibr/自定义标定）获取初始外参。
- 在线做微调（将外参作为变量在滑窗中精细优化，或定期自标定）。
标定注意：mmWave 的测角误差通常较大，外参的不确定性要考虑在内（可把其协方差设大以避免过度信任）。

六、协方差/噪声建模与调参建议

初始协方差（粗略建议，可实验调整）：
- LiDAR 点到面残差 σ ≈ 0.01–0.05 m（取决型号与点云稠密度）
- Vision 重投影 σ ≈ 1–3 px（取决特征质量）
- mmWave range σ ≈ 0.05–0.5 m（量级依雷达型号与目标回波）
- mmWave Doppler σ ≈ 0.05–0.2 m/s（若可用）
自适应调整：当检测到 LiDAR 退化，逐步缩小 mmWave 的协方差（提高信任），或增加视觉因子权重；在视觉失效（暗光/过曝）时再降低视觉权重。
鲁棒核（Huber/Tukey）：对所有测量都用鲁棒核，防止异常回波或错误匹配拉偏估计。

七、实现技巧与工程细节

数据关联（association）：mmWave 的点/回波与 LiDAR/视觉特征做关联有挑战（角度分辨率低）。常用策略：把 radar 视为检测“障碍物/平面/地面高度”的弱约束，而非逐点精确匹配。
使用 Radar 为高度/速度先验：例如把 radar 得到的地面距离作为高度先验项（尤其当 LiDAR 指向前方，地面不明显时）。
滤波/鲁棒化：对 radar 回波做门限/聚类（CFAR）来过滤杂波。
计算资源：紧耦合多传感器融合计算量大，滑窗长度/关键帧频率需与飞控机算力匹配。可先做轻量版：短滑窗（5–10 帧）、稀疏特征。

八、测试 & 验证建议

构造退化场景：平坦开阔地、走廊、重复柱状物、雨/雾（或模拟）等场景，开展对比试验（LiDAR-only vs LiDAR+vision vs LiDAR+radar vs 三传感器融合）。
指标：位置误差（相对/绝对）、漂移速率、失败/发散次数、恢复时间（从退化到恢复）。
日志：记录每个传感器的 residual、奇异值、权重变化、触发的主动机动，用于离线调参。
鲁棒性测试：低光、逆光、雨雾、尘土场景尤其要测试视觉与雷达在现实条件下的表现。

九、可能的局限与注意事项

mmWave 能提供稳定的 range/Doppler 但角度分辨率有限，在需要高精度几何约束的方向上效果有限。
视觉在弱光/快速旋转/严重运动模糊下失效，需要和 IMU/雷达配合。
系统复杂度与调参负担上升：需要做更多的标定、同步与协方差调优。
计算负载上升——在资源受限的飞控上可能需要硬件加速或把部分计算下放到地面站/更强的机载电脑。

十、落地优先级与推荐实施步骤（最小可行方案 → 完整方案）

最小可行（快速验证）：
- 在现有 LIO 框架上加入一个简单的 radar-range 因子（range→高度或前向距离约束），把 radar 的外参与协方差先定死。
- 在退化检测触发时切换权重并记录效果。
中级（增强鲁棒）：
- 加入视觉重投影因子（或集成现成 VIO 模块），做松耦合验证后切换到紧耦合。
- 改进 radar 数据预处理（聚类、CFAR、Doppler 解析）。
完整（生产级）：
- 实现紧耦合滑窗（IMU+LiDAR+Camera+Radar），外参在线优化、时间同步、动态协方差调整、故障检测与降级策略。

如果你愿意，我可以接下来做其中一项具体工作（任选其一直接给出实现代码/伪代码与配置建议）：

A. 给出 紧耦合因子图 中加入 mmWave range + Doppler 因子的数学推导与 C++/g2o/Ceres 伪实现代码；
B. 提供 python 脚本：实时计算 LiDAR 信息矩阵奇异值并在退化时打印/发布一个 trigger，方便你在飞控上联动机动；
C. 根据你现有的传感器清单（LiDAR 型号、相机、mmWave 型号、是否有硬件时间戳/IMU），直接给出一套外参初始值、协方差和滑窗长度建议。

你想先要哪个？

Compressed Depth Image Transport - Compression requires single-channel 32bit-floating point or 16bit raw depth images (input format is: bgr8).

运行

roslaunch fast_livo mapping_avia.launch
rosbag play src/FAST-LIVO2/data/HKU_Landmark.bag

修改配置文件保存地图：

pcd_save_en: true

去掉相机：

  img_en: 0

可视化：

sudo apt-get install  pcl-tools
pcl_viewer all_raw_points.pcd

在 FAST-LIO2 原版论文与代码 中，系统本质上是 LiDAR + IMU（LIO），并 不依赖相机参与主状态估计。
但作者团队在扩展系统（例如 FAST-LIO + VI、VIL-SLAM、FAST-LIVO、E-FAST-LIVO）中确实加入了相机，用意主要是为了解决 LiDAR 在退化场景下的弱约束问题。

下面分层给你解释：

✅ FAST-LIO2 中相机的作用（扩展系统，而不是 LIO2 本体）

在 FAST-LIO2 的扩展（如 FAST-LIVO/FAST-LIVO2）中，相机用于增强退化方向的观测，核心作用包括：

🟦 1. 弥补 LiDAR 的退化方向

当 LiDAR 退化时，如：

走廊（只有纵向结构）
林冠上方飞行（只有地面，缺乏侧向结构）
高空飞行（激光击中率低）
墙面平行滑行（点云主要落在平面上）

LiDAR 的约束矩阵会在某方向出现 rank 丢失。
相机会提供丰富的特征（点、线、角点、patch）用于补充约束。

例如，墙旁平行移动时 LiDAR 对侧向约束弱，但视觉特征能约束横向漂移。

🟦 2. 提供额外的姿态观测

视觉的特征点追踪（LK、光流、patch tracker）
可以提供：

旋转约束（缓解 IMU bias 漂移）
稳定的短期相对姿态变化
回环检测辅助（VIO + LIO）

特别在 LiDAR 稀疏或视野少时，视觉能够给 IMU 一个 “辅助惯导”。

🟦 3. 对高空/远距离飞行有用

无人机在几百米高度飞行时：

LiDAR 的有效回波大幅减少（点云稀疏，结构简单）
激光束角度导致水平面击中率极低

视觉可以通过：

地面纹理
城市边缘线
地表特征点

提供额外的约束。

这也是 FAST-LIVO2 在高空飞行中非常稳定的关键原因之一。

🟦 4. 融合方式（技术点）

FAST-LIVO2 的核心是：

● LiDAR：点到平面残差

● IMU：预积分

● Camera：patch-based visual residual（深度来自 LiDAR）

流程如下：

LiDAR 给出快速地图 → 投影图像特征到 3D
视觉特征在 3D 空间中形成约束
所有残差统一进入 ES-EKF

视觉约束不是独立估计，而是作为额外观测。

因此相机的作用本质上是：

为 LiDAR 在退化方向补充几何结构，为 IMU 提供短时漂移抑制。

🎯 总结：FAST-LIO2 扩展中的相机主要负责什么？

功能	说明
⭐ 增强 LiDAR 退化方向	提供 LiDAR 不敏感方向的特征约束
稳定态估计	与 LiDAR+IMU 一起提供全维约束
降低漂移	弥补 LiDAR 稀疏、弱结构场景
高空飞行支持	视觉替代 LiDAR 提供地面结构信息
任务冗余	错层传感器融合，提高鲁棒性

如果你无人机上已有 mmWave 雷达，我可以告诉你：

📌 mmWave 雷达 + LiDAR + IMU + Camera
是当前无人机对抗退化场景的最强组合。

如果你告诉我你使用的 LiDAR 型号、飞行高度、场景类型，我可以给你更具体的融合建议。

你要深入这个话题吗？我可以继续讲：

如何检测 LiDAR 退化实时发生？
如何用 mmWave 雷达补偿 LiDAR 的全局方向漂移？
如何把视觉与 LiDAR 融合成统一残差（FAST-LIVO2 方式）？

posted @ 2025-12-05 14:49 Zenith_Hugh 阅读(5) 评论(0) 收藏举报

刷新页面返回顶部

Zenith Hugh

We Go To The Moon