【C++】sophus : spline.hpp 在李群（Lie Group）上进行样条插值 (十二)

原创 于 2024-12-15 20:37:53 发布 · 437 阅读 · 3 ·
CC 4.0 BY-SA版权
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
文章标签：

#人工智能 #机器学习 #算法 #深度学习

这段代码实现了在李群（Lie Group）上进行样条插值的数学工具，基于 Lovegrove 等人在 2013 年的论文提出的算法。以下是代码的关键点总结：

1. 核心概念

  • 样条函数

    ：利用 B 样条基函数（B-Spline Basis Functions）构建插值函数，适用于连续变换的平滑插值。

  • 李群和李代数

    ：基于李群（如旋转、位移等连续变换的集合）和其对应的李代数，完成插值和微分操作。

2. 主要功能

（1）B 样条基函数实现

类 SplineBasisFunction：

  • 提供了三次 B 样条的基函数 B(u) 和其一阶、二阶导数 Dt_B(u, delta_t) 与 Dt2_B(u, delta_t) 的实现。

  • 使用矩阵 C 定义 B 样条的系数矩阵。

（2）样条插值函数

类 BasisSplineFn：

  • 在李群中通过控制点计算样条插值值（parent_T_spline）、一阶导数（Dt_parent_T_spline）和二阶导数（Dt2_parent_T_spline）。

  • 核心方法：

    • A(...)：利用基函数值计算控制点的李群变换。
    • Dt_A(...) 和 Dt2_A(...)：分别计算插值的一阶和二阶导数。

    • 使用李群的指数映射 exp 和对数映射 log 处理插值。

（3）样条段的分段计算

类 BasisSplineSegment：

  • 处理样条插值的单个段，根据分段类型（SegmentCase）计算插值值及导数。

  • 分段类型：

    • first：样条段起始。
    • normal：普通样条段。
    • last：样条段末尾。

  • 使用控制点的相对位姿（通过李代数对数计算）定义控制矢量。

（4）完整样条实现

类 BasisSplineImpl：

  • 提供多段样条的整体实现。

  • 核心功能：

    • parent_T_spline：计算第 i 段样条的插值值。
    • Dt_parent_T_spline 和 Dt2_parent_T_spline：计算插值值的一阶和二阶导数。

    • 管理控制点并提供分段数量、时间步长等信息。

3. 应用场景

  • 运动插值

    ：用于机器人学、计算机视觉等领域中平滑表示连续的位姿（如旋转和位移）。

  • 时间微分计算

    ：支持计算位姿的速度和加速度。

  • 关键帧控制

    ：利用离散控制点生成连续轨迹。

4. 总结

该代码结合了数学理论（B 样条）与几何表示（李群与李代数），提供了高效且精确的插值工具，适用于对连续变换（如旋转和平移）的光滑插值和导数计算。

/// @file// Basis spline implementation on Lie Group following:// S. Lovegrove, A. Patron-Perez, G. Sibley, BMVC 2013// http://www.bmva.org/bmvc/2013/Papers/paper0093/paper0093.pdf
#pragma once  // 确保该文件只被编译一次
#include "types.hpp"  // 包含头文件 types.hpp
namespace Sophus {  // 定义命名空间 Sophus
template <class Scalar>class SplineBasisFunction {  // 定义模板类 SplineBasisFunction public:  static SOPHUS_FUNC Eigen::Matrix<Scalar, 3, 4> C() {  // 定义静态成员函数 C 返回一个 Eigen 矩阵    Eigen::Matrix<Scalar, 3, 4> C;  // 定义 3x4 大小的矩阵    Scalar const o(0);  // 定义常量 o 值为 0
    // clang-format off    C << Scalar(5./6),  Scalar(3./6), -Scalar(3./6),  Scalar(1./6),  // 填充矩阵 C 的值         Scalar(1./6),  Scalar(3./6),  Scalar(3./6), -Scalar(2./6),                    o,             o,             o,  Scalar(1./6);    // clang-format on    return C;  // 返回填充好的矩阵 C  }
  static SOPHUS_FUNC Vector3<Scalar> B(Scalar const& u) {  // 定义静态成员函数 B 返回一个 3 维向量    // SOPHUS_ENSURE(u >= Scalar(0), "but %", u);  // 确保 u >= 0    // SOPHUS_ENSURE(u < Scalar(1), "but %", u);  // 确保 u < 1    Scalar u_square(u * u);  // 计算 u 的平方    return C() * Vector4<Scalar>(Scalar(1), u, u_square, u * u_square);  // 计算并返回向量 B  }
  static SOPHUS_FUNC Vector3<Scalar> Dt_B(Scalar const& u, Scalar const& delta_t) {  // 定义静态成员函数 Dt_B 返回一个 3 维向量    // SOPHUS_ENSURE(u >= Scalar(0), "but %", u);  // 确保 u >= 0    // SOPHUS_ENSURE(u < Scalar(1), "but %", u);  // 确保 u < 1    return (Scalar(1) / delta_t) * C() *           Vector4<Scalar>(Scalar(0), Scalar(1), Scalar(2) * u, Scalar(3) * u * u);  // 计算并返回向量 Dt_B  }
  static SOPHUS_FUNC Vector3<Scalar> Dt2_B(Scalar const& u, Scalar const& delta_t) {  // 定义静态成员函数 Dt2_B 返回一个 3 维向量    // SOPHUS_ENSURE(u >= Scalar(0), "but %", u);  // 确保 u >= 0    // SOPHUS_ENSURE(u < Scalar(1), "but %", u);  // 确保 u < 1    return (Scalar(1) / (delta_t * delta_t)) * C() *           Vector4<Scalar>(Scalar(0), Scalar(0), Scalar(2), Scalar(6) * u);  // 计算并返回向量 Dt2_B  }};
template <class LieGroup_>class BasisSplineFn {  // 定义模板类 BasisSplineFn public:  using LieGroup = LieGroup_;  // 定义类型别名 LieGroup  using Scalar = typename LieGroup::Scalar;  // 定义标量类型  using Transformation = typename LieGroup::Transformation;  // 定义变换矩阵类型  using Tangent = typename LieGroup::Tangent;  // 定义切向量类型
  static LieGroup parent_T_spline(      const LieGroup& parent_Ts_control_point,      std::tuple<Tangent, Tangent, Tangent> const& control_tagent_vectors,      double u) {  // 定义静态成员函数 parent_T_spline    auto AA = A(control_tagent_vectors, u);  // 计算 A 矩阵    return parent_Ts_control_point * std::get<0>(AA) * std::get<1>(AA) * std::get<2>(AA);  // 计算并返回 spline 变换矩阵  }
  static Transformation Dt_parent_T_spline(      const LieGroup& parent_Ts_control_point,      std::tuple<Tangent, Tangent, Tangent> const& control_tagent_vectors,      double u, double delta_t) {  // 定义静态成员函数 Dt_parent_T_spline    auto AA = A(control_tagent_vectors, u);  // 计算 A 矩阵    auto Dt_AA = Dt_A(AA, control_tagent_vectors, u, delta_t);  // 计算 Dt_A 矩阵    return parent_Ts_control_point.matrix() *           ((std::get<0>(Dt_AA) * std::get<1>(AA).matrix() * std::get<2>(AA).matrix()) +            (std::get<0>(AA).matrix() * std::get<1>(Dt_AA) * std::get<2>(AA).matrix()) +            (std::get<0>(AA).matrix() * std::get<1>(AA).matrix() * std::get<2>(Dt_AA)));  // 计算并返回 spline 的导数  }
  static Transformation Dt2_parent_T_spline(      const LieGroup& parent_Ts_control_point,      std::tuple<Tangent, Tangent, Tangent> const& control_tagent_vectors,      double u, double delta_t) {  // 定义静态成员函数 Dt2_parent_T_spline    using Scalar = typename LieGroup::Scalar;  // 定义标量类型    auto AA = A(control_tagent_vectors, u);  // 计算 A 矩阵    auto Dt_AA = Dt_A(AA, control_tagent_vectors, u, delta_t);  // 计算 Dt_A 矩阵    auto Dt2_AA = Dt2_A(AA, Dt_AA, control_tagent_vectors, u, delta_t);  // 计算 Dt2_A 矩阵
    return parent_Ts_control_point.matrix() *           ((std::get<0>(Dt2_AA) * std::get<1>(AA).matrix() * std::get<2>(AA).matrix()) +            (std::get<0>(AA).matrix() * std::get<1>(Dt2_AA) * std::get<2>(AA).matrix()) +            (std::get<0>(AA).matrix() * std::get<1>(AA).matrix() * std::get<2>(Dt2_AA)) +            Scalar(2) * ((std::get<0>(Dt_AA) * std::get<1>(Dt_AA) * std::get<2>(AA).matrix()) +                         (std::get<0>(Dt_AA) * std::get<1>(AA).matrix() * std::get<2>(Dt_AA)) +                         (std::get<0>(AA).matrix() * std::get<1>(Dt_AA) * std::get<2>(Dt_AA))));  // 计算并返回 spline 的二阶导数  }
 private:  static std::tuple<LieGroup, LieGroup, LieGroup> A(      std::tuple<Tangent, Tangent, Tangent> const& control_tagent_vectors,      double u) {  // 定义静态成员函数 A    Eigen::Vector3d B = SplineBasisFunction<double>::B(u);  // 计算基函数 B    return std::make_tuple(        LieGroup::exp(B[0] * std::get<0>(control_tagent_vectors)),        LieGroup::exp(B[1] * std::get<1>(control_tagent_vectors)),        LieGroup::exp(B[2] * std::get<2>(control_tagent_vectors)));  // 计算并返回 A 矩阵  }
  static std::tuple<Transformation, Transformation, Transformation> Dt_A(      std::tuple<LieGroup, LieGroup, LieGroup> const& AA,      const std::tuple<Tangent, Tangent, Tangent>& control_tagent_vectors,      double u, double delta_t) {  // 定义静态成员函数 Dt_A    Eigen::Vector3d Dt_B = SplineBasisFunction<double>::Dt_B(u, delta_t);  // 计算 Dt_B    return std::make_tuple(        Dt_B[0] * std::get<0>(AA).matrix() * LieGroup::hat(std::get<0>(control_tagent_vectors)),        Dt_B[1] * std::get<1>(AA).matrix() * LieGroup::hat(std::get<1>(control_tagent_vectors)),        Dt_B[2] * std::get<2>(AA).matrix() * LieGroup::hat(std::get<2>(control_tagent_vectors)));  // 计算并返回 Dt_A 矩阵  }
  static std::tuple<Transformation, Transformation, Transformation> Dt2_A(      std::tuple<LieGroup, LieGroup, LieGroup> const& AA,      std::tuple<Transformation, Transformation, Transformation> const& Dt_AA,      std::tuple<Tangent, Tangent, Tangent> const& control_tagent_vectors,      double u, double delta_t) {  // 定义静态成员函数 Dt2_A    Eigen::Vector3d Dt_B = SplineBasisFunction<double>::Dt_B(u, delta_t);  // 计算 Dt_B    Eigen::Vector3d Dt2_B = SplineBasisFunction<double>::Dt2_B(u, delta_t);  // 计算 Dt2_B
    return std::make_tuple(        (Dt_B[0] * std::get<0>(Dt_AA).matrix() +         Dt2_B[0] * std::get<0>(AA).matrix()) * LieGroup::hat(std::get<0>(control_tagent_vectors)),        (Dt_B[1] * std::get<1>(Dt_AA).matrix() +         Dt2_B[1] * std::get<1>(AA).matrix()) * LieGroup::hat(std::get<1>(control_tagent_vectors)),        (Dt_B[2] * std::get<2>(Dt_AA).matrix() +         Dt2_B[2] * std::get<2>(AA).matrix()) * LieGroup::hat(std::get<2>(control_tagent_vectors)));  // 计算并返回 Dt2_A 矩阵  }    enum class SegmentCase { first, normal, last };  // 枚举类型 SegmentCase 定义了三种状态：first（第一段），normal（正常段），last（最后一段）
template <class LieGroup>struct BasisSplineSegment {  // 模板结构体 BasisSplineSegment public:  using T = typename LieGroup::Scalar;  // 定义标量类型别名 T  using Transformation = typename LieGroup::Transformation;  // 定义变换矩阵类型别名
  BasisSplineSegment(SegmentCase segment_case, T const* const raw_ptr0,                     T const* const raw_ptr1, T const* const raw_ptr2,                     T const* const raw_ptr3)  // 构造函数，接受段类型和四个指针参数      : segment_case_(segment_case),  // 初始化段类型        raw_params0_(raw_ptr0),  // 初始化指针参数        raw_params1_(raw_ptr1),        raw_params2_(raw_ptr2),        raw_params3_(raw_ptr3) {}
  Eigen::Map<LieGroup const> const world_pose_foo_prev() const {  // 返回第一个指针指向的 LieGroup 对象    return Eigen::Map<LieGroup const>(raw_params0_);  }  Eigen::Map<LieGroup const> const world_pose_foo_0() const {  // 返回第二个指针指向的 LieGroup 对象    return Eigen::Map<LieGroup const>(raw_params1_);  }
  Eigen::Map<LieGroup const> const world_pose_foo_1() const {  // 返回第三个指针指向的 LieGroup 对象    return Eigen::Map<LieGroup const>(raw_params2_);  }
  Eigen::Map<LieGroup const> const world_pose_foo_2() const {  // 返回第四个指针指向的 LieGroup 对象    return Eigen::Map<LieGroup const>(raw_params3_);  }  LieGroup parent_T_spline(double u) {  // 计算父节点的样条曲线    switch (segment_case_) {      case SegmentCase::first:  // 如果是第一段        return BasisSplineFn<LieGroup>::parent_T_spline(            world_pose_foo_0(),            std::make_tuple(                (world_pose_foo_0().inverse() * world_pose_foo_0()).log(),                (world_pose_foo_0().inverse() * world_pose_foo_1()).log(),                (world_pose_foo_1().inverse() * world_pose_foo_2()).log()),            u);      case SegmentCase::normal:  // 如果是正常段        return BasisSplineFn<LieGroup>::parent_T_spline(            world_pose_foo_prev(),            std::make_tuple(                (world_pose_foo_prev().inverse() * world_pose_foo_0()).log(),                (world_pose_foo_0().inverse() * world_pose_foo_1()).log(),                (world_pose_foo_1().inverse() * world_pose_foo_2()).log()),            u);      case SegmentCase::last:  // 如果是最后一段        return BasisSplineFn<LieGroup>::parent_T_spline(            world_pose_foo_prev(),            std::make_tuple(                (world_pose_foo_prev().inverse() * world_pose_foo_0()).log(),                (world_pose_foo_0().inverse() * world_pose_foo_1()).log(),                (world_pose_foo_1().inverse() * world_pose_foo_1()).log()),            u);    }    SOPHUS_ENSURE(false, "logic error");  // 逻辑错误处理  }
  Transformation Dt_parent_T_spline(double u, double delta_t) {  // 计算样条曲线的一阶导数    switch (segment_case_) {      case SegmentCase::first:  // 如果是第一段        return BasisSplineFn<LieGroup>::Dt_parent_T_spline(            world_pose_foo_0(),            std::make_tuple(                (world_pose_foo_0().inverse() * world_pose_foo_0()).log(),                (world_pose_foo_0().inverse() * world_pose_foo_1()).log(),                (world_pose_foo_1().inverse() * world_pose_foo_2()).log()),            u, delta_t);      case SegmentCase::normal:  // 如果是正常段        return BasisSplineFn<LieGroup>::Dt_parent_T_spline(            world_pose_foo_prev(),            std::make_tuple(                (world_pose_foo_prev().inverse() * world_pose_foo_0()).log(),                (world_pose_foo_0().inverse() * world_pose_foo_1()).log(),                (world_pose_foo_1().inverse() * world_pose_foo_2()).log()),            u, delta_t);      case SegmentCase::last:  // 如果是最后一段        return BasisSplineFn<LieGroup>::Dt_parent_T_spline(            world_pose_foo_prev(),            std::make_tuple(                (world_pose_foo_prev().inverse() * world_pose_foo_0()).log(),                (world_pose_foo_0().inverse() * world_pose_foo_1()).log(),                (world_pose_foo_1().inverse() * world_pose_foo_1()).log()),            u, delta_t);    }    SOPHUS_ENSURE(false, "logic error");  // 逻辑错误处理  }
  Transformation Dt2_parent_T_spline(double u, double delta_t) {  // 计算样条曲线的二阶导数    switch (segment_case_) {      case SegmentCase::first:  // 如果是第一段        return BasisSplineFn<LieGroup>::Dt2_parent_T_spline(            world_pose_foo_0(),            std::make_tuple(                (world_pose_foo_0().inverse() * world_pose_foo_0()).log(),                (world_pose_foo_0().inverse() * world_pose_foo_1()).log(),                (world_pose_foo_1().inverse() * world_pose_foo_2()).log()),            u, delta_t);      case SegmentCase::normal:  // 如果是正常段        return BasisSplineFn<LieGroup>::Dt2_parent_T_spline(            world_pose_foo_prev(),            std::make_tuple(                (world_pose_foo_prev().inverse() * world_pose_foo_0()).log(),                (world_pose_foo_0().inverse() * world_pose_foo_1()).log(),                (world_pose_foo_1().inverse() * world_pose_foo_2()).log()),            u, delta_t);      case SegmentCase::last:  // 如果是最后一段        return BasisSplineFn<LieGroup>::Dt2_parent_T_spline(            world_pose_foo_prev(),            std::make_tuple(                (world_pose_foo_prev().inverse() * world_pose_foo_0()).log(),                (world_pose_foo_0().inverse() * world_pose_foo_1()).log(),                (world_pose_foo_1().inverse() * world_pose_foo_1()).log()),            u, delta_t);    }    SOPHUS_ENSURE(false, "logic error");  // 逻辑错误处理  }
 private:  SegmentCase segment_case_;  // 段的类型  T const* raw_params0_;  // 原始参数指针 0  T const* raw_params1_;  // 原始参数指针 1  T const* raw_params2_;  // 原始参数指针 2  T const* raw_params3_;  // 原始参数指针 3};
template <class LieGroup_>class BasisSplineImpl {  // 定义模板类 BasisSplineImpl public:  using LieGroup = LieGroup_;  // 定义 LieGroup 类型别名  using Scalar = typename LieGroup::Scalar;  // 定义标量类型别名  using Transformation = typename LieGroup::Transformation;  // 定义变换矩阵类型别名  using Tangent = typename LieGroup::Tangent;  // 定义切向量类型别名
  BasisSplineImpl(const std::vector<LieGroup>& parent_Ts_control_point,                  double delta_t)  // 构造函数，接受控制点和时间间隔      : parent_Ts_control_point_(parent_Ts_control_point), delta_t_(delta_t) {    SOPHUS_ENSURE(parent_Ts_control_point_.size() >= 2u, ", but {}",                  parent_Ts_control_point_.size());  // 确保控制点数量不少于2个  }
  LieGroup parent_T_spline(int i, double u) const {  // 计算样条曲线    SOPHUS_ENSURE(i >= 0, "i = {}", i);  // 确保 i 大于等于0    SOPHUS_ENSURE(i < this->getNumSegments(),                  "i = {};  this->getNumSegments() = {};  "                  "parent_Ts_control_point_.size() = {}",                  i, this->getNumSegments(), parent_Ts_control_point_.size());  // 确保 i 小于段数
    SegmentCase segment_case =        i == 0 ? SegmentCase::first               : (i == this->getNumSegments() - 1 ? SegmentCase::last                                                  : SegmentCase::normal);  // 确定段的类型
    int idx_prev = std::max(0, i - 1);  // 确定前一个索引    int idx_0 = i;  // 当前索引    int idx_1 = i + 1;  // 下一个索引    int idx_2 = std::min(i + 2, int(this->parent_Ts_control_point_.size()) - 1);  // 再下一个索引
    return BasisSplineSegment<LieGroup>(               segment_case, parent_Ts_control_point_[idx_prev].data(),               parent_Ts_control_point_[idx_0].data(),               parent_Ts_control_point_[idx_1].data(),               parent_Ts_control_point_[idx_2].data())        .parent_T_spline(u);  // 返回样条曲线  }
  Transformation Dt_parent_T_spline(int i, double u) const {  // 计算样条曲线的一阶导数    SOPHUS_ENSURE(i >= 0, "i = {}", i);  // 确保 i 大于等于0    SOPHUS_ENSURE(i < this->getNumSegments(),                  "i = {};  this->getNumSegments() = {};  "                  "parent_Ts_control_point_.size() = {}",                  i, this->getNumSegments(), parent_Ts_control_point_.size());  // 确保 i 小于段数
    SegmentCase segment_case =        i == 0 ? SegmentCase::first               : (i == this->getNumSegments() - 1 ? SegmentCase::last                                                  : SegmentCase::normal);  // 确定段的类型
    int idx_prev = std::max(0, i - 1);  // 确定前一个索引    int idx_0 = i;  // 当前索引    int idx_1 = i + 1;  // 下一个索引    int idx_2 = std::min(i + 2, int(this->parent_Ts_control_point_.size()) - 1);  // 再下一个索引
    return BasisSplineSegment<LieGroup>(               segment_case, parent_Ts_control_point_[idx_prev].data(),               parent_Ts_control_point_[idx_0].data(),               parent_Ts_control_point_[idx_1].data(),               parent_Ts_control_point_[idx_2].data())        .Dt_parent_T_spline(u, delta_t_);  // 返回样条曲线的一阶导数  }
  Transformation Dt2_parent_T_spline(int i, double u) const {  // 计算样条曲线的二阶导数    SOPHUS_ENSURE(i >= 0, "i = {}", i);  // 确保 i 大于等于0    SOPHUS_ENSURE(i < this->getNumSegments(),                  "i = {};  this->getNumSegments() = {};  "                  "parent_Ts_control_point_.size() = {}",                  i, this->getNumSegments(), parent_Ts_control_point_.size());  // 确保 i 小于段数
    SegmentCase segment_case =        i == 0 ? SegmentCase::first               : (i == this->getNumSegments() - 1 ? SegmentCase::last                                                  : SegmentCase::normal);  // 确定段的类型
    int idx_prev = std::max(0, i - 1);  // 确定前一个索引    int idx_0 = i;  // 当前索引    int idx_1 = i + 1;  // 下一个索引    int idx_2 = std::min(i + 2, int(this->parent_Ts_control_point_.size()) - 1);  // 再下一个索引
    return BasisSplineSegment<LieGroup>(               segment_case, parent_Ts_control_point_[idx_prev].data(),               parent_Ts_control_point_[idx_0].data(),               parent_Ts_control_point_[idx_1].data(),               parent_Ts_control_point_[idx_2].data())        .Dt2_parent_T_spline(u, delta_t_);  // 返回样条曲线的二阶导数  }
  const std::vector<LieGroup>& parent_Ts_control_point() const {  // 获取控制点的常量引用    return parent_Ts_control_point_;  }
  std::vector<LieGroup>& parent_Ts_control_point() {  // 获取控制点的引用    return parent_Ts_control_point_;  }
  int getNumSegments() const {  // 获取段数    return int(parent_Ts_control_point_.size()) - 1;  }
  double delta_t() const { return delta_t_; }  // 获取时间间隔
 private:  std::vector<LieGroup> parent_Ts_control_point_;  // 控制点向量  double delta_t_;  // 时间间隔};
struct IndexAndU {  // 定义结构体 IndexAndU  int i;  // 段索引  double u;  // 参数 u};
template <class LieGroup_>class BasisSpline {  // 定义模板类 BasisSpline public:  using LieGroup = LieGroup_;  // 定义 LieGroup 类型别名  using Scalar = typename LieGroup::Scalar;  // 定义标量类型别名  using Transformation = typename LieGroup::Transformation;  // 定义变换矩阵类型别名  using Tangent = typename LieGroup::Tangent;  // 定义切向量类型别名
  BasisSpline(std::vector<LieGroup> parent_Ts_control_point, double t0,              double delta_t)  // 构造函数，接受控制点、起始时间 t0 和时间间隔 delta_t      : impl_(std::move(parent_Ts_control_point), delta_t), t0_(t0) {}
  LieGroup parent_T_spline(double t) const {  // 计算样条曲线    IndexAndU index_and_u = this->index_and_u(t);  // 获取索引和参数 u
    return impl_.parent_T_spline(index_and_u.i, index_and_u.u);  // 返回样条曲线  }
  Transformation Dt_parent_T_spline(double t) const {  // 计算样条曲线的一阶导数    IndexAndU index_and_u = this->index_and_u(t);  // 获取索引和参数 u    return impl_.Dt_parent_T_spline(index_and_u.i, index_and_u.u);  // 返回样条曲线的一阶导数  }
  Transformation Dt2_parent_T_spline(double t) const {  // 计算样条曲线的二阶导数    IndexAndU index_and_u = this->index_and_u(t);  // 获取索引和参数 u    return impl_.Dt2_parent_T_spline(index_and_u.i, index_and_u.u);  // 返回样条曲线的二阶导数  }
  double t0() const { return t0_; }  // 返回起始时间 t0
  double tmax() const { return t0_ + impl_.delta_t() * getNumSegments(); }  // 返回最大时间 tmax
  const std::vector<LieGroup>& parent_Ts_control_point() const {  // 获取控制点的常量引用    return impl_.parent_Ts_control_point();  }
  std::vector<LieGroup>& parent_Ts_control_point() {  // 获取控制点的引用    return impl_.parent_Ts_control_point();  }
  int getNumSegments() const { return impl_.getNumSegments(); }  // 获取段数
  double s(double t) const { return (t - t0_) / impl_.delta_t(); }  // 计算归一化时间 s
  double delta_t() const { return impl_.delta_t(); }  // 返回时间间隔
  IndexAndU index_and_u(double t) const {  // 计算索引和参数 u    SOPHUS_ENSURE(t >= t0_, "{} vs. {}", t, t0_);  // 确保 t 大于等于 t0    SOPHUS_ENSURE(t <= this->tmax(), "{} vs. {}", t, this->tmax());  // 确保 t 小于等于 tmax
    double s = this->s(t);  // 计算归一化时间 s    double i;    IndexAndU index_and_u;    index_and_u.u = std::modf(s, &i);  // 分离整数部分和小数部分    index_and_u.i = int(i);    if (index_and_u.u > Sophus::Constants<double>::epsilon()) {  // 判断小数部分是否大于精度阈值      return index_and_u;    }
    // u ~=~ 0.0    if (t < 0.5 * this->tmax()) {      // 样条曲线的前半段，保持为 (i, 0.0)      return index_and_u;    }    // 样条曲线的后半段，使用 (i-1, 1.0) 来表示 t == tmax（而不是 t < tmax）    index_and_u.u += 1.0;    --index_and_u.i;
    return index_and_u;  }
 private:  BasisSplineImpl<LieGroup> impl_;  // 样条曲线的实现
  double t0_;  // 起始时间};
}  // namespace Sophus
视觉SLAM十四讲-第四讲补充，运行例程代码错误解决trajectoryError.cpp:5:10: fatal error: sophus/se3.hpp: 没有那个文件或目录
qq_30708051的博客
08-15 1859
我们使用高翔老师的github仓库中Sophus代码编译后即可（无需安装），看了一些书上的内容和一些博客，说是Sophus库分为模板库和非模板库。发现确少头文件，但是两个cpp代码中都引用了这个头文件，编译却发现只有这个缺少头文件，而且我们也安装了带模板的Sophus库。我们按照github上下载的代码进行编译，那么安装的应该是带模板的Sophus。然后查看example中的CMakeList文件（未补充修改）也就是说后续出现的问题，与我们安装的Sophus无关！验证了我们Sophus是带模板的！.....
【c++】Eigen::Isometry3f 位姿与 Sophus::SE3f 位姿互相转换
我才是一卓的博客
07-18 920
【代码】【c++】Eigen::Isometry3f 位姿与 Sophus::SE3f 位姿互相转换。
探索曲线之美：cpp-spline，C++中的曲线插值神器
gitblog_00069的博客
06-23 639
探索曲线之美：cpp-spline，C++中的曲线插值神器 项目地址:https://gitcode.com/gh_mirrors/cp/cpp-spline 在数字图形处理、物理仿真、机械设计等领域，平滑流畅的曲线始终占据着核心地位。今天，我们要向您隆重推介一个开源宝藏——cpp-spline，它以简洁优雅的C++代码，实现了贝塞尔曲线（Bezier）、B样条（B-Spline）和卡特尔-罗姆（...
【亲测免费】 C++ Spline 插值库教程
gitblog_00158的博客
09-01 628
C++ Spline 插值库教程 项目介绍 cpp-spline 是一个提供 C++ 实现的样条插值库。该库支持多种类型的样条曲线，包括贝塞尔曲线（Bezier Curve）、B样条（B-Spline）和Catmull-Rom样条。样条插值在图形学、动画路径生成和数据平滑处理等领域有广泛应用。 项目快速启动 安装 首先，克隆项目仓库到本地： git clone https://github.com...
旋转矩阵、欧拉角、四元数及四元数插值
weixin_41469272的博客
08-20 1万+
在三维世界中的旋转的表示的方法主要有： 旋转只是3个自由度未知量。 3*3旋转矩阵 3维坐标系经过旋转，得到新的坐标系，新坐标系三轴在旧坐标系下的表示构成的3*3矩阵，就是能够表述旋转的旋转矩阵。 但是3*3矩阵用来表述3个自由度的旋转时，变量产生冗余，因此旋转矩阵及其还要满足一系列其他的性质。且表示也不够直观 角轴 任何旋转都可以看成，围绕空间某个轴旋转一定的角度产生的，角轴对应旋转轴的方向，其模长可以用来关联旋转的角度。 欧拉角 欧拉角它使用了三个分离的转角，把一个旋转分解成三次绕不同轴的旋转。比较清
视觉SLAM十四讲第四讲
weixin_40486179的博客
10-14 444
第四章 李群与李代数 主要目标 理解李群与李代数的概念，掌握 SO(3), SE(3) 与对应李代数的表示方式。 理解 BCH 近似的意义。 学会在李代数上的扰动模型。 使用 Sophus 对李代数进行运算。 旋转矩阵自身是带有约束的（正交且行列式为 1）。它们作为优化变量时，会引入额外的约束，使优化变得困难。通过李群—李代数间的转换关系，我们希望把位姿估计变成无约束的优化问题，简化求解方式。 一、李群与李代数基础 说三维旋转矩阵构成了特殊正交群 SO(3)，而变换矩阵构成了特殊欧氏群 SE(3)。它们写
李群、李代数基本知识
咩吃咩
05-08 763
李群、李代数基本知识及Sophus用法一级目录二级目录三级目录 在学习过程中，逐渐接触到李群、李代数的有关知识，代码中也有所出现。但当前用法较多的是用于旋转的表示，进行一下记录。 一级目录 二级目录 三级目录 ...
《SLAM十四讲》中Sophus库的编译问题
01-20
4. 如果遇到源代码级别的问题，可以查阅Sophus库的官方文档或者在相关论坛上寻找解决方案，以确保改动不会破坏库的功能。 总的来说，学习SLAM过程中遇到的编译问题需要耐心和细心地排查，理解编译错误的含义以及源...
fatal error: sophus/se3.hpp: 没有那个文件或目录 29 | #include <sophus/se3.hpp>
03-15
### 编译时无法找到 `sophus/se3.hpp` 文件的解决方案 当遇到编译错误提示 `fatal error: sophus/se3.hpp: 没有那个文件或目录` 时，通常是因为编译器未能正确定位到所需的头文件路径。以下是几种可能的解决办法： ...
/home/louie/Code/learn-code/ch7/pose_estimation_3d2d.cpp:14:10: fatal error: sophus/se3.hpp: 没有那个文件或目录 14 | #include <sophus/se3.hpp>
最新发布
09-05
但在新版本（如>1.0.0）中，Sophus库已经重构，原来的se3.hpp被替换为se3.hpp和so3.hpp可能被整合到了lie_group.hpp等文件中。 因此，如果使用的是新版本的Sophus，可能需要修改代码中的包含路径。例如，改为： `...
基于单位四元数机器人姿态插补算法
02-20
基于单位四元数机器人姿态插补算法，孔民秀，季晨，机器人运动轨迹插补算法是决定机器人性能的核心技术之一。在曲面、曲线加工，喷涂、弧焊等领域，机器人的轨迹插补，尤其是姿态插
Spline曲线(穿过控制点CC++版本)
10-30
之前实现了不通过控制点生成Bezier曲线.. 本次实现了通过控制点生成曲线..(C/C++版本)..
三次样条插值函数c++实现
12-05
将三次样条函数封装成了一个CSpline类。实现了样条函数I型边界条件和II型边界条件。
T-SPLINE:C++ 中的 T-spline 面向对象包-开源
06-06
T-spline 是一种定义自由曲面的新方法，其控制点比 NURBS 少，并且能够使用单个曲面表示模型而不会出现连接错误。 而T-spline数据模型的复杂性导致其编程困难重重，阻碍了T-spline技术的研究与发展。 在这里，T-SPLINE 内核给出了一组重新设计的数据模型，这些模型对人和计算机的理解都更加方便。 有关更多详细信息，请参阅我们的文章：“使用 STEP 重新考虑 T-spline 数据模型及其交换”
B样条曲线平滑C++程序
12-07
本程序实现二次B样条平滑，三次B样条平滑；二次B样条平滑后节点间均匀插值
【亲测免费】 探索cpp-spline：高效C++样条插值库
gitblog_00982的博客
08-30 1145
探索cpp-spline：高效C++样条插值库 项目介绍 cpp-spline 是一个强大的C++样条插值库，提供了三种常用的曲线插值方法：贝塞尔曲线（Bezier Curve）、B样条曲线（B-Spline）和Catmull-Rom样条曲线。这个库旨在为开发者提供一个简单易用且功能丰富的工具，以便在2D或3D环境中进行平滑曲线生成和插值计算。 项目技术分析 cpp-spline 的核心优势在于其...
三次样条(Cubic Spline)的C++实现以及可视化
热门推荐
12-16 1万+
无论是曲线拟合，能量优化还是分段函数模拟的应用中，通过一组离散点拟合出一条完整的曲线，都是不可避免的工作。一般来说，像贝塞尔曲线，b样条等曲线拟合方法，是通过控制点来生成曲线，控制点本身不经过曲线，这就带来一些不便。我们希望使用一种控制点在曲线上，同时保证曲线光滑，拟合结果良好方法，来实现曲线拟合。这就引出了我们今天的主题：三次样条曲线。
Spline(样条)曲线(穿过控制点CC++版本)
verystory的博客
10-30 4457
之前实现了不通过控制点生成Bezier曲线.. 本次实现了通过控制点生成曲线..(C/C++版本)
【C++】sophus : interpolate_details & interpolate 两李群元素之间平滑插值 （十一）
cxyhjl的博客
12-15 574
interpolate_details这段代码为Sophus库的插值模块提供了一组工具，用于处理各种 Lie 群（如 SO(2)、SE(3)、Sim(3) 等）的特性查询，特别是关于路径歧义的检查。这些工具定义在interp_details命名空间中，通过模板化设计支持多种类型。功能和用途支持查询：检查特定类型（如 SO(2)、SE(3) 等）是否支持插值或其他操作。提供静态布尔值su...
评论 1
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
红包 添加红包
表情包 插入表情
表情包 代码片
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值