【C++】sophus : se2.hpp 提供了SE(2)群的数学操作和Lie群的基本操作 (十五)

十年一梦实验室2024-12-21 12:24

这段代码定义了二维特殊欧几里得群 SE(2)，用于表示二维空间中的旋转和平移变换。以下是代码的中文总结：

主要内容:

  • SE(2) 的定义:

    SE(2) 是 SO(2) (二维旋转群) 和二维向量空间的半直积。它描述了二维空间中的刚体变换，即旋转和平移的组合。

  • 类结构:

    代码定义了多个类和模板，以支持不同标量类型（如double 和float）和 Eigen 库的Map 功能。

    • SE2Base<Derived>

      : SE(2) 的基类，实现了 SE(2) 的核心功能，但不涉及具体的存储方式。

    • SE2<Scalar_, int Options>

      : 继承自SE2Base，使用默认的存储方式。这是用户主要使用的类。

    • Eigen::Map<Sophus::SE2<Scalar_>, Options>

      和Eigen::Map<Sophus::SE2<Scalar_> const, Options>: 允许将 SE(2) 对象映射到现有的内存区域（例如，POD 数组），从而避免不必要的内存复制。

  • 数据表示:

    SE(2) 对象由一个 SO(2) 对象（表示旋转）和一个二维向量（表示平移）组成。

  • 主要功能:

    • 群运算:

      实现了 SE(2) 的群乘法 (* 和*=)、逆运算 (inverse())。

    • 群作用:

      实现了 SE(2) 对二维点 (*)、齐次坐标点 (*)、直线 (*) 和超平面 (*) 的作用。

    • 李代数:

      实现了李代数 se(2) 的指数映射 (exp())、对数映射 (log())、hat 算子 (hat())、vee 算子 (vee()) 和李括号 (lieBracket())。

    • 伴随变换:

      提供了计算伴随变换的方法 (Adj())。

    • 其他:

      提供了矩阵表示 (matrix(),matrix2x3())、参数访问 (params())、单位复数访问 (unit_complex())、设置旋转矩阵 (setRotationMatrix())、规范化 (normalize())、类型转换 (cast()) 等方法。

  • 导数计算:

    提供了计算指数映射和相关运算的导数的方法，例如Dx_exp_x()、Dx_exp_x_at_0()、Dx_exp_x_times_point_at_0() 等，这对于优化和数值计算非常有用。

  • 构造函数:

    提供了多种构造函数，方便用户创建 SE(2) 对象，例如从旋转矩阵、旋转角、复数、平移向量等构造。

  • 静态方法:

    提供了一些静态方法，如generator() (生成李代数的基)、fitToSE2() (将任意 3x3 矩阵拟合到 SE(2))、rot() (创建纯旋转变换)、trans() (创建纯平移变换)、sampleUniform() (均匀采样 SE(2) 流形)。

  • Eigen 库集成:

    通过使用 Eigen 库的traits 模板，代码与 Eigen 库很好地集成，支持 Eigen 的矩阵和向量类型，以及Map 功能。

总结:

这段代码提供了一个完整且高效的 SE(2) 实现，支持多种操作和数据类型，并与 Eigen 库集成。它特别关注了数值计算的需要，提供了丰富的导数计算方法，适用于机器人学、计算机视觉、图形学等领域。

代码功能和作用的简要描述

这段代码是Sophus库中针对SE(2)（特殊欧氏群，表示二维平面上的刚体变换，包括旋转和平移）的实现。它主要提供了SE(2)群的数学操作和Lie群的基本操作，比如指数映射、对数映射、Lie括号运算，以及与矩阵的相互转换。以下是代码的主要部分及其功能概述：

主要功能说明

  1. 构造函数

  • 从3×3矩阵构造SE(2)，假设输入矩阵是正交的，行列式为1，且最后一行是 (0, 0, 1)。

  • 支持从旋转部分和平移部分分别构造SE(2)。

数据访问和修改

  • 提供 data() 方法，允许直接访问SE(2)的底层数据存储（旋转部分以复数表示，后接平移向量）。

  • 提供了访问或修改 so2()（表示旋转的SO(2)部分）和 translation()（表示平移部分）的接口。

SE(2)群的导数计算

  • 提供了指数映射 exp(x) 的导数，以及特定点上的导数计算。

  • 提供了在零点附近的导数简化形式，比如 Dx_exp_x_at_0() 和 Dx_exp_x_times_point_at_0(point)。

Lie群和Lie代数

  • 实现了SE(2)的Lie群操作，比如指数映射 exp() 和对数映射 log()。

  • 定义了Lie代数的hat操作（将向量形式的李代数转换为矩阵形式）和vee操作（从矩阵形式转换回向量形式）。

  • 提供了Lie括号计算 lieBracket()，即两个李代数元素的组合。

生成器

  • 提供了SE(2)的生成器 generator(i)，根据索引i生成对应的无穷小生成元矩阵。

  • 生成器定义为：

    go 复制代码 
    G_0 = |  0  0  1 |   G_1 = |  0  0  0 |   G_2 = |  0 -1  0 |      |  0  0  0 |         |  0  0  1 |         |  1  0  0 |      |  0  0  0 |         |  0  0  0 |         |  0  0  0 |

特殊构造器

  • 提供了纯旋转（rot()）、纯平移（trans()、transX() 和 transY()）的构造函数。

  • 提供了一个从随机分布中采样的SE(2)实例构造函数 sampleUniform()。

矩阵投影

  • 提供了方法 fitToSE2()，将任意3×3矩阵投影到SE(2)上，确保生成有效的刚体变换。

Eigen的Map支持

  • 对Eigen库的Map进行了特化，支持将SE(2)对象映射到底层数据数组，方便与Eigen矩阵操作兼容。

  • 提供了可变（Map<Sophus::SE2>）和不可变（Map<Sophus::SE2 const>）的版本。

安全性和布局保证

  • 使用了静态断言确保类布局符合预期，例如数据存储是紧密排列的（无填充）。

代码的核心思想

这段代码的核心思想是通过Sophus库实现SE(2)群及其Lie代数的基本运算和性质。这种实现提供了强大的工具，可以用于机器人学、计算机视觉或其他需要处理刚体变换的领域，确保数学操作既高效又符合理论定义。

cs 复制代码 
/// @file/// Special Euclidean group SE(2) - rotation and translation in 2d.
#pragma once  // 防止头文件被多次包含
#include "so2.hpp"  // 包含 SO2 类的头文件
namespace Sophus {template <class Scalar_, int Options = 0>class SE2;  // 声明一个模板类 SE2using SE2d = SE2<double>;  // 定义 SE2<double> 类型的别名 SE2dusing SE2f = SE2<float>;  // 定义 SE2<float> 类型的别名 SE2f}  // namespace Sophus
namespace Eigen {namespace internal {
template <class Scalar_, int Options>struct traits<Sophus::SE2<Scalar_, Options>> {  // traits 模板结构体，用于定义 SE2 类的特性  using Scalar = Scalar_;  // 定义标量类型  using TranslationType = Sophus::Vector2<Scalar, Options>;  // 定义平移类型  using SO2Type = Sophus::SO2<Scalar, Options>;  // 定义旋转类型};
template <class Scalar_, int Options>struct traits<Map<Sophus::SE2<Scalar_>, Options>>    : traits<Sophus::SE2<Scalar_, Options>> {  // 为 Map 类型的 SE2 特化 traits  using Scalar = Scalar_;  using TranslationType = Map<Sophus::Vector2<Scalar>, Options>;  using SO2Type = Map<Sophus::SO2<Scalar>, Options>;};
template <class Scalar_, int Options>struct traits<Map<Sophus::SE2<Scalar_> const, Options>>    : traits<Sophus::SE2<Scalar_, Options> const> {  // 为常量 Map 类型的 SE2 特化 traits  using Scalar = Scalar_;  using TranslationType = Map<Sophus::Vector2<Scalar> const, Options>;  using SO2Type = Map<Sophus::SO2<Scalar> const, Options>;};}  // namespace internal}  // namespace Eigen
namespace Sophus {
/// SE2 base type - implements SE2 class but is storage agnostic./// SE2 基类 - 实现 SE2 类，但与存储无关。
/// SE(2) is the group of rotations  and translation in 2d. It is the/// semi-direct product of SO(2) and the 2d Euclidean vector space.  The class/// is represented using a composition of SO2Group  for rotation and a 2-vector/// for translation./// SE(2) 是二维平面上的旋转和平移群。它是 SO(2) 和二维欧几里得向量空间的半直积。/// 该类使用 SO2Group 表示旋转，使用二维向量表示平移。
/// SE(2) is neither compact, nor a commutative group./// SE(2) 既不是紧致群，也不是交换群。
/// See SO2Group for more details of the rotation representation in 2d./// 详见 SO2Group 获取二维旋转的更多细节。
template <class Derived>class SE2Base { public:  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;  // 标量类型  using TranslationType =      typename Eigen::internal::traits<Derived>::TranslationType;  // 平移类型  using SO2Type = typename Eigen::internal::traits<Derived>::SO2Type;  // 旋转类型
  /// Degrees of freedom of manifold, number of dimensions in tangent space  /// (two for translation, three for rotation).  /// 流形的自由度，切空间的维度（平移为 2，旋转为 3）。  static int constexpr DoF = 3;
  /// Number of internal parameters used (tuple for complex, two for  /// translation).  /// 内部参数的数量（复数的二元组，平移为 2）。  static int constexpr num_parameters = 4;
  /// Group transformations are 3x3 matrices.  /// 群变换是 3x3 矩阵。  static int constexpr N = 3;
  /// Points are 2-dimensional  /// 点是二维的。  static int constexpr Dim = 2;
  using Transformation = Matrix<Scalar, N, N>;  // 变换矩阵类型  using Point = Vector2<Scalar>;  // 点类型  using HomogeneousPoint = Vector3<Scalar>;  // 齐次坐标点类型  using Line = ParametrizedLine2<Scalar>;  // 参数化线类型  using Hyperplane = Hyperplane2<Scalar>;  // 超平面类型  using Tangent = Vector<Scalar, DoF>;  // 切向量类型  using 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 SE2 operations.  /// 二元操作的返回类型由 Eigen 的 ScalarBinaryOpTraits 特性决定。  /// 这允许混合使用具体类型和 Map 类型，以及其他兼容的标量类型，如 Ceres::Jet 和双精度标量进行 SE2 操作。  template <typename OtherDerived>  using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<      Scalar, typename OtherDerived::Scalar>::ReturnType;
  template <typename OtherDerived>  using SE2Product = SE2<ReturnScalar<OtherDerived>>;  // SE2 乘积类型
  template <typename PointDerived>  using PointProduct = Vector2<ReturnScalar<PointDerived>>;  // 点乘积类型
  template <typename HPointDerived>  using HomogeneousPointProduct = Vector3<ReturnScalar<HPointDerived>>;  // 齐次点乘积类型
  /// 伴随变换  ///  /// 此函数返回群元素``A``的伴随变换``Ad``，使得对于所有``x``，  /// 它满足``hat(Ad_A * x) = A * hat(x) A^{-1}``。参见下方的hat运算符。  ///  SOPHUS_FUNC Adjoint Adj() const {    Matrix<Scalar, 2, 2> const& R = so2().matrix();  // 获取旋转矩阵    Transformation res;  // 定义变换矩阵    res.setIdentity();  // 将变换矩阵设为单位矩阵    res.template topLeftCorner<2, 2>() = R;  // 设置左上角2x2子矩阵为旋转矩阵    res(0, 2) = translation()[1];  // 设置平移向量的y分量    res(1, 2) = -translation()[0];  // 设置平移向量的负x分量    return res;  // 返回伴随变换矩阵  }
  /// 返回实例转换为NewScalarType类型的副本。  ///  template <class NewScalarType>  SOPHUS_FUNC SE2<NewScalarType> cast() const {    return SE2<NewScalarType>(so2().template cast<NewScalarType>(),  // 转换旋转矩阵                              translation().template cast<NewScalarType>());  // 转换平移向量  }
  /// 返回this * exp(x)相对于x在x=0处的导数。  ///  SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_this_mul_exp_x_at_0()      const {    Matrix<Scalar, num_parameters, DoF> J;  // 定义雅可比矩阵    Sophus::Vector2<Scalar> const c = unit_complex();  // 获取单位复数    Scalar o(0);  // 定义零标量    J(0, 0) = o;    J(0, 1) = o;    J(0, 2) = -c[1];  // 设置雅可比矩阵元素    J(1, 0) = o;    J(1, 1) = o;    J(1, 2) = c[0];  // 设置雅可比矩阵元素    J(2, 0) = c[0];    J(2, 1) = -c[1];    J(2, 2) = o;  // 设置雅可比矩阵元素    J(3, 0) = c[1];    J(3, 1) = c[0];    J(3, 2) = o;  // 设置雅可比矩阵元素    return J;  // 返回雅可比矩阵  }
  /// 返回log(this^{-1} * x)相对于x在x=this处的导数。  ///  SOPHUS_FUNC Matrix<Scalar, DoF, num_parameters> Dx_log_this_inv_by_x_at_this()      const {    Matrix<Scalar, DoF, num_parameters> J;  // 定义雅可比矩阵    J.template block<2, 2>(0, 0).setZero();  // 设置左上角2x2子矩阵为零    J.template block<2, 2>(0, 2) = so2().inverse().matrix();  // 设置右上角2x2子矩阵为旋转矩阵的逆    J.template block<1, 2>(2, 0) = so2().Dx_log_this_inv_by_x_at_this();  // 设置第三行前两列为旋转矩阵的导数    J.template block<1, 2>(2, 2).setZero();  // 设置第三行后两列为零    return J;  // 返回雅可比矩阵  }
  /// 返回群的逆元素。  ///  SOPHUS_FUNC SE2<Scalar> inverse() const {    SO2<Scalar> const invR = so2().inverse();  // 获取旋转矩阵的逆    return SE2<Scalar>(invR, invR * (translation() * Scalar(-1)));  // 返回逆元素  }
  /// 对数映射  ///  /// 计算对数，即群指数的逆，群指数将群的元素（刚体变换）映射到切线空间的元素（扭转）。  ///  /// 具体来说，此函数计算``vee(logmat(.))``，其中``logmat(.)``是矩阵对数，  /// ``vee(.)``是SE(2)的对运算符。  ///  SOPHUS_FUNC Tangent log() const {    using std::abs;
    Tangent upsilon_theta;  // 定义扭转向量    Scalar theta = so2().log();  // 获取旋转角度    upsilon_theta[2] = theta;  // 设置扭转向量的第三个分量为旋转角度    Scalar halftheta = Scalar(0.5) * theta;  // 计算旋转角度的一半    Scalar halftheta_by_tan_of_halftheta;  // 定义辅助变量
    Vector2<Scalar> z = so2().unit_complex();  // 获取单位复数    Scalar real_minus_one = z.x() - Scalar(1.);  // 计算单位复数的实部减1    if (abs(real_minus_one) < Constants<Scalar>::epsilon()) {      halftheta_by_tan_of_halftheta =  // 如果单位复数的实部减1接近于零          Scalar(1.) - Scalar(1. / 12) * theta * theta;  // 计算辅助变量    } else {      halftheta_by_tan_of_halftheta = -(halftheta * z.y()) / (real_minus_one);  // 计算辅助变量    }    Matrix<Scalar, 2, 2> V_inv;  // 定义2x2矩阵    V_inv << halftheta_by_tan_of_halftheta, halftheta, -halftheta,        halftheta_by_tan_of_halftheta;  // 设置矩阵元素    upsilon_theta.template head<2>() = V_inv * translation();  // 计算扭转向量的前两个分量    return upsilon_theta;  // 返回扭转向量  }
  /// 归一化SO2元素  ///  /// 它重新归一化SO2元素。  ///  SOPHUS_FUNC void normalize() { so2().normalize(); }
  /// 返回实例的3x3矩阵表示。  ///  /// 它具有以下形式：  ///  ///   | R t |  ///   | o 1 |  ///  /// 其中``R``是2x2旋转矩阵，``t``是平移2向量，``o``是2列零向量。  ///  SOPHUS_FUNC Transformation matrix() const {    Transformation homogeneous_matrix;  // 定义同质变换矩阵    homogeneous_matrix.template topLeftCorner<2, 3>() = matrix2x3();  // 设置左上角2x3子矩阵    homogeneous_matrix.row(2) = Matrix<Scalar, 1, 3>(Scalar(0), Scalar(0), Scalar(1));  // 设置第三行    return homogeneous_matrix;  // 返回同质变换矩阵  }
  /// 返回上述矩阵的前两行。  ///  SOPHUS_FUNC Matrix<Scalar, 2, 3> matrix2x3() const {    Matrix<Scalar, 2, 3> matrix;  // 定义2x3矩阵    matrix.template topLeftCorner<2, 2>() = rotationMatrix();  // 设置左上角2x2子矩阵为旋转矩阵    matrix.col(2) = translation();  // 设置第三列为平移向量    return matrix;  // 返回2x3矩阵  }
  /// 从OtherDerived进行赋值运算。  ///  template <class OtherDerived>  SOPHUS_FUNC SE2Base<Derived>& operator=(SE2Base<OtherDerived> const& other) {    so2() = other.so2();  // 赋值旋转矩阵    translation() = other.translation();  // 赋值平移向量    return *this;  // 返回当前实例  }
  /// 群乘法，即旋转连接。  ///  template <typename OtherDerived>  SOPHUS_FUNC SE2Product<OtherDerived> operator*(      SE2Base<OtherDerived> const& other) const {    return SE2Product<OtherDerived>(        so2() * other.so2(), translation() + so2() * other.translation());  // 返回群乘法结果  }
  /// 群作用于二维点。  ///  /// 此函数通过SE(2)元素``bar_T_foo = (bar_R_foo, t_bar)``（即刚体变换）旋转和平移二维点``p``：  ///  ///   ``p_bar = bar_R_foo * p_foo + t_bar``。  ///  template <typename PointDerived,            typename = typename std::enable_if_t<                IsFixedSizeVector<PointDerived, 2>::value>>  SOPHUS_FUNC PointProduct<PointDerived> operator*(      Eigen::MatrixBase<PointDerived> const& p) const {    return so2() * p + translation();  // 返回旋转和平移后的点  }
  /// 群作用于齐次二维点。更多细节见上文。  ///  template <typename HPointDerived,            typename = typename std::enable_if_t<                IsFixedSizeVector<HPointDerived, 3>::value>>  SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(      Eigen::MatrixBase<HPointDerived> const& p) const {    const PointProduct<HPointDerived> tp =  // 计算旋转和平移后的点        so2() * p.template head<2>() + p(2) * translation();    return HomogeneousPointProduct<HPointDerived>(tp(0), tp(1), p(2));  // 返回齐次点  }
  /// 群作用于直线。  ///  /// 此函数通过SE(2)元素旋转和平移参数化直线``l(t) = o + t * d``：  ///  /// 原点``o``使用SE(2)动作旋转和平移  /// 方向``d``使用SO(2)动作旋转  ///  SOPHUS_FUNC Line operator*(Line const& l) const {    return Line((*this) * l.origin(), so2() * l.direction());  // 返回旋转和平移后的直线  }
  /// 群作用于超平面。  ///  /// 此函数通过SE2元素旋转超平面``n.x + d = 0``：  ///  /// 法向量``n``被旋转  /// 偏移量``d``根据平移进行调整  ///  /// 注意在二维情况下，超平面只是直线的另一种参数化  ///  SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const {    Hyperplane const rotated = so2() * p;  // 旋转超平面    return Hyperplane(rotated.normal(),                      rotated.offset() - translation().dot(rotated.normal()));  // 返回旋转和平移后的超平面  }
  /// 就地群乘法。此方法仅在乘法的返回类型与此SO2的标量类型兼容时有效。  ///  template <typename OtherDerived,            typename = typename std::enable_if_t<                std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>>  SOPHUS_FUNC SE2Base<Derived>& operator*=(SE2Base<OtherDerived> const& other) {    *static_cast<Derived*>(this) = *this * other;  // 执行群乘法并赋值    return *this;  // 返回当前实例  }
  /// 返回SE(2)的内部参数。  ///  /// 它返回(c[0], c[1], t[0], t[1])，  /// 其中c是单位复数，t是平移3向量。  ///  SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const {    Sophus::Vector<Scalar, num_parameters> p;  // 定义参数向量    p << so2().params(), translation();  // 将旋转和平移参数组合    return p;  // 返回参数向量  }
  /// 返回旋转矩阵。  ///  SOPHUS_FUNC Matrix<Scalar, 2, 2> rotationMatrix() const {    return so2().matrix();  // 返回SO2的旋转矩阵  }
  /// 接收复数，并对其进行归一化。  ///  /// 前提条件：复数不应接近于零。  ///  SOPHUS_FUNC void setComplex(Sophus::Vector2<Scalar> const& complex) {    return so2().setComplex(complex);  // 设置并归一化复数  }
  /// 使用``rotation_matrix``设置``so3``。  ///  /// 前提条件：``R``必须是正交的，且``det(R)=1``。  ///  SOPHUS_FUNC void setRotationMatrix(Matrix<Scalar, 2, 2> const& R) {    SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n {}", (R));  // 确保旋转矩阵是正交的    SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: {}",                  (R.determinant()));  // 确保旋转矩阵的行列式为正    typename SO2Type::ComplexTemporaryType const complex(        Scalar(0.5) * (R(0, 0) + R(1, 1)), Scalar(0.5) * (R(1, 0) - R(0, 1)));  // 计算临时复数    so2().setComplex(complex);  // 设置SO2的复数  }
  /// SO3群的修改器。  ///  SOPHUS_FUNC  SO2Type& so2() { return static_cast<Derived*>(this)->so2(); }
  /// SO3群的访问器。  ///  SOPHUS_FUNC  SO2Type const& so2() const {    return static_cast<Derived const*>(this)->so2();  // 返回SO2  }
  /// 平移向量的修改器。  ///  SOPHUS_FUNC  TranslationType& translation() {    return static_cast<Derived*>(this)->translation();  // 返回平移向量  }
  /// 平移向量的访问器  ///  SOPHUS_FUNC  TranslationType const& translation() const {    return static_cast<Derived const*>(this)->translation();  // 返回平移向量  }
  /// 单位复数的访问器。  ///  SOPHUS_FUNC  typename Eigen::internal::traits<Derived>::SO2Type::ComplexT const&  unit_complex() const {    return so2().unit_complex();  // 返回单位复数  }};
/// 使用默认存储的SE2；继承自SE2Base。template <class Scalar_, int Options>class SE2 : public SE2Base<SE2<Scalar_, Options>> { public:  using Base = SE2Base<SE2<Scalar_, Options>>;  // 继承自SE2Base  static int constexpr DoF = Base::DoF;  // 自由度  static int constexpr num_parameters = Base::num_parameters;  // 参数数量
  using Scalar = Scalar_;  // 标量类型  using Transformation = typename Base::Transformation;  // 变换矩阵类型  using Point = typename Base::Point;  // 点类型  using HomogeneousPoint = typename Base::HomogeneousPoint;  // 齐次点类型  using Tangent = typename Base::Tangent;  // 切线类型  using Adjoint = typename Base::Adjoint;  // 伴随矩阵类型  using SO2Member = SO2<Scalar, Options>;  // SO2成员类型  using TranslationMember = Vector2<Scalar, Options>;  // 平移向量成员类型
  using Base::operator=;  // 使用Base的赋值运算符
  /// 显式定义复制赋值运算符。当存在用户声明的复制构造函数时，  /// 隐式复制赋值运算符的定义已被弃用（clang >= 13中的-Wdeprecated-copy）。  SOPHUS_FUNC SE2& operator=(SE2 const& other) = default;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // 启用Eigen的对齐运算符
  /// 默认构造函数将刚体运动初始化为单位矩阵。  ///  SOPHUS_FUNC SE2();
  /// 复制构造函数  ///  SOPHUS_FUNC SE2(SE2 const& other) = default;
  /// 从OtherDerived复制构造函数  ///  template <class OtherDerived>  SOPHUS_FUNC SE2(SE2Base<OtherDerived> const& other)      : so2_(other.so2()), translation_(other.translation()) {    static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,                  "must be same Scalar type");  // 静态断言，确保标量类型相同  }
  /// 从SO3和平移向量构造  ///  template <class OtherDerived, class D>  SOPHUS_FUNC SE2(SO2Base<OtherDerived> const& so2,                  Eigen::MatrixBase<D> const& translation)      : so2_(so2), translation_(translation) {    static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,                  "must be same Scalar type");  // 静态断言，确保标量类型相同    static_assert(std::is_same<typename D::Scalar, Scalar>::value,                  "must be same Scalar type");  // 静态断言，确保标量类型相同  }
  /// 从旋转矩阵和平移向量构造  ///  /// 前提条件：旋转矩阵需要是正交的，行列式为1。  ///  SOPHUS_FUNC  SE2(typename SO2<Scalar>::Transformation const& rotation_matrix,      Point const& translation)      : so2_(rotation_matrix), translation_(translation) {}
  /// 从旋转角度和平移向量构造  ///  SOPHUS_FUNC SE2(Scalar const& theta, Point const& translation)      : so2_(theta), translation_(translation) {}
  /// 从复数和平移向量构造  ///  /// 前提条件：``complex``不应接近于零。  SOPHUS_FUNC SE2(Vector2<Scalar> const& complex, Point const& translation)      : so2_(complex), translation_(translation) {}
  /// 从3x3矩阵构造  ///  /// 前提条件：旋转矩阵需要是正交的，行列式为1。最后一行必须为``(0, 0, 1)``。  ///  SOPHUS_FUNC explicit SE2(Transformation const& T)      : so2_(T.template topLeftCorner<2, 2>().eval()),  // 提取并评估左上角2x2子矩阵作为旋转矩阵        translation_(T.template block<2, 1>(0, 2)) {}  // 提取平移向量
  /// 这提供了对内部数据的不安全读/写访问。SO(2)由复数表示（两个参数）。  /// 使用直接写访问时，用户需要确保复数保持归一化。  ///  SOPHUS_FUNC Scalar* data() {    // so2_和translation_按顺序排列，没有填充    return so2_.data();  // 返回数据指针  }
  /// 上面data()的常量版本。  ///  SOPHUS_FUNC Scalar const* data() const {    // so2_和translation_按顺序排列，没有填充    return so2_.data();  // 返回常量数据指针  }
  /// SO3的访问器  ///  SOPHUS_FUNC SO2Member& so2() { return so2_; }  // 返回SO2成员的引用
  /// SO3的修改器  ///  SOPHUS_FUNC SO2Member const& so2() const { return so2_; }  // 返回常量SO2成员的引用
  /// 平移向量的修改器  ///  SOPHUS_FUNC TranslationMember& translation() { return translation_; }  // 返回平移向量的引用
  /// 平移向量的访问器  ///  SOPHUS_FUNC TranslationMember const& translation() const {    return translation_;  // 返回常量平移向量的引用  }
  /// 返回exp(x)相对于x的导数。  ///  SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(      Tangent const& upsilon_theta) {    using std::abs;    using std::cos;    using std::pow;    using std::sin;    Sophus::Matrix<Scalar, num_parameters, DoF> J;  // 定义雅可比矩阵    Sophus::Vector<Scalar, 2> upsilon = upsilon_theta.template head<2>();  // 提取前两个分量    Scalar theta = upsilon_theta[2];  // 提取旋转角度
    if (abs(theta) < Constants<Scalar>::epsilon()) {  // 如果旋转角度接近于零      Scalar const o(0);      Scalar const i(1);
      // 关闭clang格式      J << o, o, o,           o, o, i,           i, o, -Scalar(0.5) * upsilon[1],           o, i,  Scalar(0.5) * upsilon[0];      // 打开clang格式      return J;  // 返回雅可比矩阵    }
    Scalar const c0 = sin(theta);  // 计算sin值    Scalar const c1 = cos(theta);  // 计算cos值    Scalar const c2 = 1.0 / theta;  // 计算1/theta    Scalar const c3 = c0 * c2;  // 计算c0*c2    Scalar const c4 = -c1 + Scalar(1);  // 计算-c1+1    Scalar const c5 = c2 * c4;  // 计算c2*c4    Scalar const c6 = c1 * c2;  // 计算c1*c2    Scalar const c7 = pow(theta, -2);  // 计算theta的-2次方    Scalar const c8 = c0 * c7;  // 计算c0*c7    Scalar const c9 = c4 * c7;  // 计算c4*c7
    Scalar const o = Scalar(0);  // 定义零标量    J(0, 0) = o;    J(0, 1) = o;    J(0, 2) = -c0;  // 设置雅可比矩阵元素    J(1, 0) = o;    J(1, 1) = o;    J(1, 2) = c1;  // 设置雅可比矩阵元素    J(2, 0) = c3;    J(2, 1) = -c5;    J(2, 2) =        -c3 * upsilon[1] + c6 * upsilon[0] - c8 * upsilon[0] + c9 * upsilon[1];  // 设置雅可比矩阵元素    J(3, 0) = c5;    J(3, 1) = c3;    J(3, 2) =        c3 * upsilon[0] + c6 * upsilon[1] - c8 * upsilon[1] - c9 * upsilon[0];  // 设置雅可比矩阵元素    return J;  // 返回雅可比矩阵  }
  /// 返回exp(x)相对于x_i在x=0处的导数。  ///  SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>  Dx_exp_x_at_0() {    Sophus::Matrix<Scalar, num_parameters, DoF> J;  // 定义雅可比矩阵    Scalar const o(0);  // 零标量    Scalar const i(1);  // 单位标量
    // 关闭clang格式    J << o, o, o,         o, o, i,         i, o, o,         o, i, o;    // 打开clang格式    return J;  // 返回雅可比矩阵  }
  /// 返回exp(x) * p相对于x_i在x=0处的导数。  ///  SOPHUS_FUNC static Sophus::Matrix<Scalar, 2, DoF> Dx_exp_x_times_point_at_0(      Point const& point) {    Sophus::Matrix<Scalar, 2, DoF> J;  // 定义雅可比矩阵    J << Sophus::Matrix2<Scalar>::Identity(),  // 设置左边为单位矩阵        Sophus::SO2<Scalar>::Dx_exp_x_times_point_at_0(point);  // 计算并设置右边    return J;  // 返回雅可比矩阵  }
  /// 返回exp(x).matrix()相对于``x_i在x=0处``的导数。  ///  SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {    return generator(i);  // 返回第i个生成元  }
  /// 群指数  ///  /// 此函数接收切线空间的一个元素（即扭转``a``）并返回群SE(2)的对应元素。  ///  /// ``a``的前两个分量表示SE(2)切线空间中的平移部分``upsilon``，  /// 而``a``的最后三个分量表示旋转向量``omega``。  /// 更具体地说，此函数计算``expmat(hat(a))``，其中``expmat(.)``是矩阵指数，  /// ``hat(.)``是SE(2)的hat运算符，见下文。  ///  SOPHUS_FUNC static SE2<Scalar> exp(Tangent const& a) {    Scalar theta = a[2];  // 提取旋转角度    SO2<Scalar> so2 = SO2<Scalar>::exp(theta);  // 计算旋转矩阵    Scalar sin_theta_by_theta;  // 定义辅助变量    Scalar one_minus_cos_theta_by_theta;  // 定义辅助变量    using std::abs;
    if (abs(theta) < Constants<Scalar>::epsilon()) {  // 如果旋转角度接近于零      Scalar theta_sq = theta * theta;  // 计算theta的平方      sin_theta_by_theta = Scalar(1.) - Scalar(1. / 6.) * theta_sq;  // 计算辅助变量      one_minus_cos_theta_by_theta =          Scalar(0.5) * theta - Scalar(1. / 24.) * theta * theta_sq;  // 计算辅助变量    } else {      sin_theta_by_theta = so2.unit_complex().y() / theta;  // 计算辅助变量      one_minus_cos_theta_by_theta =          (Scalar(1.) - so2.unit_complex().x()) / theta;  // 计算辅助变量    }    Vector2<Scalar> trans(        sin_theta_by_theta * a[0] - one_minus_cos_theta_by_theta * a[1],        one_minus_cos_theta_by_theta * a[0] + sin_theta_by_theta * a[1]);  // 计算平移向量    return SE2<Scalar>(so2, trans);  // 返回SE2实例  }
  /// 返回与任意4x4矩阵最接近的SE3。  ///  template <class S = Scalar>  static SOPHUS_FUNC std::enable_if_t<std::is_floating_point<S>::value, SE2>  fitToSE2(Matrix3<Scalar> const& T) {    return SE2(SO2<Scalar>::fitToSO2(T.template block<2, 2>(0, 0)),               T.template block<2, 1>(0, 2));  // 调用fitToSO2函数  }
  /// 返回SE(2)的第i个无穷小生成元。  ///  /// SE(2)的无穷小生成元是：  ///  /// ```  ///         |  0  0  1 |  ///   G_0 = |  0  0  0 |  ///         |  0  0  0 |  ///  ///         |  0  0  0 |  ///   G_1 = |  0  0  1 |  ///         |  0  0  0 |  ///  ///         |  0 -1  0 |  ///   G_2 = |  1  0  0 |  ///         |  0  0  0 |  /// ```  ///  /// 前提条件：``i``必须是0、1或2。  ///  SOPHUS_FUNC static Transformation generator(int i) {    SOPHUS_ENSURE(i >= 0 || i <= 2, "i should be in range [0,2].");  // 确保i在0到2之间    Tangent e;    e.setZero();  // 将向量e初始化为零    e[i] = Scalar(1);  // 设置第i个分量为1    return hat(e);  // 返回hat运算符的结果  }
/// hat运算符// 它接收3向量表示（即扭转）并返回对应的李代数元素的矩阵表示。// 正式地，SE(3)的hat运算符定义为//   ``hat(.): R^3 -> R^{3x3}, hat(a) = sum_i a_i * G_i``  （对于i=0,1,2）// 其中``G_i``是SE(2)的第i个无穷小生成元。// 对应的逆运算是vee()运算符，见下文。///SOPHUS_FUNC static Transformation hat(Tangent const& a) {    Transformation Omega;  // 定义变换矩阵    Omega.setZero();  // 将变换矩阵初始化为零    Omega.template topLeftCorner<2, 2>() = SO2<Scalar>::hat(a[2]);  // 将前2x2子矩阵设为旋转矩阵的hat运算结果    Omega.col(2).template head<2>() = a.template head<2>();  // 将第三列的前两行设为a的前两个分量    return Omega;  // 返回变换矩阵}
/// 李括号// 它计算SE(2)的李括号。更具体地说，它计算：//   ``[omega_1, omega_2]_se2 := vee([hat(omega_1), hat(omega_2)])``// 其中``[A,B] := AB-BA``是矩阵对易子，``hat(.)``是hat运算符，``vee(.)``是SE(2)的vee运算符。///SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {    Vector2<Scalar> upsilon1 = a.template head<2>();  // 提取a的前两个分量    Vector2<Scalar> upsilon2 = b.template head<2>();  // 提取b的前两个分量    Scalar theta1 = a[2];  // 提取a的第三个分量    Scalar theta2 = b[2];  // 提取b的第三个分量
    return Tangent(-theta1 * upsilon2[1] + theta2 * upsilon1[1],  // 计算结果的第一个分量                   theta1 * upsilon2[0] - theta2 * upsilon1[0],  // 计算结果的第二个分量                   Scalar(0));  // 结果的第三个分量为零}
/// 构造纯旋转。///static SOPHUS_FUNC SE2 rot(Scalar const& x) {    return SE2(SO2<Scalar>(x), Sophus::Vector2<Scalar>::Zero());  // 返回SE2实例，旋转角度为x，平移向量为零}
/// 从SE(2)流形中抽取均匀样本。// 平移分量分别在[-1, 1]范围内抽取。///template <class UniformRandomBitGenerator>static SE2 sampleUniform(UniformRandomBitGenerator& generator) {    std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1));  // 定义均匀分布    return SE2(SO2<Scalar>::sampleUniform(generator),  // 抽取旋转部分的均匀样本               Vector2<Scalar>(uniform(generator), uniform(generator)));  // 抽取平移部分的均匀样本}
/// 构造仅包含平移的SE(2)实例。///template <class T0, class T1>static SOPHUS_FUNC SE2 trans(T0 const& x, T1 const& y) {    return SE2(SO2<Scalar>(), Vector2<Scalar>(x, y));  // 返回SE2实例，平移向量为(x, y)}
static SOPHUS_FUNC SE2 trans(Vector2<Scalar> const& xy) {    return SE2(SO2<Scalar>(), xy);  // 返回SE2实例，平移向量为xy}
/// 构造绕x轴的平移。///static SOPHUS_FUNC SE2 transX(Scalar const& x) {    return SE2::trans(x, Scalar(0));  // 返回SE2实例，平移向量为(x, 0)}
/// 构造绕y轴的平移。///static SOPHUS_FUNC SE2 transY(Scalar const& y) {    return SE2::trans(Scalar(0), y);  // 返回SE2实例，平移向量为(0, y)}
/// vee运算符// 它接收3x3矩阵表示``Omega``并将其映射到对应的李代数的3向量表示。// 这是hat()运算符的逆运算，见上文。// 前提条件：``Omega``必须具有以下结构：//                |  0 -d  a |///                |  d  0  b |///                |  0  0  0 |///SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {    SOPHUS_ENSURE(        Omega.row(2).template lpNorm<1>() < Constants<Scalar>::epsilon(),        "Omega: \n{}", (Omega));  // 确保Omega的第三行元素的1范数接近于零    Tangent upsilon_omega;    upsilon_omega.template head<2>() = Omega.col(2).template head<2>();  // 提取Omega第三列的前两个分量    upsilon_omega[2] = SO2<Scalar>::vee(Omega.template topLeftCorner<2, 2>());  // 计算并设置第三个分量    return upsilon_omega;  // 返回向量}
protected:  SO2Member so2_;  // 定义SO2成员  TranslationMember translation_;  // 定义平移成员};
template <class Scalar, int Options>SOPHUS_FUNC SE2<Scalar, Options>::SE2()    : translation_(TranslationMember::Zero()) {  // 初始化平移向量为零  static_assert(std::is_standard_layout<SE2>::value,                "Assume standard layout for the use of offset of check below.");  // 静态断言，确保SE2具有标准布局  static_assert(      offsetof(SE2, so2_) + sizeof(Scalar) * SO2<Scalar>::num_parameters ==          offsetof(SE2, translation_),      "This class assumes packed storage and hence will only work "      "correctly depending on the compiler (options) - in "      "particular when using [this->data(), this-data() + "      "num_parameters] to access the raw data in a contiguous fashion.");  // 静态断言，确保so2_和translation_的内存布局是连续的}}  // namespace Sophus
namespace Eigen {
/// Eigen::Map类的``SE2``特化；继承自SE2Base。// 允许我们将SE2对象包装在POD数组周围。template <class Scalar_, int Options>class Map<Sophus::SE2<Scalar_>, Options>    : public Sophus::SE2Base<Map<Sophus::SE2<Scalar_>, Options>> { public:  using Base = Sophus::SE2Base<Map<Sophus::SE2<Scalar_>, Options>>;  // 使用Base作为SE2Base  using Scalar = Scalar_;  // 标量类型  using Transformation = typename Base::Transformation;  // 变换矩阵类型  using Point = typename Base::Point;  // 点类型  using HomogeneousPoint = typename Base::HomogeneousPoint;  // 齐次点类型  using Tangent = typename Base::Tangent;  // 切线类型  using Adjoint = typename Base::Adjoint;  // 伴随矩阵类型
  using Base::operator=;  // 使用Base的赋值运算符  using Base::operator*=;  // 使用Base的乘法赋值运算符  using Base::operator*;  // 使用Base的乘法运算符
  SOPHUS_FUNC  explicit Map(Scalar* coeffs)      : so2_(coeffs),        translation_(coeffs + Sophus::SO2<Scalar>::num_parameters) {}  // 显式构造函数，初始化so2_和translation_
  /// SO3的修改器  ///  SOPHUS_FUNC Map<Sophus::SO2<Scalar>, Options>& so2() { return so2_; }  // 返回SO2成员的引用
  /// SO3的访问器  ///  SOPHUS_FUNC Map<Sophus::SO2<Scalar>, Options> const& so2() const {    return so2_;  // 返回常量SO2成员的引用  }
  /// 平移向量的修改器  ///  SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options>& translation() {    return translation_;  // 返回平移向量的引用  }
  /// 平移向量的访问器  ///  SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options> const& translation() const {    return translation_;  // 返回常量平移向量的引用  }
 protected:  Map<Sophus::SO2<Scalar>, Options> so2_;  // 定义SO2成员  Map<Sophus::Vector2<Scalar>, Options> translation_;  // 定义平移向量成员};
/// ``SE2 const``的Eigen::Map特化；继承自SE2Base。// 允许我们将SE2对象包装在POD数组周围。template <class Scalar_, int Options>class Map<Sophus::SE2<Scalar_> const, Options>    : public Sophus::SE2Base<Map<Sophus::SE2<Scalar_> const, Options>> { public:  using Base = Sophus::SE2Base<Map<Sophus::SE2<Scalar_> const, Options>>;  // 使用Base作为SE2Base  using Scalar = Scalar_;  // 标量类型  using Transformation = typename Base::Transformation;  // 变换矩阵类型  using Point = typename Base::Point;  // 点类型  using HomogeneousPoint = typename Base::HomogeneousPoint;  // 齐次点类型  using Tangent = typename Base::Tangent;  // 切线类型  using Adjoint = typename Base::Adjoint;  // 伴随矩阵类型
  using Base::operator*=;  // 使用Base的乘法赋值运算符  using Base::operator*;  // 使用Base的乘法运算符
  SOPHUS_FUNC explicit Map(Scalar const* coeffs)      : so2_(coeffs),        translation_(coeffs + Sophus::SO2<Scalar>::num_parameters) {}  // 显式构造函数，初始化so2_和translation_
  /// SO3的访问器  ///  SOPHUS_FUNC Map<Sophus::SO2<Scalar> const, Options> const& so2() const {    return so2_;  // 返回常量SO2成员的引用  }
  /// 平移向量的访问器  ///  SOPHUS_FUNC Map<Sophus::Vector2<Scalar> const, Options> const& translation()      const {    return translation_;  // 返回常量平移向量的引用  }
 protected:  Map<Sophus::SO2<Scalar> const, Options> const so2_;  // 定义常量SO2成员  Map<Sophus::Vector2<Scalar> const, Options> const translation_;  // 定义常量平移向量成员};}  // namespace Eigen
相关推荐
IT古董2 分钟前
【漫话机器学习系列】011.Bagging方法 VS Dropout方法
人工智能·算法·机器学习
云天徽上3 分钟前
【机器学习案列】使用随机森林（RF）进行白葡萄酒质量预测
人工智能·随机森林·机器学习
mmsx3 分钟前
android opencv导入进行编译
android·人工智能·opencv
古希腊掌管学习的神4 分钟前
[机器学习]集成学习
人工智能·算法·机器学习
旺小仔.5 分钟前
数据结构之二叉搜索树（Binary Search Tree）
开发语言·数据结构·c++·b树·算法·链表
夕泠爱吃糖6 分钟前
如何处理对象的创建和销毁？
开发语言·c++
析木不会编程10 分钟前
【C语言】特殊指针汇总
c语言·开发语言
Komorebi_999911 分钟前
javaScript中slice()和splice()的用法与区别
开发语言·前端·javascript
橘颂TA15 分钟前
【C++】大厂算法题（详解）
开发语言·c++·算法
严就方法25 分钟前
Qt creator ,语言家功能缺失解决方法
开发语言·qt
热门推荐
01HCIA-datacom数通题库和录播视频资料02文心一言最新重磅发布！03(欧拉)openEuler系统添加网卡文件配置流程、(欧拉)openEuler系统手动配置ipv6地址流程、(欧拉)openEuler系统网络管理说明04校验 GPT-4 真实性的三个经典问题：快速区分 GPT-3.5 与 GPT-4，并提供免费测试网站05如何解决电脑无声问题：排除故障的几种常见方法06玄机平台应急响应—webshell查杀07解决：使用WileyNJDv5_Template模板时，无法生成pdf文件。08WPS-0DAY-20230809的分析和利用复现09Docker 夺命连环 15 问10机密计算--基于可信执行环境（TEE）的隐私计算技术