【C++】sophus : interpolate_details & interpolate 两李群元素之间平滑插值（十一）

最新推荐文章于 2025-04-12 14:18:17 发布

原创最新推荐文章于 2025-04-12 14:18:17 发布 · 574 阅读

CC 4.0 BY-SA版权

interpolate_details

这段代码为 Sophus 库的插值模块提供了一组工具，用于处理各种 Lie 群（如 SO(2)、SE(3)、Sim(3) 等）的特性查询，特别是关于路径歧义的检查。这些工具定义在 interp_details 命名空间中，通过模板化设计支持多种类型。

功能和用途

支持查询：

检查特定类型（如 SO(2)、SE(3) 等）是否支持插值或其他操作。
提供静态布尔值 supported 标志表示支持性。

路径歧义检查：

检测在插值过程中是否存在“最短路径”的歧义问题。
使用群元素的对数（log）来分析旋转角度是否接近 ππ（即方向翻转的特殊情况）。

核心设计

Traits 模板结构：

模板类 Traits<Group> 为不同群类型定义了特化实现。
提供两个主要接口：
- supported
  ：指明该群类型是否支持特性查询。
- hasShortestPathAmbiguity(const Group&)
  ：检查给定群元素是否存在路径歧义。

路径歧义的判定逻辑：

对于旋转相关群（如 SO(2)、SO(3) 等），通过计算对数映射的角度，并检测其是否接近 ππ 来判断。
判断公式：
对非旋转群（如笛卡尔群 Cartesian），路径歧义不存在，直接返回 false。

特性与实现

代码结构总结

Traits 模板类：

通过模板特化为不同群类型定义支持性和路径歧义逻辑。

静态接口：

supported
：是否支持路径歧义检查。
hasShortestPathAmbiguity
：检查特定群元素是否存在歧义。

递归委托：

RxSO 和 SE 群类型将路径歧义检测委托给对应的旋转群（SO(2) 或 SO(3)）。

优点

模块化设计：

每个群类型的逻辑清晰，便于扩展和维护。

复用性高：

高度复用旋转群的歧义检查逻辑（如 RxSO 群和 SE 群）。

效率与精度：

使用精确的数学公式检测路径歧义，结合机器精度避免误判。

应用场景

插值与路径规划：

在 Lie 群上进行插值时，确保路径选择正确无歧义。

运动学与控制：

检查旋转群的插值路径，避免意外的翻转。

优化问题：

确保在非线性优化中，旋转路径的选择符合最短路径原则。

示例

检查 SO(2) 群的路径歧义：

Sophus::SO2<double> foo_T_bar = Sophus::SO2<double>::exp(M_PI); // 旋转 180 度bool ambiguity = Sophus::interp_details::Traits<Sophus::SO2<double>>::hasShortestPathAmbiguity(foo_T_bar);// ambiguity == true

检查 SE(3) 群的路径歧义：

Sophus::SE3<double> foo_T_bar = Sophus::SE3<double>::rot(Sophus::SO3<double>::exp(Eigen::Vector3d(0, 0, M_PI)));bool ambiguity = Sophus::interp_details::Traits<Sophus::SE3<double>>::hasShortestPathAmbiguity(foo_T_bar);// ambiguity == true

#pragma once  // 防止头文件被多次包含
#include "cartesian.hpp"  // 引入自定义的cartesian.hpp头文件#include "rxso2.hpp"  // 引入自定义的rxso2.hpp头文件#include "rxso3.hpp"  // 引入自定义的rxso3.hpp头文件#include "se2.hpp"  // 引入自定义的se2.hpp头文件#include "se3.hpp"  // 引入自定义的se3.hpp头文件#include "sim2.hpp"  // 引入自定义的sim2.hpp头文件#include "sim3.hpp"  // 引入自定义的sim3.hpp头文件#include "so2.hpp"  // 引入自定义的so2.hpp头文件#include "so3.hpp"  // 引入自定义的so3.hpp头文件
namespace Sophus {  // 开始命名空间Sophusnamespace interp_details {  // 开始命名空间interp_details
template <class Group>struct Traits;  // 定义模板结构体Traits
template <class Scalar, int Dim>struct Traits<Cartesian<Scalar, Dim>> {  // 对模板结构体Traits进行特化，适用于Cartesian类型  static bool constexpr supported = true;  // 定义静态常量，表示支持该类型
  static bool hasShortestPathAmbiguity(Cartesian<Scalar, Dim> const&) {    return false;  // 确定笛卡尔坐标系不存在最短路径模糊性  }};
template <class Scalar>struct Traits<SO2<Scalar>> {  // 对模板结构体Traits进行特化，适用于SO2类型  static bool constexpr supported = true;  // 定义静态常量，表示支持该类型
  static bool hasShortestPathAmbiguity(SO2<Scalar> const& foo_T_bar) {    using std::abs;  // 使用标准库中的abs函数    Scalar angle = abs(foo_T_bar.log());  // 获取角度的绝对值    Scalar const kPi = Constants<Scalar>::pi();  // 定义常量π    return abs(angle - kPi) / (angle + kPi) < Constants<Scalar>::epsilon();  // 判断是否存在最短路径模糊性  }};
template <class Scalar>struct Traits<RxSO2<Scalar>> {  // 对模板结构体Traits进行特化，适用于RxSO2类型  static bool constexpr supported = true;  // 定义静态常量，表示支持该类型
  static bool hasShortestPathAmbiguity(RxSO2<Scalar> const& foo_T_bar) {    return Traits<SO2<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so2());  // 调用SO2类型的方法进行判断  }};
template <class Scalar>struct Traits<SO3<Scalar>> {  // 对模板结构体Traits进行特化，适用于SO3类型  static bool constexpr supported = true;  // 定义静态常量，表示支持该类型
  static bool hasShortestPathAmbiguity(SO3<Scalar> const& foo_T_bar) {    using std::abs;  // 使用标准库中的abs函数    Scalar angle = abs(foo_T_bar.logAndTheta().theta);  // 获取角度的绝对值    Scalar const kPi = Constants<Scalar>::pi();  // 定义常量π    return abs(angle - kPi) / (angle + kPi) < Constants<Scalar>::epsilon();  // 判断是否存在最短路径模糊性  }};
template <class Scalar>struct Traits<RxSO3<Scalar>> {  // 对模板结构体Traits进行特化，适用于RxSO3类型  static bool constexpr supported = true;  // 定义静态常量，表示支持该类型
  static bool hasShortestPathAmbiguity(RxSO3<Scalar> const& foo_T_bar) {    return Traits<SO3<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so3());  // 调用SO3类型的方法进行判断  }};
template <class Scalar>struct Traits<SE2<Scalar>> {  // 对模板结构体Traits进行特化，适用于SE2类型  static bool constexpr supported = true;  // 定义静态常量，表示支持该类型
  static bool hasShortestPathAmbiguity(SE2<Scalar> const& foo_T_bar) {    return Traits<SO2<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so2());  // 调用SO2类型的方法进行判断  }};
template <class Scalar>struct Traits<SE3<Scalar>> {  // 对模板结构体Traits进行特化，适用于SE3类型  static bool constexpr supported = true;  // 定义静态常量，表示支持该类型
  static bool hasShortestPathAmbiguity(SE3<Scalar> const& foo_T_bar) {    return Traits<SO3<Scalar>>::hasShortestPathAmbiguity(foo_T_bar.so3());  // 调用SO3类型的方法进行判断  }};
template <class Scalar>struct Traits<Sim2<Scalar>> {  // 对模板结构体Traits进行特化，适用于Sim2类型  static bool constexpr supported = true;  // 定义静态常量，表示支持该类型
  static bool hasShortestPathAmbiguity(Sim2<Scalar> const& foo_T_bar) {    return Traits<SO2<Scalar>>::hasShortestPathAmbiguity(        foo_T_bar.rxso2().so2());  // 调用SO2类型的方法进行判断  }};
template <class Scalar>struct Traits<Sim3<Scalar>> {  // 对模板结构体Traits进行特化，适用于Sim3类型  static bool constexpr supported = true;  // 定义静态常量，表示支持该类型
  static bool hasShortestPathAmbiguity(Sim3<Scalar> const& foo_T_bar) {    return Traits<SO3<Scalar>>::hasShortestPathAmbiguity(        foo_T_bar.rxso3().so3());  // 调用SO3类型的方法进行判断  }};
}  // namespace interp_details}  // namespace Sophus

interpolate

该代码提供了一个在李群（Lie Group）上的插值函数，能够在两个李群元素之间进行平滑插值。

功能描述

函数名称：interpolate
参数：
- foo_T_bar：第一个李群元素。
- foo_T_baz：第二个李群元素。
- p：插值因子，范围为[0, 1]，默认为 0.5。
返回值：一个李群元素foo_T_quiz，表示插值后的结果。
- 当p=0 时，返回foo_T_bar。
- 当p=1 时，返回foo_T_baz。
- 对于其他值，则返回两者之间按比例插值的中间值。

实现细节

插值计算：

利用李群的逆变性质，先计算foo_T_bar.inverse() * foo_T_baz 得到两个李群元素之间的相对变化。
将相对变化映射到李代数（Lie Algebra）进行线性插值：p * log(...)。
使用G::exp 将结果从李代数映射回李群。
最终通过左乘foo_T_bar 将插值结果变换回目标李群坐标。

边界检查：

确保插值因子p 在[0, 1] 范围内，否则抛出错误。

支持的李群类型：

通过interp_details::Traits<G>::supported 进行静态检查，确保李群类型支持插值操作。

用途

该函数适用于在李群表示的几何对象（如旋转、位姿等）之间进行平滑变换，广泛应用于计算机视觉、机器人学等领域中的插值问题。

/// @file  // 文件注释/// 李群的插值。
#pragma once  // 防止头文件被多次包含
#include <Eigen/Eigenvalues>  // 引入用于特征值计算的Eigen库头文件
#include "interpolate_details.hpp"  // 引入自定义的interpolate_details.hpp头文件
namespace Sophus {  // 开始命名空间Sophus
/// 这个函数在两个李群元素``foo_T_bar``和``foo_T_baz``之间进行插值，/// 插值因子为[0, 1]范围内的``alpha``。////// 它返回姿态``foo_T_quiz``，其中``quiz``是``bar``和``baz``之间的一个帧。/// 如果``alpha=0``，它返回``foo_T_bar``。如果是1，它返回``foo_T_baz``。////// （由于李群上的插值具有逆变性，我们可以等效地将输入参数视为``bar_T_foo``、/// ``baz_T_foo``，返回值为``quiz_T_foo``。）////// 前提条件：``p``必须在[0, 1]范围内。///template <class G, class Scalar2 = typename G::Scalar>std::enable_if_t<interp_details::Traits<G>::supported, G> interpolate(    G const& foo_T_bar, G const& foo_T_baz, Scalar2 p = Scalar2(0.5f)) {  using Scalar = typename G::Scalar;  // 获取标量类型  Scalar inter_p(p);  // 将p转换为标量类型  SOPHUS_ENSURE(inter_p >= Scalar(0) && inter_p <= Scalar(1),                "p ({}) must in [0, 1].", (inter_p));  // 确保p在[0, 1]范围内  return foo_T_bar * G::exp(inter_p * (foo_T_bar.inverse() * foo_T_baz).log());  // 进行插值计算并返回结果}
}  // namespace Sophus