rxso3.hpp 文件实现了 3D 空间中的旋转和缩放操作的 RxSO3 类，这是正数标量 3x3 矩阵和三维特殊正交群 SO(3) 的直接积。以下是文件的主要内容和功能的总结：
- RxSO3 类定义
- 定义了
RxSO3类模板，以及RxSO3d（double 类型）和RxSO3f（float 类型）的别名。
Traits 定义
- 为
RxSO3和映射类型（Map）定义了traits模板，以支持不同标量类型和选项。
RxSO3Base 类
-
实现了 RxSO3 类的基本功能，包括旋转和缩放的矩阵表示、四元数表示等。
-
提供了各种操作符重载，例如赋值操作符、群乘法操作符、对点、齐次点、直线和平面的群作用操作符。
内部参数访问和设置
- 提供了
params函数获取 RxSO3 的内部参数，以及setQuaternion、setRotationMatrix、setScale等函数来设置四元数、旋转矩阵和缩放因子。
李群和李代数操作
-
提供了群指数（exp）和对数（log）映射，以及 hat 和 vee 操作符。
-
实现了李括号（Lie bracket）计算。
样本抽取
- 提供了
sampleUniform函数，用于从 RxSO3 流形中抽取均匀样本。
生成元
- 提供了生成元函数
generator，返回 RxSO3 的无穷小生成元。
Eigen::Map 专门化
- 为
RxSO3和RxSO3 const实现了 Eigen::Map 专门化，允许将 RxSO3 对象包装在 POD 数组中。
这些功能使得rxso3.hpp 文件能够在 3D 空间中高效地进行旋转和缩放操作，适用于计算机图形学、机器人学等领域。
cs
/// @file/// 直接积 R X SO(3) - 在 3D 中进行旋转和缩放。
#pragma once
#include "so3.hpp" // 包含 "so3.hpp" 头文件
namespace Sophus {template <class Scalar_, int Options = 0>class RxSO3; // 定义模板类 RxSO3using RxSO3d = RxSO3<double>; // 使用 double 类型的 RxSO3using RxSO3f = RxSO3<float>; // 使用 float 类型的 RxSO3} // namespace Sophus
namespace Eigen {namespace internal {
template <class Scalar_, int Options_>struct traits<Sophus::RxSO3<Scalar_, Options_>> { static constexpr int Options = Options_; // 定义常量 Options using Scalar = Scalar_; // 定义 Scalar 类型 using QuaternionType = Eigen::Quaternion<Scalar, Options>; // 定义四元数类型};
template <class Scalar_, int Options_>struct traits<Map<Sophus::RxSO3<Scalar_>, Options_>> : traits<Sophus::RxSO3<Scalar_, Options_>> { static constexpr int Options = Options_; // 定义常量 Options using Scalar = Scalar_; // 定义 Scalar 类型 using QuaternionType = Map<Eigen::Quaternion<Scalar>, Options>; // 定义映射后的四元数类型};
template <class Scalar_, int Options_>struct traits<Map<Sophus::RxSO3<Scalar_> const, Options_>> : traits<Sophus::RxSO3<Scalar_, Options_> const> { static constexpr int Options = Options_; // 定义常量 Options using Scalar = Scalar_; // 定义 Scalar 类型 using QuaternionType = Map<Eigen::Quaternion<Scalar> const, Options>; // 定义映射后的常量四元数类型};} // namespace internal} // namespace Eigen
namespace Sophus {
/// RxSO3 基本类型 - 实现 RxSO3 类，但与存储无关// 该类实现了群 ``R+ x SO(3)``, 即正数标量 3x3 矩阵的直积（同构于正实数）和三维特殊正交群 SO(3)。/// 在几何上，它是三维空间中的旋转和缩放群。/// 作为矩阵群，RxSO3 由形如 ``s * R`` 的矩阵组成，/// 其中 ``R`` 是一个正交矩阵，且 ``det(R)=1``，``s > 0`` 是一个正实数。// 内部地，RxSO3 由非零四元数群表示。/// 特别是，缩放等于四元数 ``q`` 的平方范数，``s = |q|^2``。/// 这是最紧凑的表示，因为 RxSO3 的自由度（DoF=4）等于内部参数的数量（=4）。// 该类有一个明确的类不变量，即缩放 ``s`` 不能太接近零或无穷大。/// 严格来说，必须满足：// ``quaternion().squaredNorm() >= Constants::epsilon()`` 和/// ``1. / quaternion().squaredNorm() >= Constants::epsilon()``。// 为了遵守这个条件，群乘法是通过饱和实现的，这样一个乘积总是具有大于或等于这个阈值的缩放。
template <class Derived>class RxSO3Base { public: static constexpr int Options = Eigen::internal::traits<Derived>::Options; // 定义常量 Options using Scalar = typename Eigen::internal::traits<Derived>::Scalar; // 定义 Scalar 类型 using QuaternionType = typename Eigen::internal::traits<Derived>::QuaternionType; // 定义四元数类型 using QuaternionTemporaryType = Eigen::Quaternion<Scalar, Options>; // 定义临时四元数类型
/// 流形的自由度，切空间中的维度数（旋转为三维，缩放为一维）。 static int constexpr DoF = 4; /// 使用的内部参数数量（四元数是一个四元组）。 static int constexpr num_parameters = 4; /// 群变换是 3x3 矩阵。 static int constexpr N = 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 标量与 RxSO3 操作一起使用。template <typename OtherDerived>using ReturnScalar = typename Eigen::ScalarBinaryOpTraits< Scalar, typename OtherDerived::Scalar>::ReturnType; // 定义返回标量类型
template <typename OtherDerived>using RxSO3Product = RxSO3<ReturnScalar<OtherDerived>>; // 定义 RxSO3 产品类型
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 操作符。// 对于 RxSO(3)，它仅返回对应于 ``A`` 的旋转矩阵。///SOPHUS_FUNC Adjoint Adj() const { Adjoint res; // 定义伴随矩阵结果 res.setIdentity(); // 设置结果为单位矩阵 res.template topLeftCorner<3, 3>() = rotationMatrix(); // 将旋转矩阵赋值给结果的左上角 return res; // 返回结果}
/// 返回实例的复制，类型转换为 NewScalarType。///template <class NewScalarType>SOPHUS_FUNC RxSO3<NewScalarType> cast() const { return RxSO3<NewScalarType>(quaternion().template cast<NewScalarType>()); // 将四元数转换为新的标量类型并返回新的 RxSO3 实例}
/// 提供对内部数据的不安全读写访问。RxSO(3) 由 Eigen::Quaternion 表示（四个参数）。/// 使用直接写访问时，用户需要确保四元数不会被设置得接近零。// 注意：前三个标量表示虚部，第四个标量表示实部。///SOPHUS_FUNC Scalar* data() { return quaternion_nonconst().coeffs().data(); } // 返回四元数系数的数据指针
/// data() 的常量版本。///SOPHUS_FUNC Scalar const* data() const { return quaternion().coeffs().data(); // 返回四元数系数的常量数据指针}
/// 返回群的逆。///SOPHUS_FUNC RxSO3<Scalar> inverse() const { return RxSO3<Scalar>(quaternion().inverse()); // 返回四元数的逆并创建新的 RxSO3 实例}
/// 对数映射// 计算对数，即群指数的逆，它将群的元素（缩放的旋转矩阵）映射到切空间的元素（旋转向量加上缩放因子的对数）。// 具体来说，这个函数计算 ``vee(logmat(.))``，其中 ``logmat(.)`` 是矩阵对数，``vee(.)`` 是 RxSO3 的 vee 操作符。///SOPHUS_FUNC Tangent log() const { return logAndTheta().tangent; } // 返回对数的切向量
/// 同上，但也返回 ``theta = |omega|``。///SOPHUS_FUNC TangentAndTheta logAndTheta() const { using std::log;
Scalar scale = quaternion().squaredNorm(); // 计算四元数的平方范数 TangentAndTheta result; // 定义结果 result.tangent[3] = log(scale); // 计算缩放因子的对数并赋值给结果 auto omega_and_theta = SO3<Scalar>(quaternion()).logAndTheta(); // 计算旋转向量和角度 result.tangent.template head<3>() = omega_and_theta.tangent; // 将旋转向量赋值给结果的前 3 个元素 result.theta = omega_and_theta.theta; // 将角度赋值给结果 return result; // 返回结果}
/// 返回实例的 3x3 矩阵表示。// 对于 RxSO3，矩阵表示是一个缩放的正交矩阵 ``sR``，其中 ``det(R)=s^3``，即带有缩放因子 ``s`` 的旋转矩阵 ``R``。///SOPHUS_FUNC Transformation matrix() const { Transformation sR; // 定义缩放的旋转矩阵
Scalar const vx_sq = quaternion().vec().x() * quaternion().vec().x(); // 计算四元数 x 分量的平方 Scalar const vy_sq = quaternion().vec().y() * quaternion().vec().y(); // 计算四元数 y 分量的平方 Scalar const vz_sq = quaternion().vec().z() * quaternion().vec().z(); // 计算四元数 z 分量的平方 Scalar const w_sq = quaternion().w() * quaternion().w(); // 计算四元数 w 分量的平方 Scalar const two_vx = Scalar(2) * quaternion().vec().x(); // 计算两倍的四元数 x 分量 Scalar const two_vy = Scalar(2) * quaternion().vec().y(); // 计算两倍的四元数 y 分量 Scalar const two_vz = Scalar(2) * quaternion().vec().z(); // 计算两倍的四元数 z 分量 Scalar const two_vx_vy = two_vx * quaternion().vec().y(); // 计算 x 和 y 分量的乘积的两倍 Scalar const two_vx_vz = two_vx * quaternion().vec().z(); // 计算 x 和 z 分量的乘积的两倍 Scalar const two_vx_w = two_vx * quaternion().w(); // 计算 x 分量和 w 分量的乘积的两倍 Scalar const two_vy_vz = two_vy * quaternion().vec().z(); // 计算 y 和 z 分量的乘积的两倍 Scalar const two_vy_w = two_vy * quaternion().w(); // 计算 y 分量和 w 分量的乘积的两倍 Scalar const two_vz_w = two_vz * quaternion().w(); // 计算 z 分量和 w 分量的乘积的两倍
sR(0, 0) = vx_sq - vy_sq - vz_sq + w_sq; // 计算矩阵的 (0,0) 元素 sR(1, 0) = two_vx_vy + two_vz_w; // 计算矩阵的 (1,0) 元素 sR(2, 0) = two_vx_vz - two_vy_w; // 计算矩阵的 (2,0) 元素
sR(0, 1) = two_vx_vy - two_vz_w; // 计算矩阵的 (0,1) 元素 sR(1, 1) = -vx_sq + vy_sq - vz_sq + w_sq; // 计算矩阵的 (1,1) 元素 sR(2, 1) = two_vx_w + two_vy_vz; // 计算矩阵的 (2,1) 元素
sR(0, 2) = two_vx_vz + two_vy_w; // 计算矩阵的 (0,2) 元素 sR(1, 2) = -two_vx_w + two_vy_vz; // 计算矩阵的 (1,2) 元素 sR(2, 2) = -vx_sq - vy_sq + vz_sq + w_sq; // 计算矩阵的 (2,2) 元素 return sR; // 返回缩放的旋转矩阵}
/// 从 OtherDerived 赋值的操作符。///template <class OtherDerived>SOPHUS_FUNC RxSO3Base<Derived>& operator=( RxSO3Base<OtherDerived> const& other) { quaternion_nonconst() = other.quaternion(); // 将其他对象的四元数赋值给当前对象的四元数 return *this; // 返回当前对象}
/// 群乘法，即旋转级联和缩放乘法。// 注意：该函数对接近零的乘积进行饱和处理，以确保类不变量。///template <typename OtherDerived>SOPHUS_FUNC RxSO3Product<OtherDerived> operator*( RxSO3Base<OtherDerived> const& other) const { using std::sqrt; using ResultT = ReturnScalar<OtherDerived>; using QuaternionProductType = typename RxSO3Product<OtherDerived>::QuaternionType;
QuaternionProductType result_quaternion( Sophus::SO3<double>::QuaternionProduct<QuaternionProductType>( quaternion(), other.quaternion())); // 计算四元数乘积
ResultT scale = result_quaternion.squaredNorm(); // 计算四元数的平方范数 if (scale < Constants<ResultT>::epsilon()) { SOPHUS_ENSURE(scale > ResultT(0), "Scale must be greater zero."); // 确保缩放因子大于零 /// 进行饱和处理以确保类不变量。 result_quaternion.normalize(); // 规范化四元数 result_quaternion.coeffs() *= sqrt(Constants<ResultT>::epsilonPlus()); // 调整四元数系数 } if (scale > ResultT(1.) / Constants<ResultT>::epsilon()) { result_quaternion.normalize(); // 规范化四元数 result_quaternion.coeffs() /= sqrt(Constants<ResultT>::epsilonPlus()); // 调整四元数系数 } return RxSO3Product<OtherDerived>(result_quaternion); // 返回新的 RxSO3Product 对象}
/// 对 3 维点的群作用。// 该函数通过 SO3 元素 ``bar_R_foo``（旋转矩阵）旋转 3 维点 ``p``，并通过缩放因子 ``s`` 缩放：// ``p_bar = s * (bar_R_foo * p_foo)``。///template <typename PointDerived, typename = typename std::enable_if_t< IsFixedSizeVector<PointDerived, 3>::value>>SOPHUS_FUNC PointProduct<PointDerived> operator*( Eigen::MatrixBase<PointDerived> const& p) const { // 参考 http:///eigen.tuxfamily.org/bz/show_bug.cgi?id=459 Scalar scale = quaternion().squaredNorm(); // 计算四元数的平方范数 PointProduct<PointDerived> two_vec_cross_p = quaternion().vec().cross(p); // 计算四元数虚部与点 p 的叉积 two_vec_cross_p += two_vec_cross_p; // 将叉积结果加倍 return scale * p + (quaternion().w() * two_vec_cross_p + quaternion().vec().cross(two_vec_cross_p)); // 返回旋转和缩放后的点}
/// 对齐次 3 维点的群作用。更多细节请参见上文。///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 rsp = *this * p.template head<3>(); // 对前三维点进行群作用 return HomogeneousPointProduct<HPointDerived>(rsp(0), rsp(1), rsp(2), p(3)); // 返回新的齐次点}
/// 对直线的群作用。// 该函数通过 SO3 元素旋转参数化直线 ``l(t) = o + t * d``，并通过缩放因子进行缩放：// 原点 ``o`` 被旋转并缩放/// 方向 ``d`` 被旋转（保持其范数）SOPHUS_FUNC Line operator*(Line const& l) const { return Line((*this) * l.origin(), (*this) * l.direction() / quaternion().squaredNorm()); // 返回新的直线}
/// 对平面的群作用。// 该函数通过 SO3 元素旋转参数化平面 ``n.x + d = 0``，并通过缩放因子进行缩放：// 法向量 ``n`` 被旋转/// 偏移量 ``d`` 被缩放///SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const { const auto this_scale = scale(); return Hyperplane((*this) * p.normal() / this_scale, this_scale * p.offset()); // 返回新的平面}
/// 就地群乘法。此方法仅在乘法的返回类型与此 SO3 的标量类型兼容时有效。// 注意：该函数对接近零的乘积进行饱和处理，以确保类不变量。///template <typename OtherDerived, typename = typename std::enable_if_t< std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>>SOPHUS_FUNC RxSO3Base<Derived>& operator*=( RxSO3Base<OtherDerived> const& other) { *static_cast<Derived*>(this) = *this * other; // 执行群乘法并赋值给当前对象 return *this; // 返回当前对象}
/// 返回 RxSO(3) 的内部参数。// 它返回 (q.imag[0], q.imag[1], q.imag[2], q.real)，其中 q 是四元数。///SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const { return quaternion().coeffs(); // 返回四元数的系数}
/// 设置非零四元数// 前提条件：``quat`` 不能接近零或无穷大SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quat) { SOPHUS_ENSURE(quat.squaredNorm() > Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon(), "缩放因子必须大于等于 epsilon。"); SOPHUS_ENSURE( quat.squaredNorm() < Scalar(1.) / (Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon()), "逆缩放因子必须大于等于 epsilon。"); static_cast<Derived*>(this)->quaternion_nonconst() = quat; // 设置四元数}
/// 四元数的访问器。///SOPHUS_FUNC QuaternionType const& quaternion() const { return static_cast<Derived const*>(this)->quaternion(); // 返回四元数}
/// 返回旋转矩阵。///SOPHUS_FUNC Transformation rotationMatrix() const { QuaternionTemporaryType norm_quad = quaternion(); // 获取并复制四元数 norm_quad.normalize(); // 规范化四元数 return norm_quad.toRotationMatrix(); // 返回对应的旋转矩阵}
/// 返回缩放因子。///SOPHUS_FUNCScalar scale() const { return quaternion().squaredNorm(); } // 返回四元数的平方范数
/// 使用旋转矩阵 ``R`` 设置四元数，保持缩放不变。///SOPHUS_FUNC void setRotationMatrix(Transformation const& R) { using std::sqrt; Scalar saved_scale = scale(); // 保存当前缩放因子 quaternion_nonconst() = R; // 设置旋转矩阵 quaternion_nonconst().coeffs() *= sqrt(saved_scale); // 调整四元数系数以匹配缩放因子}
/// 设置缩放因子，保持旋转不变。// 注意：此函数的计算成本较高，因为它需要调用两次平方根函数。///SOPHUS_FUNCvoid setScale(Scalar const& scale) { using std::sqrt; quaternion_nonconst().normalize(); // 规范化四元数 quaternion_nonconst().coeffs() *= sqrt(scale); // 调整四元数系数以匹配缩放因子}
/// 使用缩放的旋转矩阵 ``sR`` 设置四元数。// 前提条件：3x3 矩阵必须是 "缩放的正交矩阵" 并且具有正的行列式。///SOPHUS_FUNC void setScaledRotationMatrix(Transformation const& sR) { Transformation squared_sR = sR * sR.transpose(); // 计算 sR 的平方矩阵 Scalar squared_scale = Scalar(1. / 3.) * (squared_sR(0, 0) + squared_sR(1, 1) + squared_sR(2, 2)); // 计算平方缩放因子 SOPHUS_ENSURE(squared_scale >= Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon(), "缩放因子必须大于等于 epsilon。"); SOPHUS_ENSURE(squared_scale < Scalar(1.) / (Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon()), "逆缩放因子必须大于等于 epsilon。"); Scalar scale = sqrt(squared_scale); // 计算缩放因子 quaternion_nonconst() = sR / scale; // 设置四元数为 sR 除以缩放因子 quaternion_nonconst().coeffs() *= sqrt(scale); // 调整四元数系数}
/// 设置 SO(3) 旋转，保持缩放不变。///SOPHUS_FUNC void setSO3(SO3<Scalar> const& so3) { using std::sqrt; Scalar saved_scale = scale(); // 保存当前缩放因子 quaternion_nonconst() = so3.unit_quaternion(); // 设置四元数为单位四元数 quaternion_nonconst().coeffs() *= sqrt(saved_scale); // 调整四元数系数以匹配缩放因子}
SOPHUS_FUNC SO3<Scalar> so3() const { return SO3<Scalar>(quaternion()); } // 返回 SO3 对象
/// 返回 this * RxSO3::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; // 定义雅可比矩阵 Eigen::Quaternion<Scalar> const q = quaternion(); // 获取四元数 J.col(3) = q.coeffs() * Scalar(0.5); // 计算导数 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 = quaternion(); // 获取四元数 Matrix<Scalar, DoF, num_parameters> J; // 定义雅可比矩阵 // clang-format off 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(), q.x(), q.y(), q.z(), q.w(); // clang-format on const Scalar scaler = Scalar(2.) / q.squaredNorm(); // 计算比例因子 return J * scaler; // 返回乘以比例因子的雅可比矩阵}
private:/// 四元数的修改器是私有的，以确保类不变量。///SOPHUS_FUNC QuaternionType& quaternion_nonconst() { return static_cast<Derived*>(this)->quaternion_nonconst(); // 返回非 const 四元数}};
/// 使用存储的 RxSO3；派生自 RxSO3Base。template <class Scalar_, int Options>class RxSO3 : public RxSO3Base<RxSO3<Scalar_, Options>> {public: using Base = RxSO3Base<RxSO3<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 QuaternionMember = Eigen::Quaternion<Scalar, Options>; // 四元数成员类型
/// ``Base`` 是友类，因此 quaternion_nonconst 可以从 ``Base`` 访问。 friend class RxSO3Base<RxSO3<Scalar_, Options>>;
using Base::operator=; // 使用基类的赋值操作符
/// 显式定义复制赋值操作符。当存在用户声明的复制构造函数时，隐式复制赋值操作符的定义已弃用（在 clang >= 13 中的 -Wdeprecated-copy）。 SOPHUS_FUNC RxSO3& operator=(RxSO3 const& other) = default;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // 确保内存对齐
/// 默认构造函数将四元数初始化为单位旋转并将缩放初始化为 1。 /// SOPHUS_FUNC RxSO3() : quaternion_(Scalar(1), Scalar(0), Scalar(0), Scalar(0)) {}
/// 复制构造函数 /// SOPHUS_FUNC RxSO3(RxSO3 const& other) = default;
/// 从 OtherDerived 复制构造函数 /// template <class OtherDerived> SOPHUS_FUNC RxSO3(RxSO3Base<OtherDerived> const& other) : quaternion_(other.quaternion()) {}
/// 从缩放旋转矩阵构造函数 /// /// 前提条件：旋转矩阵需要是缩放正交矩阵，行列式为 ``s^3``。 /// SOPHUS_FUNC explicit RxSO3(Transformation const& sR) { this->setScaledRotationMatrix(sR); // 设置缩放旋转矩阵 }
/// 从缩放因子和旋转矩阵 ``R`` 构造函数 /// /// 前提条件：旋转矩阵 ``R`` 必须是正交的，行列式为 1，并且 ``scale`` 不能接近零或无穷大。 /// SOPHUS_FUNC RxSO3(Scalar const& scale, Transformation const& R) : quaternion_(R) { SOPHUS_ENSURE(scale >= Constants<Scalar>::epsilon(), "缩放因子必须大于等于 epsilon。"); SOPHUS_ENSURE(scale < Scalar(1.) / Constants<Scalar>::epsilon(), "逆缩放因子必须大于等于 epsilon。"); using std::sqrt; quaternion_.coeffs() *= sqrt(scale); // 调整四元数系数以匹配缩放因子 }
/// 从缩放因子和 SO3 构造函数 /// /// 前提条件：``scale`` 不能接近零或无穷大。 /// SOPHUS_FUNC RxSO3(Scalar const& scale, SO3<Scalar> const& so3) : quaternion_(so3.unit_quaternion()) { SOPHUS_ENSURE(scale >= Constants<Scalar>::epsilon(), "缩放因子必须大于等于 epsilon。"); SOPHUS_ENSURE(scale < Scalar(1.) / Constants<Scalar>::epsilon(), "逆缩放因子必须大于等于 epsilon。"); using std::sqrt; quaternion_.coeffs() *= sqrt(scale); // 调整四元数系数以匹配缩放因子 }
/// 从四元数构造函数 /// /// 前提条件：四元数不能接近零或无穷大。 /// template <class D> SOPHUS_FUNC explicit RxSO3(Eigen::QuaternionBase<D> const& quat) : quaternion_(quat) { static_assert(std::is_same<typename D::Scalar, Scalar>::value, "必须是相同的标量类型。"); SOPHUS_ENSURE(quaternion_.squaredNorm() >= Constants<Scalar>::epsilon(), "缩放因子必须大于等于 epsilon。"); SOPHUS_ENSURE( quat.squaredNorm() < Scalar(1.) / Constants<Scalar>::epsilon(), "逆缩放因子必须大于等于 epsilon。"); }
/// 从缩放因子和单位四元数构造函数// 前提条件：四元数不能接近零。///template <class D>SOPHUS_FUNC explicit RxSO3(Scalar const& scale, Eigen::QuaternionBase<D> const& unit_quat) : RxSO3(scale, SO3<Scalar>(unit_quat)) {} // 调用 RxSO3 构造函数，用缩放因子和 SO3 构造
/// 四元数的访问器。///SOPHUS_FUNC QuaternionMember const& quaternion() const { return quaternion_; } // 返回四元数成员
/// 返回 exp(x) 关于 x_i 在 x=0 处的导数。///SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>Dx_exp_x_at_0() { static Scalar const h(0.5); return h * Sophus::Matrix<Scalar, num_parameters, DoF>::Identity(); // 返回导数矩阵}
/// 返回 exp(x) 关于 x 的导数。///SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x( const Tangent& a) { using std::exp; using std::sqrt; Sophus::Matrix<Scalar, num_parameters, DoF> J; Vector3<Scalar> const omega = a.template head<3>(); Scalar const sigma = a[3]; Eigen::Quaternion<Scalar> quat = SO3<Scalar>::exp(omega).unit_quaternion(); Scalar const scale = sqrt(exp(sigma)); Scalar const scale_half = scale * Scalar(0.5);
J.template block<4, 3>(0, 0) = SO3<Scalar>::Dx_exp_x(omega) * scale; J.col(3) = quat.coeffs() * scale_half; return 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) { Sophus::Matrix<Scalar, 3, DoF> j; j << Sophus::SO3<Scalar>::hat(-point), 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); // 返回生成矩阵}
/// 群指数// 该函数接受切空间的一个元素（= 旋转 3 维向量加上缩放的对数）并返回相应的 RxSO3 群元素。// 更具体地说，该函数计算 ``expmat(hat(omega))``，其中 ``expmat(.)`` 是矩阵指数，``hat(.)`` 是 RSO3 的 hat() 操作符。///SOPHUS_FUNC static RxSO3<Scalar> exp(Tangent const& a) { Scalar theta; return expAndTheta(a, &theta); // 返回群指数}
/// 同上，但也返回 ``theta = |omega|`` 作为输出参数。// 前提条件：``theta`` 不能为 ``nullptr``。///SOPHUS_FUNC static RxSO3<Scalar> expAndTheta(Tangent const& a, Scalar* theta) { SOPHUS_ENSURE(theta != nullptr, "必须不为 nullptr。"); using std::exp; using std::max; using std::min; using std::sqrt;
Vector3<Scalar> const omega = a.template head<3>(); Scalar sigma = a[3]; Scalar scale = exp(sigma); // 确保缩放因子约束始终有效 scale = max(scale, Constants<Scalar>::epsilonPlus()); scale = min(scale, Scalar(1.) / Constants<Scalar>::epsilonPlus()); Scalar sqrt_scale = sqrt(scale); Eigen::Quaternion<Scalar> quat = SO3<Scalar>::expAndTheta(omega, theta).unit_quaternion(); quat.coeffs() *= sqrt_scale; return RxSO3<Scalar>(quat); // 返回群指数}
/// 返回 ``R+ x SO(3)`` 的第 i 个无穷小生成元。// RxSO3 的无穷小生成元是：// ```/// | 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 |// | 1 0 0 |/// G_3 = | 0 1 0 |/// | 0 0 1 |/// ```// 前提条件：``i`` 必须是 0，1，2 或 3。///SOPHUS_FUNC static Transformation generator(int i) { SOPHUS_ENSURE(i >= 0 && i <= 3, "i 应该在 [0,3] 范围内。"); Tangent e; e.setZero(); e[i] = Scalar(1); return hat(e); // 返回生成矩阵}
/// hat 操作符// 它接受 4 维向量表示 ``a``（= 旋转向量加上缩放的对数）并返回相应的李代数元素的矩阵表示。// 形式上，RxSO3 的 hat() 操作符定义为// ``hat(.): R^4 -> R^{3x3}, hat(a) = sum_i a_i * G_i`` （对于 i=0,1,2,3）// 其中 ``G_i`` 是 RxSO3 的第 i 个无穷小生成元。// 相应的逆操作是 vee() 操作符，见下文。///SOPHUS_FUNC static Transformation hat(Tangent const& a) { Transformation A; // clang-format off A << a(3), -a(2), a(1), a(2), a(3), -a(0), -a(1), a(0), a(3); // clang-format on return A; // 返回矩阵 A}
/// 李括号// 它计算 RxSO(3) 的李括号。更具体地，它计算// ``[omega_1, omega_2]_rxso3 := vee([hat(omega_1), hat(omega_2)])``// 其中 ``[A,B] := AB-BA`` 是矩阵对易子，``hat(.)`` 是 hat() 操作符，``vee(.)`` 是 RxSO3 的 vee() 操作符。///SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) { Vector3<Scalar> const omega1 = a.template head<3>(); // 获取 a 的前 3 个元素（旋转向量） Vector3<Scalar> const omega2 = b.template head<3>(); // 获取 b 的前 3 个元素（旋转向量） Vector4<Scalar> res; res.template head<3>() = omega1.cross(omega2); // 计算旋转向量的叉积 res[3] = Scalar(0); return res; // 返回结果向量}
/// 从 RxSO(3) 流形中抽取均匀样本。// 缩放因子在 log2 空间中从 [-1, 1] 均匀抽取，因此缩放在 [0.5, 2] 范围内。///template <class UniformRandomBitGenerator>static RxSO3 sampleUniform(UniformRandomBitGenerator& generator) { std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1)); using std::exp2; return RxSO3(exp2(uniform(generator)), SO3<Scalar>::sampleUniform(generator)); // 返回均匀样本}
/// vee 操作符// 它接受 3x3 矩阵表示 ``Omega`` 并将其映射到相应的李代数的向量表示。// 这是 hat() 操作符的逆操作，见上文。// 前提条件：``Omega`` 必须具有以下结构：// | d -c b |/// | c d -a |/// | -b a d |///SOPHUS_FUNC static Tangent vee(Transformation const& Omega) { using std::abs; return Tangent(Omega(2, 1), Omega(0, 2), Omega(1, 0), Omega(0, 0)); // 返回向量表示}
protected:SOPHUS_FUNC QuaternionMember& quaternion_nonconst() { return quaternion_; } // 返回非 const 的四元数
QuaternionMember quaternion_; // 四元数成员};
} // namespace Sophus
namespace Eigen {
/// ``RxSO3`` 的 Eigen::Map 专门化；派生自 RxSO3Base// 允许我们将 RxSO3 对象包装在 POD 数组（例如外部 C 风格的四元数）中。template <class Scalar_, int Options>class Map<Sophus::RxSO3<Scalar_>, Options> : public Sophus::RxSO3Base<Map<Sophus::RxSO3<Scalar_>, Options>> {public: using Base = Sophus::RxSO3Base<Map<Sophus::RxSO3<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`` 是友类，因此 quaternion_nonconst 可以从 ``Base`` 访问。 friend class Sophus::RxSO3Base<Map<Sophus::RxSO3<Scalar_>, Options>>;
using Base::operator=; using Base::operator*=; using Base::operator*;
SOPHUS_FUNC explicit Map(Scalar* coeffs) : quaternion_(coeffs) {} // 显式构造函数
/// 四元数的访问器。 /// SOPHUS_FUNC Map<Eigen::Quaternion<Scalar>, Options> const& quaternion() const { return quaternion_; }
protected: SOPHUS_FUNC Map<Eigen::Quaternion<Scalar>, Options>& quaternion_nonconst() { return quaternion_; }
Map<Eigen::Quaternion<Scalar>, Options> quaternion_; // 四元数成员};
/// ``RxSO3 const`` 的 Eigen::Map 专门化；派生自 RxSO3Base。// 允许我们将 RxSO3 对象包装在 POD 数组（例如外部 C 风格的四元数）中。template <class Scalar_, int Options>class Map<Sophus::RxSO3<Scalar_> const, Options> : public Sophus::RxSO3Base<Map<Sophus::RxSO3<Scalar_> const, Options>> {public: using Base = Sophus::RxSO3Base<Map<Sophus::RxSO3<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) : quaternion_(coeffs) {} // 显式构造函数
/// 四元数的访问器。 /// SOPHUS_FUNC Map<Eigen::Quaternion<Scalar> const, Options> const& quaternion() const { return quaternion_; }
protected: Map<Eigen::Quaternion<Scalar> const, Options> const quaternion_; // 常量四元数成员};} // namespace Eigen- 该文件定义了
RxSO3类，用于表示三维空间的旋转和缩放变换。
类定义
-
RxSO3类表示直接积
R+ x SO(3)，其中R+表示正实数标量组成的群（等价于正实数），SO(3)表示三维特殊正交群（旋转矩阵行列式为 1）。 -
几何上，它表示三维空间的旋转和缩放变换。
-
内部上，
RxSO3类使用非零四元数表示。具体来说，缩放因子等于四元数q的模的平方，s = |q|^2。 -
该类有一个显式的类不变性约束，即缩放因子
s不能太接近零或无穷大。
类的成员函数
-
Adj(): 计算伴随变换。
-
cast<NewScalarType>(): 转换实例类型。
-
data(): 获取/设置内部数据的指针（不安全操作，需使用者谨慎维护四元数的模）。
-
inverse(): 求解反元素。
-
log(): 计算对数映射。
-
logAndTheta(): 计算对数映射并返回旋转向量和缩放因子的模。
-
matrix(): 返回 3x3 矩阵表示。
-
operator=: 赋值运算符。
-
operator*: 群元素的乘法（旋转连接和缩放乘法）。
-
operator*: 作用于 3D 点的变换。
-
operator*: 作用于齐次 3D 点的变换。
-
operator*: 作用于线段的变换。
-
operator*: 作用于平面的变换。
总结
RxSO3 类提供了一种使用四元数表示三维空间的旋转和缩放变换的方法。它具有各种成员函数，可以进行常见的几何变换计算。