【C++】sophus : sim2.hpp 用于二维空间中相似性变换的类和相关操作 (十八)-优快云博客

本文链接：https://blog.youkuaiyun.com/cxyhjl/article/details/144598421

sim2.hpp 是一个定义了相似性变换 Sim(2) 相关类和函数的头文件。以下是它的主要内容和功能的总结：

引入头文件：

包含了rxso2.hpp 和sim_details.hpp 头文件，用于 RxSO2 和相似性变换的实现细节。

命名空间 Sophus：

声明了模板类Sim2，并定义了Sim2d 和Sim2f 为Sim2<double> 和Sim2<float> 的别名。

命名空间 Eigen：

特化了Eigen::Map 模板，用于包裹Sim2 对象，允许对普通数据进行包装。
特化了Eigen::internal::traits 模板，用于描述Sim2 类型的属性，包括标量类型、平移类型和 RxSO2 类型。

Sim2 基类：

Sim2Base 类实现了 Sim2 的基础功能，包括平移、缩放、旋转的变换和计算。
定义了 Sim2 的自由度、参数数量、矩阵维度、点和切向量的类型。
提供了指数映射、对数映射、伴随变换、逆变换等函数。

Sim2 类：

Sim2 类继承自Sim2Base，使用默认存储方式。
定义了构造函数、复制构造函数和从其他类型复制的构造函数。
提供了对内存数据的不安全读写访问。

Sim2 变换相关操作：

提供了对 RxSO2 和平移向量的访问器和修改器。
提供了计算李括号、指数映射、对数映射等函数。
提供了群运算符重载，包括群乘法、点和齐次点的变换操作。

其他工具函数：

提供了hat 算子和vee 算子，进行向量与矩阵之间的变换。
提供了生成无穷小生成元的函数和从 Sim(2) 流形中抽取均匀样本的函数。

总体而言，sim2.hpp 文件定义了一个用于二维空间中相似性变换的类和相关操作，主要包括旋转、缩放和平移的组合。通过这个头文件，用户可以方便地在 C++ 程序中使用和操作 Sim(2) 变换。

/// @file/// Similarity group Sim(2) - scaling, rotation and translation in 2d. /// @文件/// 相似群 Sim(2) - 2D 中的缩放、旋转和平移。
#pragma once// 防止文件被多次包含
#include "rxso2.hpp"// 包含头文件 "rxso2.hpp"
#include "sim_details.hpp"// 包含头文件 "sim_details.hpp"
namespace Sophus {// 定义命名空间 Sophustemplate <class Scalar_, int Options = 0>class Sim2;// 声明模板类 Sim2，带有类型参数 Scalar_ 和整数参数 Options（默认值为0）
using Sim2d = Sim2<double>;// 定义 Sim2d 为 Sim2<double> 的别名
using Sim2f = Sim2<float>;// 定义 Sim2f 为 Sim2<float> 的别名}  // namespace Sophus// 结束命名空间 Sophus
namespace Eigen {// 定义命名空间 Eigennamespace internal {// 定义命名空间 internal
template <class Scalar_, int Options>struct traits<Sophus::Sim2<Scalar_, Options>> {// 特化模板 struct traits，用于 Sophus::Sim2  using Scalar = Scalar_;// 定义类型别名 Scalar，等于模板参数 Scalar_  using TranslationType = Sophus::Vector2<Scalar, Options>;// 定义类型别名 TranslationType，等于 Sophus::Vector2<Scalar, Options>  using RxSO2Type = Sophus::RxSO2<Scalar, Options>;// 定义类型别名 RxSO2Type，等于 Sophus::RxSO2<Scalar, Options>};
template <class Scalar_, int Options>struct traits<Map<Sophus::Sim2<Scalar_>, Options>>    : traits<Sophus::Sim2<Scalar_, Options>> {// 特化模板 struct traits，用于 Map 类型的 Sophus::Sim2// 并继承 traits<Sophus::Sim2<Scalar_, Options>>  using Scalar = Scalar_;// 定义类型别名 Scalar，等于模板参数 Scalar_  using TranslationType = Map<Sophus::Vector2<Scalar>, Options>;// 定义类型别名 TranslationType，等于 Map 类型的 Sophus::Vector2  using RxSO2Type = Map<Sophus::RxSO2<Scalar>, Options>;// 定义类型别名 RxSO2Type，等于 Map 类型的 Sophus::RxSO2};
template <class Scalar_, int Options>struct traits<Map<Sophus::Sim2<Scalar_> const, Options>>    : traits<Sophus::Sim2<Scalar_, Options> const> {// 特化模板 struct traits，用于 Map 类型且 const 的 Sophus::Sim2// 并继承 traits<Sophus::Sim2<Scalar_, Options> const>  using Scalar = Scalar_;// 定义类型别名 Scalar，等于模板参数 Scalar_  using TranslationType = Map<Sophus::Vector2<Scalar> const, Options>;// 定义类型别名 TranslationType，等于 Map 类型且 const 的 Sophus::Vector2  using RxSO2Type = Map<Sophus::RxSO2<Scalar> const, Options>;// 定义类型别名 RxSO2Type，等于 Map 类型且 const 的 Sophus::RxSO2};}  // namespace internal// 结束命名空间 internal}  // namespace Eigen// 结束命名空间 Eigen
namespace Sophus {// 定义命名空间 Sophus
/// Sim2 base type - implements Sim2 class but is storage agnostic./// Sim2 基础类型 - 实现 Sim2 类但与存储无关。
// Sim(2) is the group of rotations  and translation and scaling in 2d. It is/// the semi-direct product of R+xSO(2) and the 2d Euclidean vector space. The/// class is represented using a composition of RxSO2  for scaling plus/// rotation and a 2-vector for translation./// Sim(2) 是在二维空间中旋转、平移和缩放的群。它是 R+xSO(2) 与二维欧几里得向量空间的半直积。/// 该类使用 RxSO2 表示缩放和旋转，并使用二维向量表示平移。
// Sim(2) is neither compact, nor a commutative group./// Sim(2) 既不是紧致的，也不是交换群。
// See RxSO2 for more details of the scaling + rotation representation in 2d./// 有关二维空间中缩放和旋转表示的详细信息，请参见 RxSO2。
///template <class Derived>class Sim2Base {// 声明模板类 Sim2Base
 public:  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;// 定义类型别名 Scalar，等于 Eigen::internal::traits<Derived>::Scalar  using TranslationType =      typename Eigen::internal::traits<Derived>::TranslationType;// 定义类型别名 TranslationType，等于 Eigen::internal::traits<Derived>::TranslationType  using RxSO2Type = typename Eigen::internal::traits<Derived>::RxSO2Type;// 定义类型别名 RxSO2Type，等于 Eigen::internal::traits<Derived>::RxSO2Type
  /// Degrees of freedom of manifold, number of dimensions in tangent space  /// (two for translation, one for rotation and one for scaling).  static int constexpr DoF = 4;  /// 流形的自由度，切空间的维度数量（平移2个，旋转1个，缩放1个）。
  /// Number of internal parameters used (2-tuple for complex number, two for  /// translation).  static int constexpr num_parameters = 4;  /// 使用的内部参数数量（复数为2元组，平移为2）
  /// Group transformations are 3x3 matrices.  static int constexpr N = 3;  /// 群变换为3x3矩阵。
  /// Points are 2-dimensional  static int constexpr Dim = 2;  /// 点为二维。
  using Transformation = Matrix<Scalar, N, N>;// 定义类型别名 Transformation，等于 Matrix<Scalar, N, N>  using Point = Vector2<Scalar>;// 定义类型别名 Point，等于 Vector2<Scalar>  using HomogeneousPoint = Vector3<Scalar>;// 定义类型别名 HomogeneousPoint，等于 Vector3<Scalar>  using Line = ParametrizedLine2<Scalar>;// 定义类型别名 Line，等于 ParametrizedLine2<Scalar>  using Hyperplane = Hyperplane2<Scalar>;// 定义类型别名 Hyperplane，等于 Hyperplane2<Scalar>  using Tangent = Vector<Scalar, DoF>;// 定义类型别名 Tangent，等于 Vector<Scalar, DoF>  using Adjoint = Matrix<Scalar, DoF, DoF>;// 定义类型别名 Adjoint，等于 Matrix<Scalar, DoF, DoF>
  /// For binary operations the return type is determined with the  /// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map  /// types, as well as other compatible scalar types such as Ceres::Jet and  /// double scalars with SIM2 operations.  template <typename OtherDerived>  using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<      Scalar, typename OtherDerived::Scalar>::ReturnType;  /// 对于二元操作，返回类型由 Eigen 的 ScalarBinaryOpTraits 特性决定。  /// 这允许混合具体和 Map 类型，以及其他兼容的标量类型，如 Ceres::Jet 和 double 标量。
  template <typename OtherDerived>  using Sim2Product = Sim2<ReturnScalar<OtherDerived>>;// 定义模板别名 Sim2Product，返回 Sim2<ReturnScalar<OtherDerived>>
  template <typename PointDerived>  using PointProduct = Vector2<ReturnScalar<PointDerived>>;// 定义模板别名 PointProduct，返回 Vector2<ReturnScalar<PointDerived>>
  template <typename HPointDerived>  using HomogeneousPointProduct = Vector3<ReturnScalar<HPointDerived>>;// 定义模板别名 HomogeneousPointProduct，返回 Vector3<ReturnScalar<HPointDerived>>
  /// Adjoint transformation  ///  /// This function return the adjoint transformation ``Ad`` of the group  /// element ``A`` such that for all ``x`` it holds that  /// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below.  ///  /// 伴随变换  ///  /// 此函数返回群元素 ``A`` 的伴随变换 ``Ad``，对于所有 ``x`` 都满足  /// ``hat(Ad_A * x) = A * hat(x) A^{-1}``。详见下文的帽算子。
  SOPHUS_FUNC Adjoint Adj() const {    Adjoint res;    res.setZero();    res.template block<2, 2>(0, 0) = rxso2().matrix();    res(0, 2) = translation()[1];    res(1, 2) = -translation()[0];    res.template block<2, 1>(0, 3) = -translation();
    res(2, 2) = Scalar(1);    // 设置伴随矩阵 res 的 (2, 2) 元素为 1
    res(3, 3) = Scalar(1);    // 设置伴随矩阵 res 的 (3, 3) 元素为 1    return res;    // 返回计算出的伴随矩阵 res  }
  /// Returns copy of instance casted to NewScalarType.  /// 返回实例的副本，并将其转换为 NewScalarType 类型。  template <class NewScalarType>  SOPHUS_FUNC Sim2<NewScalarType> cast() const {    return Sim2<NewScalarType>(rxso2().template cast<NewScalarType>(),                               translation().template cast<NewScalarType>());    // 返回一个新的 Sim2<NewScalarType> 实例，并将内部的 rxso2 和 translation 成员转换为 NewScalarType 类型。  }
  /// Returns group inverse.  /// 返回群的逆元素。  SOPHUS_FUNC Sim2<Scalar> inverse() const {    RxSO2<Scalar> invR = rxso2().inverse();    // 计算 rxso2() 的逆，并存储在 inv