[RA-L 2023] Coco-LIC：基于非均匀 B 样条的连续时间紧密耦合 LiDAR-惯性-相机里程计

这段代码是一个基于 C++ 的均匀 B 样条（Uniform B-spline）实现，专门用于表示 SE(3) 变换（即三维空间中的刚体变换，包括旋转和平移）。以下是对代码的总结：

1. 许可证和版权

• 使用 BSD 3-Clause License，允许在满足条件的情况下自由分发和修改。
• 版权归 Vladyslav Usenko 和 Nikolaus Demmel 所有，属于 Basalt 项目的一部分。

2. 功能概述

• 文件定义了一个模板类 Se3Spline，用于在 SE(3) 空间中构造和操作均匀 B 样条。
• SE(3) 表示三维空间的刚体变换，内部通过分离表示法实现：
  • 旋转部分：使用 SO(3) 样条（So3Spline）表示。
  • 平移部分：使用三维欧几里得样条（RdSpline）表示。
• 支持 N 阶样条（N 为模板参数），阶数和次数（degree）分别为 N 和 N-1。
• 参考文献：arXiv:1911.08860。

3. 主要特性

• 时间参数化：样条基于均匀时间间隔（time_interval_ns），支持纳秒级时间戳。
• 控制点（Knots）：存储 SE(3) 变换的控制点，支持动态添加、删除和修改。
• NURBS 支持：增加了非均匀有理 B 样条（NURBS）的功��，通过 knts（knot vector）和混合矩阵（blending_mats、cumu_blending_mats）实现。
• 评估功能：
  • 计算指定时间的位姿（poseNs）、位置（positionWorld）、速度（transVelWorld）、加速度（transAccelWorld）等。
  • 支持 NURBS 版本的评估（如 poseNsNURBS）。
• 雅可比矩阵：支持计算位姿对控制点的导数（Jacobian），用于优化或分析。
• 随机轨迹生成：提供生成随机轨迹的功能（genRandomTrajectory）。

4. 核心方法

• 构造函数：初始化样条，指定时间间隔和起始时间。
• 控制点操作：
  • setKnot：设置特定控制点的 SE(3) 位姿。
  • knots_push_back / knots_pop_back：添加或移除控制点。
• 时间管理：
  • minTimeNs / maxTimeNs：获取样条的时间范围。
  • extendKnotsTo：扩展控制点到指定时间。
• 调试工具：print_knots 用于输出控制点信息。
• 混合矩阵：InitBlendMat 和 AddBlendMat 用于计算 NURBS 的混合矩阵。

5. 数据结构

• 模板参数：
  • _N：样条阶数。
  • _Scalar：数据类型（如 double）。
• 内部成员：
  • pos_spline：平移部分的样条（RdSpline）。
  • so3_spline：旋转部分的样条（So3Spline）。
  • knts：NURBS 的节点向量。
  • blending_mats / cumu_blending_mats：混合矩阵及其累积形式。
• 数学类型：使用 Eigen 库定义向量和矩阵（如 Vec3、Mat3），以及 Sophus 库的 SO3 和 SE3。

6. 使用场景

• 该实现适用于需要平滑插值 SE(3) 变换的场景，例如机器人运动规划、SLAM（同步定位与建图）或计算机视觉中的轨迹优化。

7. 注意事项

• 代码依赖外部库（如 Eigen、Sophus 和 Basalt 项目头文件）。
• 包含大量注释和调试输出，便于开发和验证。
• NURBS 功能通过 [nurbs] 标记的代码段扩展，增加了非均匀样条的支持。

总结

Se3Spline 是一个功能强大且灵活的工具，用于在 SE(3) 空间中构造和管理均匀 B 样条，支持传统样条和 NURBS 的操作，适用于需要高精度轨迹插值的应用场景。代码结构清晰，注重数学严谨性和性能优化。

https://github.com/APRIL-ZJU/Coco-LIC

以下是代码的总结：

1. 总体描述：

• 实现SE(3)空间（三维刚体运动群）的均匀B样条，结合了SO(3)旋转样条和R³平移样条
• 基于BSD 3-Clause许可证，属于Basalt项目
• 使用模板编程，支持不同阶数（N）和标量类型

2. 核心功能：

• 样条管理：

  • 支持节点(knot)的增删改操作（push_back/pop_front等）
  • 可设置初始位姿、生成随机轨迹、扩展时间范围
  • 时间间隔固定（均匀样条），支持纳秒级时间精度

• 运动计算：

  • 位姿插值（poseNs）
  • 线速度/加速度（transVelWorld/transAccelWorld）
  • 角速度/加速度（rotVelBody/rotAccelBody）
  • 支持NURBS扩展实现（非均匀样条，部分未完成）

3. 关键数据结构：

• RdSpline：三维欧氏空间样条（平移）
• So3Spline：SO(3)旋转样条
• Jacobian结构体：用于优化时的导数计算
• 混合矩阵（blending_mats）：NURBS相关参数

4. 主要方法：

// 基本操作
setKnot()      // 设置节点位姿
poseNs()       // 计算指定时间的SE(3)位姿
genRandomTrajectory() // 生成随机轨迹

// 时间管理
extendKnotsTo() // 扩展时间范围
computeTIndexNs() // 计算时间对应的索引

// 导数计算
transVelWorld()  // 平移速度
rotVelBody()     // 旋转角速度（物体坐标系）

5. 实现特点：

• 分离式实现：旋转和平移分别用不同样条处理
• 高性能：利用Eigen矩阵运算和模板元编程
• 扩展性：预留NURBS支持接口（非均匀时间间隔）
• 安全断言：通过BASALT_ASSERT进行运行时检查

6. 应用场景：

• 机器人轨迹规划
• SLAM中的连续时间轨迹估计
• 运动捕捉数据插值
• 传感器数据的时间对齐

附：类关系示意图

Se3Spline
├─ RdSpline (平移)
└─ So3Spline (旋转)
   ├─ SO3操作
   └─ B样条基础

以下是代码总结的延续部分：

7. NURBS扩展实现：

• 核心改动：

  • 引入非均匀节点向量knts存储时间戳（纳秒），替代原有均匀间隔dt_ns_
  • 添加混合矩阵blending_mats和累积混合矩阵cumu_blending_mats用于NURBS基函数计算
  • 新增poseNsNURBS系列方法实现非均匀时间插值

• 关键实现细节：

  • 节点查询：通过GetCtrlIndexNURBS和GetIdxT在knts中二分查找时间区间
  • 混合矩阵生成：
    • InitBlendMat和AddBlendMatForGen根据节点间隔动态计算4x4混合矩阵
    • 矩阵元素基于NURBS递推公式，考虑相邻节点的时间差
    • 累积混合矩阵通过行累加简化后续计算
  • 特殊处理：
    • 平移和旋转分别调用RdSpline::evaluateNURBS和So3Spline::evaluateNURBS
    • 速度计算需传入delta_t和混合矩阵以处理非均匀间隔

• 示例代码片段：

  // NURBS位姿插值
  SE3 poseNsNURBS(int64_t time_ns) const {
    std::pair<int, double> su; // 节点索引和归一化时间
    GetIdxT(time_ns, su);      // 查找当前时间对应的节点区间
    
    Eigen::Matrix4d blending_matrix = blending_mats[su.first - 3];
    Eigen::Matrix4d cumulative_matrix = cumu_blending_mats[su.first - 3];
    
    SE3 res;
    res.so3() = so3_spline.evaluateNURBS(su.first-3, su.second, cumulative_matrix);
    res.translation() = pos_spline.evaluateNURBS(su.first-3, su.second, 
                                             (knts[su.first+1]-knts[su.first])*NS_TO_S,
                                             blending_matrix);
    return res;
  }
  

• 待完善部分：

  • 节点向量动态管理（当前示例硬编码初始knts）
  • 混合矩阵的自动化生成（现有代码需手动调用AddBlendMat）
  • 导数计算未完全适配NURBS（如Jacobian结构体仍为均匀样条设计）
  • 缺少节点插入/删除的配套逻辑

8. 性能与优化：

• 内存布局：
  • 使用Eigen矩阵固定尺寸（Mat3/Mat4）避免动态分配
  • SO3采用Sophus库的优化四元数操作
• 计算优化：
  • 预计算混合矩阵减少实时计算量
  • 利用模板展开循环（如Matrix3Array等类型）
• 潜在瓶颈：
  • 频繁的矩阵拷贝（如blending_mats存储完整矩阵）
  • NURBS查询未使用二分查找优化

9. 代码质量分析：

• 优点：
  • 模块化设计：分离SO3与平移计算
  • 强类型检查：通过Eigen和模板确保维度安全
  • 丰富的断言（BASALT_ASSERT）保障运行时正确性
• 改进建议：
  • 增加NURBS数学公式的文档注释
  • 重构重复代码（如时间查询逻辑）
  • 使用constexpr优化编译期计算
  • 添加单元测试覆盖NURBS分支

10. 扩展应用场景：

• 非均匀采样系统：处理传感器数据不规则时间戳
• 自适应轨迹优化：通过调整节点密度实现局部细化
• 运动重定向：NURBS特性适合编辑复杂运动曲线

总结示意图更新：

Se3Spline
├─ Uniform B-spline Core
│  ├─ RdSpline (均匀平移)
│  └─ So3Spline (均匀旋转)
└─ NURBS Extension (开发中)
   ├─ 非均匀节点管理(knts)
   ├─ 混合矩阵生成(blending_mats)
   └─ NURBS专用插值方法

此实现为SE(3)运动建模提供了灵活的基础框架，均匀B样条部分成熟稳定，NURBS扩展展示了向非均匀场景延伸的潜力，但需进一步完善数学一致性和接口完整性。