【C++】sophus : so2.hpp 特殊正交群 SO(2) 的一个数学表示和操作库-旋转矩阵的构造、操作和转换(十三)...

最新推荐文章于 2025-04-11 21:39:32 发布
原创最新推荐文章于 2025-04-11 21:39:32 发布 · 640 阅读
CC 4.0 BY-SA版权
文章标签：
/// @file  // 文件注释/// 特殊正交群SO(2) - 2维旋转。
#pragma once  // 防止头文件被多次包含
#include <type_traits>  // 引入类型特征库
// 只包含我们需要的Eigen头文件。// 这有助于在使用不寻常的编译器（如nvcc）时使用Sophus。#include <Eigen/LU>  // 引入Eigen的LU分解头文件
#include "rotation_matrix.hpp"  // 引入自定义的rotation_matrix.hpp头文件#include "types.hpp"  // 引入自定义的types.hpp头文件
namespace Sophus {  // 开始命名空间Sophus
template <class Scalar_, int Options = 0>class SO2;  // 声明模板类SO2using SO2d = SO2<double>;  // 使用别名SO2d表示SO2<double>using SO2f = SO2<float>;  // 使用别名SO2f表示SO2<float>
}  // namespace Sophus
namespace Eigen {  // 开始命名空间Eigennamespace internal {  // 开始命名空间internal
template <class Scalar_, int Options_>struct traits<Sophus::SO2<Scalar_, Options_>> {  // 特化traits模板结构体，适用于Sophus::SO2  static constexpr int Options = Options_;  // 设置Options为模板参数  using Scalar = Scalar_;  // 使用模板参数Scalar_作为Scalar类型  using ComplexType = Sophus::Vector2<Scalar, Options>;  // 使用Sophus::Vector2表示复数类型};
template <class Scalar_, int Options_>struct traits<Map<Sophus::SO2<Scalar_>, Options_>>  // 特化traits模板结构体，适用于Map<Sophus::SO2>    : traits<Sophus::SO2<Scalar_, Options_>> {  // 继承自traits<Sophus::SO2>  static constexpr int Options = Options_;  // 设置Options为模板参数  using Scalar = Scalar_;  // 使用模板参数Scalar_作为Scalar类型  using ComplexType = Map<Sophus::Vector2<Scalar>, Options>;  // 使用Map<Sophus::Vector2>表示复数类型};
template <class Scalar_, int Options_>struct traits<Map<Sophus::SO2<Scalar_> const, Options_>>  // 特化traits模板结构体，适用于常量Map<Sophus::SO2>    : traits<Sophus::SO2<Scalar_, Options_> const> {  // 继承自const traits<Sophus::SO2>  static constexpr int Options = Options_;  // 设置Options为模板参数  using Scalar = Scalar_;  // 使用模板参数Scalar_作为Scalar类型  using ComplexType = Map<Sophus::Vector2<Scalar> const, Options>;  // 使用常量Map<Sophus::Vector2>表示复数类型};
}  // namespace internal}  // namespace Eigen
namespace Sophus {  // 开始命名空间Sophus
/// SO2基础类型 - 实现SO2类但与存储无关。////// SO(2)是2维空间中的旋转群。作为矩阵群，它是所有正交矩阵的集合，/// 满足``R * R' = I``（其中``R'``是``R``的转置）并且具有正的行列式。特别地，/// 行列式为1。设``theta``为旋转角，旋转矩阵可以写成闭形式：//////      | cos(theta) -sin(theta) |///      | sin(theta)  cos(theta) |////// 事实上，这些矩阵的第一列与单位复数集合U(1)同构。因此，SO2在内部表示为长度为1的复数。////// SO(2)是一个紧致且交换的群。首先，它是紧致的，因为旋转矩阵集合是一个闭合且有界的集合。/// 其次，它是交换的，因为``R(alpha) * R(beta) = R(beta) * R(alpha)``，/// 这仅仅是因为``alpha + beta = beta + alpha``，其中``alpha``和``beta``是旋转角（关于同一轴）。////// 类不变量：``unit_complex``的2-范数必须接近1。/// 从技术上讲，必须满足：//////   ``|unit_complex().squaredNorm() - 1| <= Constants::epsilon()``。template <class Derived>class SO2Base { public:  static constexpr int Options = Eigen::internal::traits<Derived>::Options;  // 获取Options  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;  // 获取标量类型  using ComplexT = typename Eigen::internal::traits<Derived>::ComplexType;  // 获取复数类型  using ComplexTemporaryType = Sophus::Vector2<Scalar, Options>;  // 定义临时复数类型
  /// 流形的自由度，切线空间的维度（由于只有平面内旋转，因此自由度为1）。  static int constexpr DoF = 1;  /// 内部使用的参数数量（复数是一个二元组）。  static int constexpr num_parameters = 2;  /// 群变换是2x2矩阵。  static int constexpr N = 2;  /// 点是3维的  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 = Scalar;  // 定义切线类型  using Adjoint = Scalar;  // 定义伴随矩阵类型
  /// 对于二元运算，返回类型由Eigen的ScalarBinaryOpTraits特性确定。  /// 这允许混合具体和Map类型，以及其他兼容的标量类型，如Ceres::Jet和  /// double标量与SO2运算。  template <typename OtherDerived>  using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<      Scalar, typename OtherDerived::Scalar>::ReturnType;
  template <typename OtherDerived>  using SO2Product = SO2<ReturnScalar<OtherDerived>>;
  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算子。  ///  /// 对于SO(2)，由于它是一个交换群，因此伴随变换简单地为``1``。  ///  SOPHUS_FUNC Adjoint Adj() const { return Scalar(1); }
  /// 返回实例的副本，类型为NewScalarType。  ///  template <class NewScalarType>  SOPHUS_FUNC SO2<NewScalarType> cast() const {    return SO2<NewScalarType>(unit_complex().template cast<NewScalarType>());  }    /// 这提供了对内部数据的不安全读/写访问。SO(2)由一个单位复数表示（两个参数）。  /// 当使用直接写访问时，用户需要确保复数保持归一化。  ///  SOPHUS_FUNC Scalar* data() { return unit_complex_nonconst().data(); }
  /// 上面data()的常量版本。  ///  SOPHUS_FUNC Scalar const* data() const { return unit_complex().data(); }
  /// 返回群的逆元素。  ///  SOPHUS_FUNC SO2<Scalar> inverse() const {    return SO2<Scalar>(unit_complex().x(), -unit_complex().y());  }
  /// 对数映射  ///  /// 计算对数，即群指数的逆，群指数将群的元素（旋转矩阵）映射到切线空间的元素（旋转角）。  ///  /// 具体来说，此函数计算``vee(logmat(.))``，其中``logmat(.)``是矩阵对数，  /// ``vee(.)``是SO(2)的对算子。  ///  SOPHUS_FUNC Scalar log() const {    using std::atan2;    return atan2(unit_complex().y(), unit_complex().x());  }
  /// 将``unit_complex``重新归一化为单位长度。  ///  /// 注意：由于类的不变量，通常不需要直接调用此函数。  ///  SOPHUS_FUNC void normalize() {    using std::hypot;    // 为了更高精度，避免下溢/上溢    Scalar length = hypot(unit_complex().x(), unit_complex().y());    SOPHUS_ENSURE(length >= Constants<Scalar>::epsilon(),                  "复数不应接近于零！");    unit_complex_nonconst() /= length;  }
  /// 返回实例的2x2矩阵表示。  ///  /// 对于SO(2)，矩阵表示是一个正交矩阵``R``，其行列式为1，因此称为“旋转矩阵”。  ///  SOPHUS_FUNC Transformation matrix() const {    Scalar const& real = unit_complex().x();    Scalar const& imag = unit_complex().y();    Transformation R;    // clang格式关闭    R <<      real, -imag,      imag,  real;    // clang格式打开    return R;  }
  /// 从OtherDerived进行赋值运算。  ///  template <class OtherDerived>  SOPHUS_FUNC SO2Base<Derived>& operator=(SO2Base<OtherDerived> const& other) {    unit_complex_nonconst() = other.unit_complex();    return *this;  }
  /// 群乘法，即旋转连接。  ///  template <typename OtherDerived>  SOPHUS_FUNC SO2Product<OtherDerived> operator*(      SO2Base<OtherDerived> const& other) const {    using ResultT = ReturnScalar<OtherDerived>;    Scalar const lhs_real = unit_complex().x();    Scalar const lhs_imag = unit_complex().y();    typename OtherDerived::Scalar const& rhs_real = other.unit_complex().x();    typename OtherDerived::Scalar const& rhs_imag = other.unit_complex().y();    // 复数乘法    ResultT const result_real = lhs_real * rhs_real - lhs_imag * rhs_imag;    ResultT const result_imag = lhs_real * rhs_imag + lhs_imag * rhs_real;
    ResultT const squared_norm =        result_real * result_real + result_imag * result_imag;    // 由于我们处理的是单位复数，我们可以假设平方范数接近于1。    // 由于数值精度问题，在位姿连接后可能会有小的漂移。    // 因此，我们需要在此重新归一化复数。    // 由于平方范数接近于1，我们不需要计算耗时的平方根，    // 而是可以使用1附近的近似（详情见：http://stackoverflow.com/a/12934750）。    if (squared_norm != ResultT(1.0)) {      ResultT const scale = ResultT(2.0) / (ResultT(1.0) + squared_norm);      return SO2Product<OtherDerived>(result_real * scale, result_imag * scale);    }    return SO2Product<OtherDerived>(result_real, result_imag);  }
  /// 群作用于二维点。  ///  /// 此函数将SO2元素``bar_R_foo``（=旋转矩阵）旋转二维点``p``：  /// ``p_bar = bar_R_foo * p_foo``。  ///  template <typename PointDerived,            typename = typename std::enable_if_t<                IsFixedSizeVector<PointDerived, 2>::value>>  SOPHUS_FUNC PointProduct<PointDerived> operator*(      Eigen::MatrixBase<PointDerived> const& p) const {    Scalar const& real = unit_complex().x();    Scalar const& imag = unit_complex().y();    return PointProduct<PointDerived>(real * p[0] - imag * p[1],                                      imag * p[0] + real * p[1]);  }
  /// 群作用于二维齐次点。  ///  /// 此函数将SO2元素``bar_R_foo``（=旋转矩阵）旋转二维齐次点``p``：  /// ``p_bar = bar_R_foo * p_foo``。  ///  template <typename HPointDerived,            typename = typename std::enable_if_t<                IsFixedSizeVector<HPointDerived, 3>::value>>  SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(      Eigen::MatrixBase<HPointDerived> const& p) const {    Scalar const& real = unit_complex().x();    Scalar const& imag = unit_complex().y();    return HomogeneousPointProduct<HPointDerived>(        real * p[0] - imag * p[1], imag * p[0] + real * p[1],        static_cast<ReturnScalar<HPointDerived>>(p[2]));  }
  /// 群作用于直线。  ///  /// 此函数将SO2元素旋转参数化直线``l(t) = o + t * d``：  ///  /// 方向``d``和起点``o``都作为二维点旋转。  ///  SOPHUS_FUNC Line operator*(Line const& l) const {    return Line((*this) * l.origin(), (*this) * l.direction());  }    /// 群作用于超平面。  ///  /// 此函数通过SO2元素旋转超平面``n.x + d = 0``：  ///  /// 法向量``n``被旋转  /// 偏移量``d``保持不变  ///  /// 注意在二维情况下，超平面只是直线的另一种参数化  ///  SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const {    return Hyperplane((*this) * p.normal(), p.offset());  }
  /// 就地群乘法。如果乘法的返回类型与该SO2的标量类型兼容，则此方法有效。  ///  template <typename OtherDerived,            typename = typename std::enable_if_t<                std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>>  SOPHUS_FUNC SO2Base<Derived> operator*=(SO2Base<OtherDerived> const& other) {    *static_cast<Derived*>(this) = *this * other;    return *this;  }
  /// 返回this * SO2::exp(x)相对于x在x=0处的导数。  ///  SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_this_mul_exp_x_at_0()      const {    return Matrix<Scalar, num_parameters, DoF>(-unit_complex()[1],                                               unit_complex()[0]);  }
  /// 返回SO(2)的内部参数。  ///  /// 它返回(c[0], c[1])，其中c是单位复数。  ///  SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const {    return unit_complex();  }
  /// 返回log(this^{-1} * x)相对于x在x=this处的导数。  ///  SOPHUS_FUNC Matrix<Scalar, DoF, num_parameters> Dx_log_this_inv_by_x_at_this()      const {    return Matrix<Scalar, DoF, num_parameters>(-unit_complex()[1],                                               unit_complex()[0]);  }
  /// 接收复数/元组并对其进行归一化。  ///  /// 前提条件：复数不应接近于零。  ///  SOPHUS_FUNC void setComplex(Point const& complex) {    unit_complex_nonconst() = complex;    normalize();  }
  /// 单位四元数的访问器。  ///  SOPHUS_FUNC  ComplexT const& unit_complex() const {    return static_cast<Derived const*>(this)->unit_complex();  }
 private:  /// 单位复数的修改器是私有的，以确保类不变量。即复数必须保持接近单位长度。  ///  SOPHUS_FUNC  ComplexT& unit_complex_nonconst() {    return static_cast<Derived*>(this)->unit_complex_nonconst();  }};
/// 使用默认存储的SO2；继承自SO2Base。template <class Scalar_, int Options>class SO2 : public SO2Base<SO2<Scalar_, Options>> { public:  using Base = SO2Base<SO2<Scalar_, Options>>;  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 ComplexMember = Vector2<Scalar, Options>;
  /// ``Base`` 是朋友类，因此unit_complex_nonconst可以从``Base``访问。  friend class SO2Base<SO2<Scalar, Options>>;
  using Base::operator=;
  /// 显式定义复制赋值运算符。当存在用户声明的复制构造函数时，  /// 隐式复制赋值运算符的定义已被弃用（clang >= 13中的-Wdeprecated-copy）。  SOPHUS_FUNC SO2& operator=(SO2 const& other) = default;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  /// 默认构造函数将单位复数初始化为单位旋转。  ///  SOPHUS_FUNC SO2() : unit_complex_(Scalar(1), Scalar(0)) {}
  /// 复制构造函数  ///  SOPHUS_FUNC SO2(SO2 const& other) = default;
  /// 从OtherDerived复制构造函数。  ///  template <class OtherDerived>  SOPHUS_FUNC SO2(SO2Base<OtherDerived> const& other)      : unit_complex_(other.unit_complex()) {}
  /// 从旋转矩阵构造  ///  /// 前提条件：旋转矩阵需要是正交的，行列式为1。  ///  SOPHUS_FUNC explicit SO2(Transformation const& R)      : unit_complex_(Scalar(0.5) * (R(0, 0) + R(1, 1)),                      Scalar(0.5) * (R(1, 0) - R(0, 1))) {    SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n {}", (R));    SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: {}",                  (R.determinant()));  }
  /// 从一对实数和虚数构造。  ///  /// 前提条件：该对数值不应接近于零。  ///  SOPHUS_FUNC SO2(Scalar const& real, Scalar const& imag)      : unit_complex_(real, imag) {    Base::normalize();  }    /// 从二维向量构造函数。  ///  /// 前提条件：向量不应接近于零。  ///  template <class D>  SOPHUS_FUNC explicit SO2(Eigen::MatrixBase<D> const& complex)      : unit_complex_(complex) {    static_assert(std::is_same<typename D::Scalar, Scalar>::value,                  "must be same Scalar type");    Base::normalize();  }
  /// 从旋转角度构造函数。  ///  SOPHUS_FUNC explicit SO2(Scalar theta) {    unit_complex_nonconst() = SO2<Scalar>::exp(theta).unit_complex();  }
  /// 单位复数的访问器。  ///  SOPHUS_FUNC ComplexMember const& unit_complex() const {    return unit_complex_;  }
  /// 群指数。  ///  /// 此函数接收切线空间的一个元素（即旋转角``theta``），并返回群SO(2)的对应元素。  ///  /// 更具体地说，此函数计算``expmat(hat(omega))``，其中``expmat(.)``是矩阵指数，  /// ``hat(.)``是SO(2)的帽运算符。  ///  SOPHUS_FUNC static SO2<Scalar> exp(Tangent const& theta) {    using std::cos;    using std::sin;    return SO2<Scalar>(cos(theta), sin(theta));  }
  /// 返回exp(x)相对于x的导数。  ///  SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(      Tangent const& theta) {    using std::cos;    using std::sin;    return Sophus::Matrix<Scalar, num_parameters, DoF>(-sin(theta), cos(theta));  }
  /// 返回exp(x)相对于x_i在x=0处的导数。  ///  SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>  Dx_exp_x_at_0() {    return Sophus::Matrix<Scalar, num_parameters, DoF>(Scalar(0), Scalar(1));  }
  /// 返回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) {    return Point(-point.y(), point.x());  }
  /// 返回exp(x).matrix()相对于``x_i在x=0处``的导数。  ///  SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int) {    return generator();  }
  /// 返回SO(2)的无穷小生成元。  ///  /// SO(2)的无穷小生成元是：  ///  ///     |  0 -1 |  ///     |  1  0 |  ///  SOPHUS_FUNC static Transformation generator() { return hat(Scalar(1)); }
  /// 帽运算符  ///  /// 它接收标量表示``theta``（即旋转角度），并返回对应的李代数元素的矩阵表示。  ///  /// 正式地，SO(2)的帽运算符定义为：  ///  ///   ``hat(.): R^2 -> R^{2x2},  hat(theta) = theta * G``  ///  /// 其中``G``是SO(2)的无穷小生成元。  ///  /// 对应的逆运算是vee()运算符，见下文。  ///  SOPHUS_FUNC static Transformation hat(Tangent const& theta) {    Transformation Omega;    // 关闭clang格式    Omega <<        Scalar(0),   -theta,            theta, Scalar(0);    // 打开clang格式    return Omega;  }
  /// 返回给定任意2x2矩阵的闭合SO2。  ///  template <class S = Scalar>  static SOPHUS_FUNC std::enable_if_t<std::is_floating_point<S>::value, SO2>  fitToSO2(Transformation const& R) {    return SO2(makeRotationMatrix(R));  }
  /// 李括号  ///  /// 返回SO(2)的李括号。由于SO(2)是一个交换群，因此李括号简单为``0``。  ///  SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) {    return Scalar(0);  }
  /// 从SO(2)流形中抽取均匀样本。  ///  template <class UniformRandomBitGenerator>  static SO2 sampleUniform(UniformRandomBitGenerator& generator) {    static_assert(IsUniformRandomBitGenerator<UniformRandomBitGenerator>::value,                  "generator must meet the UniformRandomBitGenerator concept");    std::uniform_real_distribution<Scalar> uniform(-Constants<Scalar>::pi(),                                                   Constants<Scalar>::pi());    return SO2(uniform(generator));  }
  /// vee运算符  ///  /// 它接收2x2矩阵表示``Omega``，并将其映射到对应的李代数标量表示。  ///  /// 这是帽()运算符的逆运算，见上文。  ///  /// 前提条件：``Omega``必须具有以下结构：  ///  ///                |  0 -a |  ///                |  a  0 |  ///  SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {    using std::abs;    return Omega(1, 0);  }
 protected:  /// 复数的修改器是受保护的，以确保类不变量。  ///  SOPHUS_FUNC ComplexMember& unit_complex_nonconst() { return unit_complex_; }
  ComplexMember unit_complex_;};
}  // namespace Sophus
namespace Eigen {
/// ``SO2``的Eigen::Map特化；继承自SO2Base。////// 允许我们将SO2对象包装在POD数组周围（例如外部C风格的复数/元组）。template <class Scalar_, int Options>class Map<Sophus::SO2<Scalar_>, Options>    : public Sophus::SO2Base<Map<Sophus::SO2<Scalar_>, Options>> { public:  using Base = Sophus::SO2Base<Map<Sophus::SO2<Scalar_>, Options>>;  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;
  /// ``Base`` 是朋友类，因此unit_complex_nonconst可以从``Base``访问。  friend class Sophus::SO2Base<Map<Sophus::SO2<Scalar_>, Options>>;
  using Base::operator=;  using Base::operator*=;  using Base::operator*;
  SOPHUS_FUNC  explicit Map(Scalar* coeffs) : unit_complex_(coeffs) {}
  /// 单位复数的访问器。  ///  SOPHUS_FUNC  Map<Sophus::Vector2<Scalar>, Options> const& unit_complex() const {    return unit_complex_;  }
 protected:  /// 单位复数的修改器是受保护的，以确保类不变量。  ///  SOPHUS_FUNC  Map<Sophus::Vector2<Scalar>, Options>& unit_complex_nonconst() {    return unit_complex_;  }
  Map<Matrix<Scalar, 2, 1>, Options> unit_complex_;};
/// ``SO2 const``的Eigen::Map特化；继承自SO2Base。////// 允许我们将SO2对象包装在POD数组周围（例如外部C风格的复数/元组）。template <class Scalar_, int Options>class Map<Sophus::SO2<Scalar_> const, Options>    : public Sophus::SO2Base<Map<Sophus::SO2<Scalar_> const, Options>> { public:  using Base = Sophus::SO2Base<Map<Sophus::SO2<Scalar_> const, Options>>;  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*=;  using Base::operator*;
  SOPHUS_FUNC explicit Map(Scalar const* coeffs) : unit_complex_(coeffs) {}
  /// 单位复数的访问器。  ///  SOPHUS_FUNC Map<Sophus::Vector2<Scalar> const, Options> const& unit_complex()      const {    return unit_complex_;  }
 protected:  /// 单位复数的修改器是受保护的，以确保类不变量。  ///  Map<Matrix<Scalar, 2, 1> const, Options> const unit_complex_;};}  // namespace Eigen