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

该文件定义了特殊正交群 SO(3) 的实现，用于描述 3D 空间中的旋转操作。文件内包含了与旋转矩阵、四元数和李代数相关的功能，适用于几何计算和机器人学等领域。

关键内容

1. 包含的头文件：

• 自定义模块：

  • 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

应用场景

1. 3D 旋转的表示与操作，包括使用旋转矩阵或四元数。

2. 支持李群和李代数的计算，可用于优化、运动学和动力学建模。

3. 提供数学工具函数，如雅可比矩阵、指数映射和旋转采样。

特点

• 模板化设计

  ：支持不同类型的标量（如浮点或双精度浮点）。

• 与 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)，它简单地返回