【UV-SLAM】eLSD/LBD 数据维度 UV-SLAM吸收借鉴

实施计划：UV-SLAM 两项改进借鉴

Context

当前线特征仅 2-3 条能三角化，对精度贡献有限（RMSE 0.001735m，-0.64%）。

借鉴 UV-SLAM 两项技术继续改进：

1. MARGIN_SECOND_NEW 自适应边缘化：当前帧运动量不足时，保留旧关键帧约束，防止退化
2. VP 约束：利用场景中结构化直线（导轨/边缘）强化线特征方向精度

---

Phase A：MARGIN_SECOND_NEW 自适应边缘化

现状

• 边缘化触发：frame_poses.size() > max_kfs 时
• 选择策略（两层）：
  a. 特征连接度 < 0.1（vio_kf_marg_feature_ratio）→ 边缘化该 KF
  b. DSO 评分函数：score = sqrt(dist_to_latest) * Σ(1/dist_to_others) → 最小 score 被边缘化
• 始终保护最新 2 个 KF (end_minus_2)
• 无视差计算，无"视差不足时保留旧帧"机制

UV-SLAM 做法

feature_manager.cpp#addFeatureCheckParallax：

• 计算当前帧与上上帧之间的平均视差（像素）
• 视差 >= MIN_PARALLAX → MARGIN_OLD（边缘化最老帧，当前帧成为 KF）
• 视差 < MIN_PARALLAX → MARGIN_SECOND_NEW（当前帧不是 KF，丢次新帧，保留旧帧约束）

实施方案（最小改动）

文件：core/vi_estimator/sqrt_keypoint_vio.cpp（marginalize 函数，~1362行附近）

核心逻辑：在已有两层选择之前，增加一个"视差判断"：当 num_points_connected 中当前帧与最老 KF

的特征连接比例较高（运动量不足），将 id_to_marg 设为倒数第二个 KF 而非最老 KF。

具体实现：

// 在第一层选择之前，检查当前帧视差（用 num_points_connected 间接代替视差）

// 当最老 KF 特征连接度 >= high_ratio（说明运动小、视差小）→ 改边缘化次新 KF

const float high_ratio_thresh = 0.5f; // 可配置

if (kf_ids.size() > 3) {

int64_t oldest_kf = *kf_ids.begin();

int64_t second_newest_kf = *std::prev(kf_ids.end(), 2);

float oldest_ratio = 0.f;

if (num_points_kf.count(oldest_kf) && num_points_kf.at(oldest_kf) > 0 &&

num_points_connected.count(oldest_kf)) {

oldest_ratio = num_points_connected.at(oldest_kf) /

static_cast(num_points_kf.at(oldest_kf));

}

if (oldest_ratio >= high_ratio_thresh) {

// 最老 KF 仍被大量点连接（运动量小），保留旧帧约束，边缘化次新 KF

id_to_marg = second_newest_kf;

}

}

阈值说明：

• high_ratio_thresh = 0.5：50% 以上的老 KF 地标仍可见 → 认为运动量不足 → 保留旧帧
• 可通过 config.vio_kf_marg_high_ratio（新增配置项）配置

需要修改的内容：

1. sqrt_keypoint_vio.cpp：在第一层选择（~1372行）之前添加上述逻辑（约20行）
2. config/genrobot_config.json（可选）：添加 "config.vio_kf_marg_high_ratio": 0.5
3. core/utils/vio_config.h（可选）：添加配置字段声明

---

Phase B：VP（消失点）约束

VP 约束原理

• 每条线段有一个"消失点方向" VP（3D 单位向量）
• 残差：r = acos(|d_c · vp|) ，d_c 是线在相机系的方向向量
• 约束含义：线方向必须指向它所属的消失点

实施方案（最小实现）

消失点检测（前端，frame_to_frame_optical_flow.h 中线特征检测后）：

• 每次 LSD 检测（关键帧时）收集当前帧所有线段的方向向量
• 使用简单 K-means（K=3，对应 Manhattan 三方向）聚类方向向量
• 每条线分配到最近的 VP（角度距离），超过 10° 阈值的线标为无 VP

数据传输：

• OpticalFlowResult::LineObservation2d 新增字段 vp_dir(3) 和 has_vp(bool)

后端 VP 残差（sqrt_keypoint_vio.cpp#addLineLandmarkResiduals）：

• 对有 VP 的线，在 H/b 累加中加入 VP 残差贡献：
  r_vp = acos(|Rcw * vp_w · d_c|)（需要将 vp_w 变换到相机系）
  Jacobian 对位姿 SE3 扰动（6维）
• VP 方向 vp_w 首次出现时需变换到世界系并存入 LineLandmark

涉及文件：

1. core/optical_flow/frame_to_frame_optical_flow.h：VP 检测 + LineObservation2d 扩展
2. core/vi_estimator/line_landmark_database.h：LineLandmark 新增 vp_world 字段
3. core/vi_estimator/sqrt_keypoint_vio.cpp：addLineLandmarkResiduals 加入 VP 残差

---

验证计划

1. 编译无报错：cmake --build build --parallel 6
2. 本地运行无崩溃：grslam_vio_run_mcap（开 vio_debug 确认线数/VP数）
3. Docker 评估：./init.sh && VIO_MAX_FRAMES=30 docker run ... pipeline_eval.sh
4. 比较 RMSE 与基准 0.001746m 和当前 0.001735m

---

执行顺序

1. Phase A（自适应边缘化）→ vio-review → 通过后提交
2. Phase B（VP 约束）→ vio-review → 通过后提交

1. UV-SLAM吸收借鉴

Phase 1（已完成，今天实施）

┌────────────────┬───────────────────────┬──────────────────────┐

│ 改动 │ 文件 │ 效果 │

├────────────────┼───────────────────────┼──────────────────────┤

│ 平行视差阈值 │ sqrt_keypoint_vio.cpp │ 减少欠约束三角化 │

│ 3.6° → 5.7° │ :993 │ │

├────────────────┼───────────────────────┼──────────────────────┤

│ 三角化后重投影 │ sqrt_keypoint_vio.cpp │ 坏线不进 BA，RMSE │

│ 验收 │ :1007+ │ −1.24% │

├────────────────┼───────────────────────┼──────────────────────┤

│ Cauchy 替换 Hu │ sqrt_keypoint_vio.cpp │ 更重尾，抑制线外点 │

│ ber（线专属） │ :2248 │ │

├────────────────┼───────────────────────┼──────────────────────┤

│ Phase C 端点门 │ linefeature_tracker.c │ 前端 ID 不被 LSD │

│ → │ pp:534 │ 端点抖动截断 │

│ lineGeomMatch │ │ │

├────────────────┼───────────────────────┼──────────────────────┤

│ Layer1 best_d │ linefeature_tracker.c │ 几何匹配是主门，端点 │

│ = FLT_MAX │ pp:352 │ 只排序 │

├────────────────┼───────────────────────┼──────────────────────┤

│ LBD Hamming 25 │ linefeature_tracker.h │ 减少有效匹配被误杀 │

│ → 40 │ │ │

├────────────────┼───────────────────────┼──────────────────────┤

│ importBeforeMa │ local_line_map.h │ 阻止 2 帧低质量线进 │

│ rg 最小 4 帧 │ │ LocalLineMap │

└────────────────┴───────────────────────┴──────────────────────┘

Phase 2（中期，架构级）

线参数联合优化（Orthonormal 4-DOF）

当前 Plucker 坐标在 BA 中是固定常量，只有位姿被优化。UV-SLAM

用正交归一化 4 参数 (U∈St(3,2), W∈SO(2)) 表示线，与位姿联合优化。

• 影响文件：line_landmark_database.h（存储参数）、sqrt_keypoint_vio.
  cpp（H/b 加 4×4 线块 + 4×6 位姿-线交叉块）
• 预期增益：错误三角化可被修正，线约束更精确

解析 Jacobian 替换数值差分

当前每个线残差做 6 次 plk_to_pose 数值微分。解析 Jacobian：

dr/dT = (1/s)·(p^T - (p^T·e)·e_xyT) · d(nc_c)/dT

其中 d(nc_c)/dT 有闭合解。

• 预期增益：BA 速度 ~6× 提升，可扩大 max_lines

Phase 3（长期，系统级）

线特征边缘化 Schur 补偿

当前关键帧边缘化直接删线观测，线参数不入先验信息矩阵。应做与点特征相

同的 Schur 消元。

ELSED 检测器替换 LSD

ELSED 端点稳定性显著优于 LSD（不断截断/分裂），从根源提升前端 ID

连续性。

消失点（VP）约束

室内结构化环境中，曼哈顿假设下 VP 方向约束可显著改善线方向估计精度。

LSD/LBD 数据维度

LSD 输出：vector，每条线包含：

• startPointX/Y、endPointX/Y：像素端点（float 2D），运行在预缩放图像上，匹配后乘
  scale_inv 还原
• lineLength、angle、octave：长度（px）、方向角（rad）、金字塔层

LBD 输出：cv::Mat [N × 32] uint8，即每条线 256 位二进制描述子，与 ORB

格式相同，匹配用 cv::NORM_HAMMING

传入 VIO 的结构：

struct LineObservation2d {

int line_id; // 跨帧追踪 ID

cv::Point2f start_px; // 原图像素起点

cv::Point2f end_px; // 原图像素终点

};

VIO 后端 3D 表示：Plücker 坐标 Eigen::Matrix<double,6,1>（方向3维 +

矩3维），归一化后用于线段 BA 残差。

---

2. Line 特征完整流程

MCAP图像(uint16)

→ uint8灰度

→ cv::resize → lsd_img (长边320px) ← lsd_resize_long_edge

→ LSD::detect(lsd_img) → vector ← ~30ms 主要瓶颈

→ filter(length) + lsd_max_raw截断 + partial_sort(lsd_pre_lbd_max)

→ BinaryDescriptor::compute(lsd_img) → Mat[N×32]

→ 坐标 × scale_inv 还原到原图

|

├─ 阶段A（有位姿先验时）

│ ├─ LocalLineMap先验候选 → 端点近邻匹配 → 复用line_id

│ └─ 上帧端点 × IMU投影 → 近邻搜索 + LBD Hamming验证 → 复用line_id

│

├─ 阶段B：网格4×3均匀筛选（每格取最长线段）

│

└─ 阶段C：LBD match补充未追踪线段 + 汇总至max_lines=20

|

OpticalFlowResult::line_observations[cam_id]

|

VIO后端：

双目三角化 → 3D Plücker坐标

LocalLineMap维护（跨帧线段地图）

线段BA残差 → 联合优化位姿+地图

投影先验 → 下帧setPriorCandidates