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

发布于:2025-03-15 ⋅ 阅读:(18) ⋅ 点赞:(0)

这段代码是一个基于 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)分别为 NN-1
  • 参考文献:arXiv:1911.08860

3. 主要特性

  • 时间参数化:样条基于均匀时间间隔(time_interval_ns),支持纳秒级时间戳。
  • 控制点(Knots):存储 SE(3) 变换的控制点,支持动态添加、删除和修改。
  • NURBS 支持:增加了非均匀有理 B 样条(NURBS)的功��,通过 knts(knot vector)和混合矩阵(blending_matscumu_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 用于输出控制点信息。
  • 混合矩阵InitBlendMatAddBlendMat 用于计算 NURBS 的混合矩阵。

5. 数据结构

  • 模板参数
    • _N:样条阶数。
    • _Scalar:数据类型(如 double)。
  • 内部成员
    • pos_spline:平移部分的样条(RdSpline)。
    • so3_spline:旋转部分的样条(So3Spline)。
    • knts:NURBS 的节点向量。
    • blending_mats / cumu_blending_mats:混合矩阵及其累积形式。
  • 数学类型:使用 Eigen 库定义向量和矩阵(如 Vec3Mat3),以及 Sophus 库的 SO3SE3

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)和标量类型
  1. 核心功能
  • 样条管理

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

    • 位姿插值(poseNs)
    • 线速度/加速度(transVelWorld/transAccelWorld)
    • 角速度/加速度(rotVelBody/rotAccelBody)
    • 支持NURBS扩展实现(非均匀样条,部分未完成)
  1. 关键数据结构
  • RdSpline:三维欧氏空间样条(平移)
  • So3Spline:SO(3)旋转样条
  • Jacobian结构体:用于优化时的导数计算
  • 混合矩阵(blending_mats):NURBS相关参数
  1. 主要方法
// 基本操作
setKnot()      // 设置节点位姿
poseNs()       // 计算指定时间的SE(3)位姿
genRandomTrajectory() // 生成随机轨迹

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

// 导数计算
transVelWorld()  // 平移速度
rotVelBody()     // 旋转角速度(物体坐标系)
  1. 实现特点
  • 分离式实现:旋转和平移分别用不同样条处理
  • 高性能:利用Eigen矩阵运算和模板元编程
  • 扩展性:预留NURBS支持接口(非均匀时间间隔)
  • 安全断言:通过BASALT_ASSERT进行运行时检查
  1. 应用场景
  • 机器人轨迹规划
  • SLAM中的连续时间轨迹估计
  • 运动捕捉数据插值
  • 传感器数据的时间对齐

附:类关系示意图

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

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


7. NURBS扩展实现

  • 核心改动

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

    • 节点查询:通过GetCtrlIndexNURBSGetIdxTknts中二分查找时间区间
    • 混合矩阵生成
      • InitBlendMatAddBlendMatForGen根据节点间隔动态计算4x4混合矩阵
      • 矩阵元素基于NURBS递推公式,考虑相邻节点的时间差
      • 累积混合矩阵通过行累加简化后续计算
    • 特殊处理
      • 平移和旋转分别调用RdSpline::evaluateNURBSSo3Spline::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扩展展示了向非均匀场景延伸的潜力,但需进一步完善数学一致性和接口完整性。


网站公告

今日签到

点亮在社区的每一天
去签到