【C++】sophus : so3.hpp 特殊正交群 SO(3) 的实现，包含了与旋转矩阵、四元数和李代数相关的功能 (十四)...

原创于 2024-12-17 19:25:44 发布
· 457 阅读
7 ·
版权
文章标签：
#c++ #矩阵 #开发语言 #线性代数
该文件定义了特殊正交群 SO(3) 的实现，用于描述 3D 空间中的旋转操作。文件内包含了与旋转矩阵、四元数和李代数相关的功能，适用于几何计算和机器人学等领域。
关键内容

包含的头文件：
自定义模块：
- rotation_matrix.hpp
- so2.hpp
  （处理 2D 旋转）
- types.hpp
  （数据类型定义）
第三方库：使用 Eigen 库进行矩阵和几何运算，包括：
- Eigen/Geometry
- Eigen/src/Geometry/Quaternion.h
- Eigen/src/Geometry/RotationBase.h
命名空间：
Sophus
：可能是文件主要功能的命名空间。
Eigen
和 internal：与 Eigen 库及其内部实现相关。
主要类：
SO3
：核心类，用于表示 3D 旋转。
SO3Base<Derived>
：基础模板类，提供扩展的接口。
Map
：用于矩阵或数据的映射操作。
其他与数据类型或工具相关的类。
主要功能：
数据访问：data()
正规化四元数：normalize()
群运算：operator*=, setQuaternion()
李代数操作：
- 左雅可比矩阵计算：leftJacobian 和其逆 leftJacobianInverse
- 指数映射：exp, expAndTheta
- 李括号：lieBracket
特定旋转：rotX, rotY, rotZ
随机旋转采样：sampleUniform
符号映射：hat, vee
应用场景

3D 旋转的表示与操作，包括使用旋转矩阵或四元数。
支持李群和李代数的计算，可用于优化、运动学和动力学建模。
提供数学工具函数，如雅可比矩阵、指数映射和旋转采样。
特点

模板化设计
：支持不同类型的标量（如浮点或双精度浮点）。
与 Eigen 库深度集成
：利用其高效矩阵运算能力。
数学严谨性
：包含正规化、李括号等底层几何计算。
/// @file  // 文件注释/// 特殊正交群SO(3) - 3维旋转。
#pragma once  // 防止头文件被多次包含
#include "rotation_matrix.hpp"  // 引入自定义的rotation_matrix.hpp头文件#include "so2.hpp"  // 引入自定义的so2.hpp头文件#include "types.hpp"  // 引入自定义的types.hpp头文件
#include <Eigen/Geometry>  // 引入Eigen的几何库// 只包含我们需要的Eigen头文件。// 这有助于在使用Sophus时使用不寻常的编译器，如nvcc。// #include <Eigen/src/Geometry/OrthoMethods.h>// #include <Eigen/src/Geometry/Quaternion.h>// #include <Eigen/src/Geometry/RotationBase.h> 此方法不再有效。较新的Eigen版本会报错：// error: #error "Please include Eigen/Geometry instead of including headers// inside the src directory directly."//
namespace Sophus {template <class Scalar_, int Options = 0>class SO3;  // 声明模板类SO3using SO3d = SO3<double>;  // 使用别名SO3d表示SO3<double>using SO3f = SO3<float>;  // 使用别名SO3f表示SO3<float>}  // namespace Sophus
namespace Eigen {namespace internal {
template <class Scalar_, int Options_>struct traits<Sophus::SO3<Scalar_, Options_>> {  // 特化traits模板结构体，适用于Sophus::SO3  static constexpr int Options = Options_;  // 设置Options为模板参数  using Scalar = Scalar_;  // 使用模板参数Scalar_作为Scalar类型  using QuaternionType = Eigen::Quaternion<Scalar, Options>;  // 使用Eigen的四元数类型表示四元数};
template <class Scalar_, int Options_>struct traits<Map<Sophus::SO3<Scalar_>, Options_>>    : traits<Sophus::SO3<Scalar_, Options_>> {  // 特化traits模板结构体，适用于Map<Sophus::SO3>  static constexpr int Options = Options_;  // 设置Options为模板参数  using Scalar = Scalar_;  // 使用模板参数Scalar_作为Scalar类型  using QuaternionType = Map<Eigen::Quaternion<Scalar>, Options>;  // 使用Map表示四元数类型};
template <class Scalar_, int Options_>struct traits<Map<Sophus::SO3<Scalar_> const, Options_>>    : traits<Sophus::SO3<Scalar_, Options_> const> {  // 特化traits模板结构体，适用于常量Map<Sophus::SO3>  static constexpr int Options = Options_;  // 设置Options为模板参数  using Scalar = Scalar_;  // 使用模板参数Scalar_作为Scalar类型  using QuaternionType = Map<Eigen::Quaternion<Scalar> const, Options>;  // 使用常量Map表示四元数类型};}  // namespace internal}  // namespace Eigen
namespace Sophus {
/// SO3基础类型 - 实现SO3类但与存储无关。// SO(3)是3维空间中的旋转群。作为矩阵群，它是所有正交矩阵的集合，/// 满足``R * R' = I``（其中``R'``是``R``的转置）并且具有正的行列式。特别地，/// 行列式为1。在内部，群表示为单位四元数。单位四元数可以看作特殊酉群SU(2)的成员。/// SU(2)是SO(3)的双覆盖。因此，对于每个旋转矩阵``R``，存在两个单位四元数：/// ``(r, v)``和``(-r, -v)``，其中``r``是实部，``v``是四元数的虚部3向量。// SO(3)是一个紧致但非交换的群。首先，它是紧致的，因为旋转矩阵集合是一个闭合且有界的集合。/// 其次，它是非交换的，因为方程``R_1 * R_2 = R_2 * R_1``通常不成立。/// 例如，将物体绕其``x``轴旋转一些角度，然后绕其``y``轴旋转一些角度，/// 与先绕``y``旋转再绕``x``旋转不会导致相同的方向。// 类不变量：``unit_quaternion``的2-范数必须接近1。/// 从技术上讲，必须满足：//   ``|unit_quaternion().squaredNorm() - 1| <= Constants::epsilon()``。template <class Derived>class SO3Base { public:  static constexpr int Options = Eigen::internal::traits<Derived>::Options;  // 获取Options  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;  // 获取标量类型  using QuaternionType =      typename Eigen::internal::traits<Derived>::QuaternionType;  // 获取四元数类型  using QuaternionTemporaryType = Eigen::Quaternion<Scalar, Options>;  // 定义临时四元数类型
  /// 群的自由度，切线空间的维度。  static int constexpr DoF = 3;  /// 内部使用的参数数量（四元数是一个四元组）。  static int constexpr num_parameters = 4;  /// 群变换是3x3矩阵。  static int constexpr N = 3;  /// 点是3维的  static int constexpr Dim = 3;
  using Transformation = Matrix<Scalar, N, N>;  // 定义变换矩阵类型  using Point = Vector3<Scalar>;  // 定义点类型  using HomogeneousPoint = Vector4<Scalar>;  // 定义齐次点类型  using Line = ParametrizedLine3<Scalar>;  // 定义直线类型  using Hyperplane = Hyperplane3<Scalar>;  // 定义超平面类型  using Tangent = Vector<Scalar, DoF>;  // 定义切线类型  using Adjoint = Matrix<Scalar, DoF, DoF>;  // 定义伴随矩阵类型
  struct TangentAndTheta {    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    Tangent tangent;    Scalar theta;  };
  /// 对于二元运算，返回类型由Eigen的ScalarBinaryOpTraits特性确定。  /// 这允许混合具体和Map类型，以及其他兼容的标量类型，如Ceres::Jet和  /// double标量与SO3运算。  template <typename OtherDerived>  using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<      Scalar, typename OtherDerived::Scalar>::ReturnType;
  template <typename OtherDerived>  using SO3Product = SO3<ReturnScalar<OtherDerived>>;
  template <typename PointDerived>  using PointProduct = Vector3<ReturnScalar<PointDerived>>;
  template <typename HPointDerived>  using HomogeneousPointProduct = Vector4<ReturnScalar<HPointDerived>>;
  /// 伴随变换  //  /// 此函数返回群元素``A``的伴随变换``Ad``，使得对于所有``x``，  /// 它满足``hat(Ad_A * x) = A * hat(x) A^{-1}``。参见下方的hat运算符。  //  /// 对于SO(3)，它简单地返回对应``A``的旋转矩阵。  ///  SOPHUS_FUNC Adjoint Adj() const { return matrix(); }
  /// 提取绕标准X轴的旋转角度  ///  template <class S = Scalar>  SOPHUS_FUNC std::enable_if_t<std::is_floating_point<S>::value, S> angleX()      const {    Sophus::Matrix3<Scalar> R = matrix();  // 获取旋转矩阵    Sophus::Matrix2<Scalar> Rx = R.template block<2, 2>(1, 1);  // 提取2x2子矩阵    return SO2<Scalar>(makeRotationMatrix(Rx)).log();  // 返回SO2对数映射的值  }
  /// 提取绕标准Y轴的旋转角度  ///  template <class S = Scalar>  SOPHUS_FUNC std::enable_if_t<std::is_floating_point<S>::value, S> angleY()      const {    Sophus::Matrix3<Scalar> R = matrix();  // 获取旋转矩阵    Sophus::Matrix2<Scalar> Ry;    // 关闭clang格式    Ry <<      R(0, 0), R(2, 0),      R(0, 2), R(2, 2);    // 打开clang格式    return SO2<Scalar>(makeRotationMatrix(Ry)).log();  // 返回SO2对数映射的值  }
  /// 提取绕标准Z轴的旋转角度  ///  template <class S = Scalar>  SOPHUS_FUNC std::enable_if_t<std::is_floating_point<S>::value, S> angleZ()      const {    Sophus::Matrix3<Scalar> R = matrix();  // 获取旋转矩阵    Sophus::Matrix2<Scalar> Rz = R.template block<2, 2>(0, 0);  // 提取2x2子矩阵    return SO2<Scalar>(makeRotationMatrix(Rz)).log();  // 返回SO2对数映射的值  }
  /// 返回实例的副本，类型为NewScalarType。  ///  /// 注意：对于相同类型的转换，专门处理以避免不必要的四元数重新归一化。  /// 在一般情况下，返回的SO3的构造会重新归一化四元数。  template <class NewScalarType>  SOPHUS_FUNC std::enable_if_t<std::is_same<Scalar, NewScalarType>::value,                               SO3<NewScalarType>>  cast() const {    return *this;  }
  template <class NewScalarType>  SOPHUS_FUNC std::enable_if_t<!std::is_same<Scalar, NewScalarType>::value,                               SO3<NewScalarType>>  cast() const {    return SO3<NewScalarType>(unit_quaternion().template cast<NewScalarType>());  }
  /// 这提供了对内部数据的不安全读/写访问。SO(3)由Eigen::Quaternion（四个参数）表示。  /// 使用直接写访问时，用户需要确保四元数保持归一化。  ///  /// 注意：前三个标量表示虚部，而第四个标量表示实部。  ///  SOPHUS_FUNC Scalar* data() {    return unit_quaternion_nonconst().coeffs().data();  }
  /// 上面data()的常量版本。  ///  SOPHUS_FUNC Scalar const* data() const {    return unit_quaternion().coeffs().data();  }
  /// 返回this * SO3::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;  // 定义矩阵J    Eigen::Quaternion<Scalar> const q = unit_quaternion();  // 获取单位四元数    Scalar const c0 = Scalar(0.5) * q.w();    Scalar const c1 = Scalar(0.5) * q.z();    Scalar const c2 = -c1;    Scalar const c3 = Scalar(0.5) * q.y();    Scalar const c4 = Scalar(0.5) * q.x();    Scalar const c5 = -c4;    Scalar const c6 = -c3;    J(0, 0) = c0;    J(0, 1) = c2;    J(0, 2) = c3;    J(1, 0) = c1;    J(1, 1) = c0;    J(1, 2) = c5;    J(2, 0) = c6;    J(2, 1) = c4;    J(2, 2) = c0;    J(3, 0) = c5;    J(3, 1) = c6;    J(3, 2) = c2;
    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 {    auto& q = unit_quaternion();  // 获取单位四元数    Matrix<Scalar, DoF, num_parameters> J;  // 定义矩阵J    // 关闭clang格式    J << q.w(),  q.z(), -q.y(), -q.x(),        -q.z(),  q.w(),  q.x(), -q.y(),         q.y(), -q.x(),  q.w(), -q.z();    // 打开clang格式    return J * Scalar(2.);  // 返回矩阵J乘以2  }
  /// 返回SO(3)的内部参数。  ///  /// 它返回(q.imag[0], q.imag[1], q.imag[2], q.real)，其中q是单位四元数。  ///  SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const {    return unit_quaternion().coeffs();  }
  /// 返回群的逆元素。  ///  SOPHUS_FUNC SO3<Scalar> inverse() const {    return SO3<Scalar>(unit_quaternion().conjugate());  }
  /// 对数映射  ///  /// 计算对数，即群指数的逆，群指数将群的元素（旋转矩阵）映射到切线空间的元素（旋转向量）。  ///  /// 具体来说，此函数计算``vee(logmat(.))``，其中``logmat(.)``是矩阵对数，  /// ``vee(.)``是SO(3)的对算子。  ///  SOPHUS_FUNC Tangent log() const { return logAndTheta().tangent; }
  /// 同上，但也返回``theta = |omega|``。  ///  SOPHUS_FUNC TangentAndTheta logAndTheta() const {    TangentAndTheta J;  // 定义切线和角度结构体    using std::abs;    using std::atan2;    using std::sqrt;    Scalar squared_n = unit_quaternion().vec().squaredNorm();  // 计算虚部的平方范数    Scalar w = unit_quaternion().w();  // 获取四元数的实部
    Scalar two_atan_nbyw_by_n;
    /// 基于atan的对数计算，感谢：    ///    /// C. Hertzberg 等人：    /// "通过流形封装集成通用传感器融合算法与可靠状态表示"    /// 信息融合, 2011
    if (squared_n <        Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon()) {      // 如果四元数归一化且n=0，则w应为1;      // w=0不应在此发生！      SOPHUS_ENSURE(abs(w) >= Constants<Scalar>::epsilon(),                    "四元数 ({}) 应该是归一化的！",                    (unit_quaternion().coeffs().transpose()));      Scalar squared_w = w * w;      two_atan_nbyw_by_n =          Scalar(2) / w - Scalar(2.0 / 3.0) * (squared_n) / (w * squared_w);      J.theta = Scalar(2) * squared_n / w;    } else {      Scalar n = sqrt(squared_n);
      // w < 0 ==> cos(theta/2) < 0 ==> theta > pi      //      // 按照惯例，通过将theta包装到pi来施加条件|theta| < pi;      // 包装操作可以在atan2的计算中完成      //      // theta - pi = atan(sin(theta - pi), cos(theta - pi))      //            = atan(-sin(theta), -cos(theta))      //      Scalar atan_nbyw =          (w < Scalar(0)) ? Scalar(atan2(-n, -w)) : Scalar(atan2(n, w));      two_atan_nbyw_by_n = Scalar(2) * atan_nbyw / n;      J.theta = two_atan_nbyw_by_n * n;    }
    J.tangent = two_atan_nbyw_by_n * unit_quaternion().vec();  // 计算切线    return J;  // 返回切线和角度结构体  }
  /// 重新归一化``unit_quaternion``为单位长度。  ///  /// 注意：由于类的不变量，通常不需要直接调用此函数。  ///  SOPHUS_FUNC void normalize() {    Scalar length = unit_quaternion_nonconst().norm();  // 计算单位四元数的范数    SOPHUS_ENSURE(length >= Constants<Scalar>::epsilon(),                  "四元数 ({}) 不应接近于零！",                  (unit_quaternion_nonconst().coeffs().transpose()));  // 确保范数不接近于零    unit_quaternion_nonconst().coeffs() /= length;  // 将四元数归一化  }
  /// 返回实例的3x3矩阵表示。  ///  /// 对于SO(3)，矩阵表示是一个正交矩阵``R``，其行列式为1，因此称为“旋转矩阵”。  ///  SOPHUS_FUNC Transformation matrix() const {    return unit_quaternion().toRotationMatrix();  // 将单位四元数转换为旋转矩阵并返回  }
  /// 从OtherDerived进行赋值运算。  ///  template <class OtherDerived>  SOPHUS_FUNC SO3Base<Derived>& operator=(SO3Base<OtherDerived> const& other) {    unit_quaternion_nonconst() = other.unit_quaternion();  // 将其他实例的单位四元数赋值给当前实例    return *this;  }
  template <typename QuaternionProductType, typename QuaternionTypeA,            typename QuaternionTypeB>  SOPHUS_FUNC static QuaternionProductType QuaternionProduct(      const QuaternionTypeA& a, const QuaternionTypeB& b) {    return QuaternionProductType(        a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),        a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),        a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),        a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x());  // 计算四元数乘积并返回  }
  /// 群乘法，即旋转连接。  ///  template <typename OtherDerived>  SOPHUS_FUNC SO3Product<OtherDerived> operator*(      SO3Base<OtherDerived> const& other) const {    using QuaternionProductType =        typename SO3Product<OtherDerived>::QuaternionType;  // 定义四元数乘积类型    const QuaternionType& a = unit_quaternion();  // 获取当前实例的单位四元数    const typename OtherDerived::QuaternionType& b = other.unit_quaternion();  // 获取其他实例的单位四元数    /// 注意：我们不能使用Eigen的四元数乘法，因为它总是返回与此对象相同标量的四元数，    /// 因此不能正确地乘以Jets和doubles。    return SO3Product<OtherDerived>(        QuaternionProduct<QuaternionProductType>(a, b));  // 返回四元数乘积的结果  }
  /// 群作用于三维点。  ///  /// 此函数通过SO3元素``bar_R_foo``（=旋转矩阵）旋转三维点``p``：  /// ``p_bar = bar_R_foo * p_foo``。  ///  /// 由于SO3在内部表示为单位四元数``q``，其实现为``p_bar = q * p_foo * q^{*}``  /// 其中``q^{*}``是``q``的四元数共轭。  ///  /// 从几何上看，``p``围绕轴``omega/|omega|``旋转角度``|omega|``  /// 其中``omega := vee(log(bar_R_foo))``。  ///  /// 参见下文的``vee``运算符。  ///  template <typename PointDerived,            typename = typename std::enable_if_t<                IsFixedSizeVector<PointDerived, 3>::value>>  SOPHUS_FUNC PointProduct<PointDerived> operator*(      Eigen::MatrixBase<PointDerived> const& p) const {    /// 注意：我们不能使用Eigen的四元数transformVector，因为它总是返回    /// 与此四元数相同标量的Vector3，因此不能正确应用于Jets和doubles。    const QuaternionType& q = unit_quaternion();  // 获取单位四元数    PointProduct<PointDerived> uv = q.vec().cross(p);  // 计算叉积    uv += uv;    return p + q.w() * uv + q.vec().cross(uv);  // 计算旋转后的点并返回  }
  /// 群作用于三维齐次点。更多细节见上文。  template <typename HPointDerived,            typename = typename std::enable_if_t<                IsFixedSizeVector<HPointDerived, 4>::value>>  SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(      Eigen::MatrixBase<HPointDerived> const& p) const {    const auto rp = *this * p.template head<3>();  // 获取旋转后的三维点    return HomogeneousPointProduct<HPointDerived>(rp(0), rp(1), rp(2), p(3));  // 返回三维齐次点  }
  /// 群作用于直线。  ///  /// 此函数通过SO3元素旋转参数化直线``l(t) = o + t * d``：  ///  /// 方向``d``和起点``o``都作为三维点旋转。  ///  SOPHUS_FUNC Line operator*(Line const& l) const {    return Line((*this) * l.origin(), (*this) * l.direction());  // 返回旋转后的直线  }
  /// 群作用于平面。  ///  /// 此函数通过SO3元素旋转平面  /// ``n.x + d = 0``：  ///  /// 法向量``n``被旋转  /// 偏移量``d``保持不变  ///  SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const {    return Hyperplane((*this) * p.normal(), p.offset());  // 返回旋转后的平面  }
  /// 就地群乘法。如果乘法的返回类型与此SO3的标量类型兼容，则此方法有效。  ///  template <typename OtherDerived,            typename = typename std::enable_if_t<                std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>>  SOPHUS_FUNC SO3Base<Derived>& operator*=(SO3Base<OtherDerived> const& other) {    *static_cast<Derived*>(this) = *this * other;  // 执行就地群乘法    return *this;  }
  /// 接收四元数，并对其进行归一化。  ///  /// 前提条件：四元数不应接近于零。  ///  SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quaternion) {    unit_quaternion_nonconst() = quaternion;  // 设置单位四元数    normalize();  // 对其进行归一化  }
  /// 单位四元数的访问器。  ///  SOPHUS_FUNC QuaternionType const& unit_quaternion() const {    return static_cast<Derived const*>(this)->unit_quaternion();  }
 private:  /// 单位四元数的修改器是私有的，以确保类不变量。即四元数必须保持接近单位长度。  ///  SOPHUS_FUNC QuaternionType& unit_quaternion_nonconst() {    return static_cast<Derived*>(this)->unit_quaternion_nonconst();  }};
/// 使用默认存储的SO3；继承自SO3Base。template <class Scalar_, int Options>class SO3 : public SO3Base<SO3<Scalar_, Options>> { public:  using Base = SO3Base<SO3<Scalar_, Options>>;  // 继承自SO3Base  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 QuaternionMember = Eigen::Quaternion<Scalar, Options>;  // 四元数成员类型
  /// ``Base`` 是朋友类，因此unit_quaternion_nonconst可以从``Base``访问。  friend class SO3Base<SO3<Scalar, Options>>;
  using Base::operator=;  // 使用Base的赋值运算符
  /// 显式定义复制赋值运算符。当存在用户声明的复制构造函数时，  /// 隐式复制赋值运算符的定义已被弃用（clang >= 13中的-Wdeprecated-copy）。  SOPHUS_FUNC SO3& operator=(SO3 const& other) = default;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // 启用Eigen的对齐运算符
  /// 默认构造函数将单位四元数初始化为单位旋转。  ///  SOPHUS_FUNC SO3()      : unit_quaternion_(Scalar(1), Scalar(0), Scalar(0), Scalar(0)) {}  // 初始化单位四元数
  /// 复制构造函数  ///  SOPHUS_FUNC SO3(SO3 const& other) = default;  // 默认复制构造函数
  /// 从OtherDerived复制构造函数。  ///  template <class OtherDerived>  SOPHUS_FUNC SO3(SO3Base<OtherDerived> const& other)      : unit_quaternion_(other.unit_quaternion()) {}  // 使用其他实例的单位四元数进行初始化
  /// 从旋转矩阵构造  ///  /// 前提条件：旋转矩阵需要是正交的，行列式为1。  ///  SOPHUS_FUNC SO3(Transformation const& R) : unit_quaternion_(R) {    SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n {}",                  (R * R.transpose()));  // 确保旋转矩阵是正交的    SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: {}",                  (R.determinant()));  // 确保旋转矩阵的行列式为正  }
  /// 从四元数构造  ///  /// 前提条件：四元数不应接近于零。  ///  template <class D>  SOPHUS_FUNC explicit SO3(Eigen::QuaternionBase<D> const& quat)      : unit_quaternion_(quat) {    static_assert(        std::is_same<typename Eigen::QuaternionBase<D>::Scalar, Scalar>::value,        "Input must be of same scalar type");  // 确保输入的四元数类型与标量类型相同    Base::normalize();  // 对四元数进行归一化  }
  /// 单位四元数的访问器。  ///  SOPHUS_FUNC QuaternionMember const& unit_quaternion() const {    return unit_quaternion_;  // 返回单位四元数  }
  /// 返回李群上的左雅可比矩阵。参见右侧列的第一项：  /// http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf  ///  /// 可以选择传递预计算的`theta`  ///  /// 警告：不要与Dx_exp_x()混淆，Dx_exp_x()是SO3的内部四元数表示相对于切线向量的导数  ///  SOPHUS_FUNC static Sophus::Matrix<Scalar, DoF, DoF> leftJacobian(      Tangent const& omega,      std::optional<Scalar> const& theta_o = std::nullopt) {  // 左雅可比矩阵    using std::cos;    using std::sin;    using std::sqrt;
    Scalar const theta_sq = theta_o ? *theta_o * *theta_o : omega.squaredNorm();  // 计算theta平方    Matrix3<Scalar> const Omega = SO3<Scalar>::hat(omega);  // 计算hat运算符    Matrix3<Scalar> const Omega_sq = Omega * Omega;  // 计算Omega的平方    Matrix3<Scalar> V;
    if (theta_sq <        Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon()) {  // 判断theta平方是否接近零      V = Matrix3<Scalar>::Identity() + Scalar(0.5) * Omega;  // 计算左雅可比矩阵    } else {      Scalar theta = theta_o ? *theta_o : sqrt(theta_sq);  // 计算theta      V = Matrix3<Scalar>::Identity() +          (Scalar(1) - cos(theta)) / theta_sq * Omega +          (theta - sin(theta)) / (theta_sq * theta) * Omega_sq;  // 计算左雅可比矩阵    }    return V;  }
  SOPHUS_FUNC static Sophus::Matrix<Scalar, DoF, DoF> leftJacobianInverse(      Tangent const& omega,      std::optional<Scalar> const& theta_o = std::nullopt) {  // 左雅可比矩阵的逆    using std::cos;    using std::sin;    using std::sqrt;    Scalar const theta_sq = theta_o ? *theta_o * *theta_o : omega.squaredNorm();  // 计算theta平方    Matrix3<Scalar> const Omega = SO3<Scalar>::hat(omega);  // 计算hat运算符
    Matrix3<Scalar> V_inv;    if (theta_sq <        Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon()) {  // 判断theta平方是否接近零      V_inv = Matrix3<Scalar>::Identity() - Scalar(0.5) * Omega +              Scalar(1. / 12.) * (Omega * Omega);  // 计算左雅可比矩阵的逆
    } else {      Scalar const theta = theta_o ? *theta_o : sqrt(theta_sq);  // 计算theta      Scalar const half_theta = Scalar(0.5) * theta;
      V_inv = Matrix3<Scalar>::Identity() - Scalar(0.5) * Omega +              (Scalar(1) -               Scalar(0.5) * theta * cos(half_theta) / sin(half_theta)) /                  (theta * theta) * (Omega * Omega);  // 计算左雅可比矩阵的逆    }    return V_inv;  }
  /// 返回exp(x)相对于x的导数。  ///  SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(      Tangent const& omega) {    using std::cos;    using std::exp;    using std::sin;    using std::sqrt;    Scalar const c0 = omega[0] * omega[0];  // 计算omega[0]的平方    Scalar const c1 = omega[1] * omega[1];  // 计算omega[1]的平方    Scalar const c2 = omega[2] * omega[2];  // 计算omega[2]的平方    Scalar const c3 = c0 + c1 + c2;  // 计算omega的平方和
    if (c3 < Constants<Scalar>::epsilon()) {  // 如果c3小于epsilon，则返回x=0处的导数      return Dx_exp_x_at_0();    }
    Scalar const c4 = sqrt(c3);  // 计算c3的平方根    Scalar const c5 = 1.0 / c4;  // 计算c4的倒数    Scalar const c6 = 0.5 * c4;  // 计算0.5乘c4    Scalar const c7 = sin(c6);  // 计算c6的正弦值    Scalar const c8 = c5 * c7;  // 计算c5乘c7    Scalar const c9 = pow(c3, -3.0L / 2.0L);  // 计算c3的-3/2次方    Scalar const c10 = c7 * c9;  // 计算c7乘c9    Scalar const c11 = Scalar(1.0) / c3;  // 计算c3的倒数    Scalar const c12 = cos(c6);  // 计算c6的余弦值    Scalar const c13 = Scalar(0.5) * c11 * c12;  // 计算0.5乘c11乘c12    Scalar const c14 = c7 * c9 * omega[0];  // 计算c7乘c9乘omega[0]    Scalar const c15 = Scalar(0.5) * c11 * c12 * omega[0];  // 计算0.5乘c11乘c12乘omega[0]    Scalar const c16 = -c14 * omega[1] + c15 * omega[1];  // 计算-c14乘omega[1]加c15乘omega[1]    Scalar const c17 = -c14 * omega[2] + c15 * omega[2];  // 计算-c14乘omega[2]加c15乘omega[2]    Scalar const c18 = omega[1] * omega[2];  // 计算omega[1]乘omega[2]    Scalar const c19 = -c10 * c18 + c13 * c18;  // 计算-c10乘c18加c13乘c18    Scalar const c20 = Scalar(0.5) * c5 * c7;  // 计算0.5乘c5乘c7    Sophus::Matrix<Scalar, num_parameters, DoF> J;  // 定义雅可比矩阵J    J(0, 0) = -c0 * c10 + c0 * c13 + c8;  // 计算J(0, 0)    J(0, 1) = c16;  // 计算J(0, 1)    J(0, 2) = c17;  // 计算J(0, 2)    J(1, 0) = c16;  // 计算J(1, 0)    J(1, 1) = -c1 * c10 + c1 * c13 + c8;  // 计算J(1, 1)    J(1, 2) = c19;  // 计算J(1, 2)    J(2, 0) = c17;  // 计算J(2, 0)    J(2, 1) = c19;  // 计算J(2, 1)    J(2, 2) = -c10 * c2 + c13 * c2 + c8;  // 计算J(2, 2)    J(3, 0) = -c20 * omega[0];  // 计算J(3, 0)    J(3, 1) = -c20 * omega[1];  // 计算J(3, 1)    J(3, 2) = -c20 * omega[2];  // 计算J(3, 2)    return J;  // 返回雅可比矩阵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;  // 定义雅可比矩阵J    // 关闭clang格式    J <<  Scalar(0.5),   Scalar(0),   Scalar(0),            Scalar(0), Scalar(0.5),   Scalar(0),            Scalar(0),   Scalar(0), Scalar(0.5),            Scalar(0),   Scalar(0),   Scalar(0);    // 打开clang格式    return J;  // 返回雅可比矩阵J  }
  /// 返回exp(x) * p相对于x_i在x=0处的导数。  ///  SOPHUS_FUNC static Sophus::Matrix<Scalar, 3, DoF> Dx_exp_x_times_point_at_0(      Point const& point) {    return hat(-point);  // 返回hat(-point)  }
  /// 返回exp(x).matrix()相对于``x_i在x=0处``的导数。  ///  SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {    return generator(i);  // 返回第i个生成元  }
  /// 群指数  ///  /// 此函数接收切线空间的一个元素（即旋转向量``omega``），并返回群SO(3)的对应元素。  ///  /// 更具体地说，此函数计算``expmat(hat(omega))``，其中``expmat(.)``是矩阵指数，  /// ``hat(.)``是SO(3)的帽运算符。  ///  SOPHUS_FUNC static SO3<Scalar> exp(Tangent const& omega) {    Scalar theta;  // 定义旋转角    return expAndTheta(omega, &theta);  // 调用expAndTheta函数  }
  /// 同上，但也返回``theta = |omega|``作为输出参数。  ///  /// 前提条件：``theta``不应为``nullptr``。  ///  SOPHUS_FUNC static SO3<Scalar> expAndTheta(Tangent const& omega,                                             Scalar* theta) {    SOPHUS_ENSURE(theta != nullptr, "must not be nullptr.");  // 确保theta不为空    using std::abs;    using std::cos;    using std::sin;    using std::sqrt;    Scalar theta_sq = omega.squaredNorm();  // 计算omega的平方范数
    Scalar imag_factor;  // 虚部因子    Scalar real_factor;  // 实部因子    if (theta_sq <  // 如果theta平方小于一个非常小的常量的平方        Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon()) {      *theta = Scalar(0);  // 设置theta为0      Scalar theta_po4 = theta_sq * theta_sq;  // 计算theta平方的平方      imag_factor = Scalar(0.5) - Scalar(1.0 / 48.0) * theta_sq +                    Scalar(1.0 / 3840.0) * theta_po4;  // 计算虚部因子      real_factor = Scalar(1) - Scalar(1.0 / 8.0) * theta_sq +                    Scalar(1.0 / 384.0) * theta_po4;  // 计算实部因子    } else {      *theta = sqrt(theta_sq);  // 计算theta      Scalar half_theta = Scalar(0.5) * (*theta);  // 计算theta的一半      Scalar sin_half_theta = sin(half_theta);  // 计算theta一半的正弦值      imag_factor = sin_half_theta / (*theta);  // 计算虚部因子      real_factor = cos(half_theta);  // 计算实部因子    }
    SO3 q;  // 定义SO3实例    q.unit_quaternion_nonconst() =        QuaternionMember(real_factor, imag_factor * omega.x(),                         imag_factor * omega.y(), imag_factor * omega.z());  // 设置单位四元数    SOPHUS_ENSURE(abs(q.unit_quaternion().squaredNorm() - Scalar(1)) <                      Sophus::Constants<Scalar>::epsilon(),                  "SO3::exp failed! omega: {}, real: {}, img: {}",                  (omega.transpose()), (real_factor), (imag_factor));  // 确保单位四元数的范数接近1    return q;  // 返回SO3实例  }
  /// 返回与任意3x3矩阵最接近的SO3。  ///  template <class S = Scalar>  static SOPHUS_FUNC std::enable_if_t<std::is_floating_point<S>::value, SO3>  fitToSO3(Transformation const& R) {    return SO3(::Sophus::makeRotationMatrix(R));  // 调用makeRotationMatrix函数  }
  /// 返回SO(3)的第i个无穷小生成元。  ///  /// SO(3)的无穷小生成元是：  ///  /// ```  ///         |  0  0  0 |  ///   G_0 = |  0  0 -1 |  ///         |  0  1  0 |  ///  ///         |  0  0  1 |  ///   G_1 = |  0  0  0 |  ///         | -1  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[i] = Scalar(1);  // 设置第i个分量为1    return hat(e);  // 返回hat运算符的结果  }
  /// hat运算符  ///  /// 它接收3向量表示``omega``（即旋转向量），并返回对应的李代数元素的矩阵表示。  ///  /// 正式地，SO(3)的hat运算符定义为：  ///  ///   ``hat(.): R^3 -> R^{3x3},  hat(omega) = sum_i omega_i * G_i``  ///   （对于i=0,1,2）  ///  /// 其中``G_i``是SO(3)的第i个无穷小生成元。  ///  /// 对应的逆运算是vee()运算符，见下文。  ///  SOPHUS_FUNC static Transformation hat(Tangent const& omega) {    Transformation Omega;  // 定义矩阵Omega    // 关闭clang格式    Omega <<        Scalar(0), -omega(2),  omega(1),         omega(2), Scalar(0), -omega(0),        -omega(1),  omega(0), Scalar(0);    // 打开clang格式    return Omega;  // 返回矩阵Omega  }
  /// 李括号  ///  /// 它计算SO(3)的李括号。更具体地说，它计算：  ///  ///   ``[omega_1, omega_2]_so3 := vee([hat(omega_1), hat(omega_2)])``  ///  /// 其中``[A,B] := AB-BA``是矩阵对易子，``hat(.)``是SO3的hat运算符，  /// ``vee(.)``是SO3的vee运算符。  ///  /// 对于李代数so3，李括号是简单的叉积：  ///  /// ``[omega_1, omega_2]_so3 = omega_1 x omega_2.``  ///  SOPHUS_FUNC static Tangent lieBracket(Tangent const& omega1,                                        Tangent const& omega2) {    return omega1.cross(omega2);  // 返回omega1和omega2的叉积  }
  /// 构造绕x轴的旋转。  ///  static SOPHUS_FUNC SO3 rotX(Scalar const& x) {    return SO3::exp(Sophus::Vector3<Scalar>(x, Scalar(0), Scalar(0)));  // 返回绕x轴的旋转  }
  /// 构造绕y轴的旋转。  ///  static SOPHUS_FUNC SO3 rotY(Scalar const& y) {    return SO3::exp(Sophus::Vector3<Scalar>(Scalar(0), y, Scalar(0)));  // 返回绕y轴的旋转  }
  /// 构造绕z轴的旋转。  ///  static SOPHUS_FUNC SO3 rotZ(Scalar const& z) {    return SO3::exp(Sophus::Vector3<Scalar>(Scalar(0), Scalar(0), z));  // 返回绕z轴的旋转  }
  /// 从SO(3)流形中抽取均匀样本。  /// 基于：http://planning.cs.uiuc.edu/node198.html  ///  template <class UniformRandomBitGenerator>  static SO3 sampleUniform(UniformRandomBitGenerator& generator) {  // 抽取均匀样本    static_assert(IsUniformRandomBitGenerator<UniformRandomBitGenerator>::value,                  "generator must meet the UniformRandomBitGenerator concept");  // 确保生成器符合UniformRandomBitGenerator概念
    std::uniform_real_distribution<Scalar> uniform(Scalar(0), Scalar(1));  // 均匀分布    std::uniform_real_distribution<Scalar> uniform_twopi(        Scalar(0), 2 * Constants<Scalar>::pi());  // 均匀分布在[0, 2*pi]范围内
    const Scalar u1 = uniform(generator);  // 生成均匀随机数u1    const Scalar u2 = uniform_twopi(generator);  // 生成均匀随机数u2    const Scalar u3 = uniform_twopi(generator);  // 生成均匀随机数u3
    const Scalar a = sqrt(1 - u1);  // 计算a    const Scalar b = sqrt(u1);  // 计算b
    return SO3(        QuaternionMember(a * sin(u2), a * cos(u2), b * sin(u3), b * cos(u3)));  // 返回均匀样本  }  /// vee运算符  ///  /// 它接收3x3矩阵表示``Omega``并将其映射到对应的李代数的向量表示。  ///  /// 这是hat()运算符的逆运算，见上文。  ///  /// 前提条件：``Omega``必须具有以下结构：  ///  ///                |  0 -c  b |  ///                |  c  0 -a |  ///                | -b  a  0 |  ///  SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {    return Tangent(Omega(2, 1), Omega(0, 2), Omega(1, 0));  // 返回Omega矩阵对应的向量表示  }
 protected:  /// 单位四元数的修改器是受保护的，以确保类不变量。  ///  SOPHUS_FUNC QuaternionMember& unit_quaternion_nonconst() {    return unit_quaternion_;  // 返回单位四元数  }
  QuaternionMember unit_quaternion_;  // 定义单位四元数成员};
}  // namespace Sophus
namespace Eigen {/// ``SO3``的Eigen::Map特化；继承自SO3Base。// 允许我们将SO3对象包装在POD数组周围（例如外部C风格的四元数）。template <class Scalar_, int Options>class Map<Sophus::SO3<Scalar_>, Options>    : public Sophus::SO3Base<Map<Sophus::SO3<Scalar_>, Options>> { public:  using Base = Sophus::SO3Base<Map<Sophus::SO3<Scalar_>, Options>>;  // 继承自SO3Base  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_quaternion_nonconst可以从``Base``访问。  friend class Sophus::SO3Base<Map<Sophus::SO3<Scalar_>, Options>>;
  using Base::operator=;  // 使用Base的赋值运算符  using Base::operator*=;  // 使用Base的乘法赋值运算符  using Base::operator*;  // 使用Base的乘法运算符
  SOPHUS_FUNC explicit Map(Scalar* coeffs) : unit_quaternion_(coeffs) {}  // 显式构造函数，初始化单位四元数
  /// 单位四元数的访问器。  ///  SOPHUS_FUNC Map<Eigen::Quaternion<Scalar>, Options> const& unit_quaternion()      const {    return unit_quaternion_;  // 返回单位四元数  }
 protected:  /// 单位四元数的修改器是受保护的，以确保类不变量。  ///  SOPHUS_FUNC Map<Eigen::Quaternion<Scalar>, Options>&  unit_quaternion_nonconst() {    return unit_quaternion_;  // 返回单位四元数  }
  Map<Eigen::Quaternion<Scalar>, Options> unit_quaternion_;  // 定义单位四元数};
/// ``SO3 const``的Eigen::Map特化；继承自SO3Base。// 允许我们将SO3对象包装在POD数组周围（例如外部C风格的四元数）。template <class Scalar_, int Options>class Map<Sophus::SO3<Scalar_> const, Options>    : public Sophus::SO3Base<Map<Sophus::SO3<Scalar_> const, Options>> { public:  using Base = Sophus::SO3Base<Map<Sophus::SO3<Scalar_> const, Options>>;  // 继承自SO3Base  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) : unit_quaternion_(coeffs) {}  // 显式构造函数，初始化单位四元数
  /// 单位四元数的访问器。  ///  SOPHUS_FUNC Map<Eigen::Quaternion<Scalar> const, Options> const&  unit_quaternion() const {    return unit_quaternion_;  // 返回单位四元数  }
 protected:  /// 单位四元数的修改器是受保护的，以确保类不变量。  ///  Map<Eigen::Quaternion<Scalar> const, Options> const unit_quaternion_;  // 定义单位四元数};}  // namespace Eigen
这是一个用于处理三维旋转（特殊正交群SO(3)）的 C++ 头文件，位于 Sophus 库的命名空间中。主要特点和功能包括：