sim3.hpp 文件定义了与 Sim(3) 群相关的类和操作。Sim(3) 群描述了在 3D 空间中的缩放、旋转和平移。以下是对该文件主要内容的总结：
主要类和命名空间
命名空间 Sophus
Sophus命名空间包含了与 Sim(3) 群相关的所有类和函数定义。
类模板 Sim3Base
Sim3Base是一个模板类，实现了 Sim(3) 群的基本操作，但与具体存储无关。包含自由度、内部参数数量、群变换矩阵等静态常量定义。
包含诸如群操作、指数映射、对数映射、伴随变换等成员函数。
类模板 Sim3
Sim3继承自Sim3Base，并使用默认存储实现 Sim(3) 群。提供了一些构造函数，如从四元数和平移向量构造，从 4x4 矩阵构造等。
类模板 Map
提供了
Eigen::Map的特化版本，用于将Sim3对象包装在简单数据数组中，允许高效的内存操作。分别提供了对非 const 和 const
Sim3的特化。
主要函数
构造函数
提供了多种构造函数，包括从缩放因子、四元数和平移向量构造，从 4x4 矩阵构造等。
指数映射和对数映射
实现了指数映射（
exp）和对数映射（log）函数，用于在群元素和李代数元素之间转换。
伴随变换和李括号
实现了伴随变换（
Adj）和李括号（lieBracket）的计算函数。
其他操作
提供了对 RxSO3 和平移向量的访问和修改函数。
实现了
hat和vee运算符，用于在矩阵表示和向量表示之间转换。
重要的常量和类型别名
自由度和参数数量
静态常量
DoF表示自由度，等于 7。静态常量
num_parameters表示内部参数数量，等于 7。
类型别名
定义了如
Transformation、Point、HomogeneousPoint、Tangent、Adjoint等类型别名，方便使用和代码阅读。
通过这些类和函数的定义，sim3.hpp 文件提供了完整的 Sim(3) 群的数学表示和基本操作，使得在 3D 计算机视觉和机器人领域中可以方便地进行 3D 空间中的变换计算。
/// @file/// 相似群 Sim(3) - 3D 中的缩放、旋转和平移。
#pragma once // 防止文件被多次包含
#include "rxso3.hpp" // 包含 RxSO3 的头文件#include "sim_details.hpp" // 包含 sim_details 的头文件
namespace Sophus { // Sophus 命名空间开始template <class Scalar_, int Options = 0>class Sim3; // 声明 Sim3 类模板using Sim3d = Sim3<double>; // 定义 Sim3<double> 类型别名using Sim3f = Sim3<float>; // 定义 Sim3<float> 类型别名} // namespace Sophus // Sophus 命名空间结束
namespace Eigen { // Eigen 命名空间开始namespace internal { // internal 命名空间开始
template <class Scalar_, int Options>struct traits<Sophus::Sim3<Scalar_, Options>> { // 定义 traits 模板结构体 using Scalar = Scalar_; // 定义 Scalar 类型别名 using TranslationType = Sophus::Vector3<Scalar, Options>; // 定义 TranslationType 类型别名 using RxSO3Type = Sophus::RxSO3<Scalar, Options>; // 定义 RxSO3Type 类型别名};
template <class Scalar_, int Options>struct traits<Map<Sophus::Sim3<Scalar_>, Options>> : traits<Sophus::Sim3<Scalar_, Options>> { // traits 结构体的特化 using Scalar = Scalar_; // 定义 Scalar 类型别名 using TranslationType = Map<Sophus::Vector3<Scalar>, Options>; // 定义 TranslationType 类型别名 using RxSO3Type = Map<Sophus::RxSO3<Scalar>, Options>; // 定义 RxSO3Type 类型别名};
template <class Scalar_, int Options>struct traits<Map<Sophus::Sim3<Scalar_> const, Options>> : traits<Sophus::Sim3<Scalar_, Options> const> { // traits 结构体的特化 using Scalar = Scalar_; // 定义 Scalar 类型别名 using TranslationType = Map<Sophus::Vector3<Scalar> const, Options>; // 定义 TranslationType 类型别名 using RxSO3Type = Map<Sophus::RxSO3<Scalar> const, Options>; // 定义 RxSO3Type 类型别名};} // namespace internal // internal 命名空间结束} // namespace Eigen // Eigen 命名空间结束
namespace Sophus { // Sophus 命名空间再次开始
/// Sim3 基础类型 - 实现 Sim3 类，但与存储无关。// Sim(3) 是在 3D 空间中的旋转、平移和缩放的群。它是 R+xSO(3) 和 3D 欧几里得向量空间的半直积。/// 该类通过 RxSO3 的缩放加旋转和 3D 向量的平移来表示。// Sim(3) 既不是紧致的，也不是交换群。// 参见 RxSO3 以获取 3D 空间中缩放和旋转的更多细节。///template <class Derived>class Sim3Base { // Sim3Base 类模板 public: using Scalar = typename Eigen::internal::traits<Derived>::Scalar; // 定义 Scalar 类型别名 using TranslationType = typename Eigen::internal::traits<Derived>::TranslationType; // 定义 TranslationType 类型别名 using RxSO3Type = typename Eigen::internal::traits<Derived>::RxSO3Type; // 定义 RxSO3Type 类型别名 using QuaternionType = typename RxSO3Type::QuaternionType; // 定义 QuaternionType 类型别名
/// 流形的自由度，切空间的维数（平移三维，旋转三维和一个缩放）。 static int constexpr DoF = 7; /// 使用的内部参数数量（四元数的 4 元组，平移的 3 个）。 static int constexpr num_parameters = 7; /// 群变换是 4x4 矩阵。 static int constexpr N = 4; /// 点是 3 维的 static int constexpr Dim = 3; using Transformation = Matrix<Scalar, N, N>; // 定义 Transformation 类型别名 using Point = Vector3<Scalar>; // 定义 Point 类型别名 using HomogeneousPoint = Vector4<Scalar>; // 定义 HomogeneousPoint 类型别名 using Line = ParametrizedLine3<Scalar>; // 定义 Line 类型别名 using Hyperplane = Hyperplane3<Scalar>; // 定义 Hyperplane 类型别名 using Tangent = Vector<Scalar, DoF>; // 定义 Tangent 类型别名 using Adjoint = Matrix<Scalar, DoF, DoF>; // 定义 Adjoint 类型别名
/// 对于二元操作，返回类型通过 Eigen 的 ScalarBinaryOpTraits 特性来确定。 /// 这允许将具体类型和 Map 类型，以及其他兼容的标量类型（如 Ceres::Jet 和 Sim3 操作的 double 标量）混合使用。 template <typename OtherDerived> using ReturnScalar = typename Eigen::ScalarBinaryOpTraits< Scalar, typename OtherDerived::Scalar>::ReturnType; // 定义 ReturnScalar 类型别名
template <typename OtherDerived> using Sim3Product = Sim3<ReturnScalar<OtherDerived>>; // 定义 Sim3Product 类型别名
template <typename PointDerived> using PointProduct = Vector3<ReturnScalar<PointDerived>>; // 定义 PointProduct 类型别名
template <typename HPointDerived> using HomogeneousPointProduct = Vector4<ReturnScalar<HPointDerived>>; // 定义 HomogeneousPointProduct 类型别名
/// 伴随变换 /// /// 该函数返回群元素 ``A`` 的伴随变换 ``Ad``，使得对于所有 ``x`` 都成立 /// ``hat(Ad_A * x) = A * hat(x) A^{-1}``。参见下方的 hat-算子。 /// SOPHUS_FUNC Adjoint Adj() const { // 定义伴随变换函数 Adj Matrix3<Scalar> const R = rxso3().rotationMatrix(); // 获取旋转矩阵 Adjoint res; // 定义 Adjoint 类型变量 res res.setZero(); // 将 res 初始化为零 res.template block<3, 3>(0, 0) = rxso3().matrix(); // 填充 res 的左上角 3x3 块 res.template block<3, 3>(0, 3) = SO3<Scalar>::hat(translation()) * R; // 填充 res 的右上角 3x3 块 res.template block<3, 1>(0, 6) = -translation(); // 填充 res 的最右列
res.template block<3, 3>(3, 3) = R; // 填充 res 的右下角 3x3 块
res(6, 6) = Scalar(1); // 设置 res 的右下角元素 return res; // 返回 res }
/// 返回转换为新标量类型的实例副本。 /// template <class NewScalarType> SOPHUS_FUNC Sim3<NewScalarType> cast() const { // 定义转换函数 cast return Sim3<NewScalarType>(rxso3().template cast<NewScalarType>(), translation().template cast<NewScalarType>()); // 返回转换后的 Sim3 实例 }
/// 返回群逆元素。 /// SOPHUS_FUNC Sim3<Scalar> inverse() const { // 定义逆变换函数 inverse RxSO3<Scalar> invR = rxso3().inverse(); // 计算 rxso3() 的逆 return Sim3<Scalar>(invR, invR * (translation() * Scalar(-1))); // 返回逆变换后的 Sim3 实例 }
/// Logarithmic map /// 对数映射 /// /// Computes the logarithm, the inverse of the group exponential which maps /// element of the group (rigid body transformations) to elements of the /// tangent space (twist). /// /// 计算对数，即群指数的逆运算，将群的元素（刚体变换）映射到切空间的元素（扭转）。 /// /// To be specific, this function computes ``vee(logmat(.))`` with /// ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator /// of Sim(3). /// /// 具体来说，这个函数计算 ``vee(logmat(.))``，其中 ``logmat(.)`` 是矩阵对数，``vee(.)`` 是 Sim(3) 的 vee 运算符。 /// SOPHUS_FUNC Tangent log() const { // The derivation of the closed-form Sim(3) logarithm for is done // analogously to the closed-form solution of the SE(3) logarithm, see // J. Gallier, D. Xu, "Computing exponentials of skew symmetric matrices // and logarithms of orthogonal matrices", IJRA 2002. // https:///pdfs.semanticscholar.org/cfe3/e4b39de63c8cabd89bf3feff7f5449fc981d.pdf // (Sec. 6., pp. 8) // Sim(3) 对数的闭式求解类似于 SE(3) 对数的闭式解法，参见 J. Gallier, D. Xu, "Computing exponentials of skew symmetric matrices // and logarithms of orthogonal matrices", IJRA 2002。 Tangent res; // 定义切空间变量 res auto omega_sigma_and_theta = rxso3().logAndTheta(); // 计算 rxso3() 的对数和角度 Vector3<Scalar> const omega = omega_sigma_and_theta.tangent.template head<3>(); // 提取切向量的前三个分量为 omega Scalar sigma = omega_sigma_and_theta.tangent[3]; // 提取切向量的第四个分量为 sigma Matrix3<Scalar> const Omega = SO3<Scalar>::hat(omega); // 计算 omega 的反对称矩阵 Matrix3<Scalar> const W_inv = details::calcWInv<Scalar, 3>( Omega, omega_sigma_and_theta.theta, sigma, scale()); // 计算 W 的逆
res.segment(0, 3) = W_inv * translation(); // 计算前3个分量 res.segment(3, 3) = omega; // 计算后3个分量 res[6] = sigma; // 设置第7个分量 return res; // 返回结果 }
/// Returns 4x4 matrix representation of the instance. /// 返回实例的 4x4 矩阵表示。 /// /// It has the following form: /// 它的形式如下： /// /// | s*R t | /// | o 1 | /// /// where ``R`` is a 3x3 rotation matrix, ``s`` a scale factor, ``t`` a /// translation 3-vector and ``o`` a 3-column vector of zeros. /// /// 其中 ``R`` 是 3x3 旋转矩阵，``s`` 是缩放因子，``t`` 是 3 维平移向量，``o`` 是 3 列零向量。 /// SOPHUS_FUNC Transformation matrix() const { Transformation homogeneous_matrix; // 定义齐次变换矩阵 homogeneous_matrix.template topLeftCorner<3, 4>() = matrix3x4(); // 设置矩阵的左上角 3x4 块 homogeneous_matrix.row(3) = Matrix<Scalar, 4, 1>(Scalar(0), Scalar(0), Scalar(0), Scalar(1)); // 设置矩阵的最后一行 return homogeneous_matrix; // 返回齐次变换矩阵 }
/// Returns the significant first three rows of the matrix above. /// 返回上面矩阵的前三行。 /// SOPHUS_FUNC Matrix<Scalar, 3, 4> matrix3x4() const { Matrix<Scalar, 3, 4> matrix; // 定义 3x4 矩阵 matrix.template topLeftCorner<3, 3>() = rxso3().matrix(); // 设置矩阵的左上角 3x3 块 matrix.col(3) = translation(); // 设置矩阵的第4列 return matrix; // 返回矩阵 }
/// Assignment-like operator from OtherDerived. /// 从 OtherDerived 赋值的赋值操作符。 /// template <class OtherDerived> SOPHUS_FUNC Sim3Base<Derived>& operator=( Sim3Base<OtherDerived> const& other) { // 赋值操作符函数 rxso3() = other.rxso3(); // 赋值 rxso3 translation() = other.translation(); // 赋值 translation return *this; // 返回当前实例 }
/// Group multiplication, which is rotation plus scaling concatenation. /// 群乘法，即旋转加缩放的连接。 /// /// Note: That scaling is calculated with saturation. See RxSO3 for /// details. /// /// 注意：缩放是通过饱和计算的。详见 RxSO3。 /// template <typename OtherDerived> SOPHUS_FUNC Sim3Product<OtherDerived> operator*( Sim3Base<OtherDerived> const& other) const { // 群乘法操作符函数 return Sim3Product<OtherDerived>( rxso3() * other.rxso3(), translation() + rxso3() * other.translation()); // 返回群乘法结果 }
/// Group action on 3-points. /// 对3点进行群作用。 /// /// This function rotates, scales and translates a three dimensional point /// ``p`` by the Sim(3) element ``(bar_sR_foo, t_bar)`` (= similarity /// transformation): /// /// 这个函数通过 Sim(3) 元素 ``(bar_sR_foo, t_bar)`` （相似变换）旋转、缩放和平移一个三维点 ``p``： /// /// ``p_bar = bar_sR_foo * p_foo + t_bar``. /// template <typename PointDerived, typename = typename std::enable_if_t< IsFixedSizeVector<PointDerived, 3>::value>> SOPHUS_FUNC PointProduct<PointDerived> operator*( Eigen::MatrixBase<PointDerived> const& p) const { return rxso3() * p + translation(); // 计算并返回点的群作用结果 }
/// Group action on homogeneous 3-points. See above for more details. /// 对齐次三维点的群作用。详情见上文。 /// 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 PointProduct<HPointDerived> tp = rxso3() * p.template head<3>() + p(3) * translation(); // 计算 tp return HomogeneousPointProduct<HPointDerived>(tp(0), tp(1), tp(2), p(3)); // 返回齐次点的群作用结果 }
/// Group action on lines. /// 对直线的群作用。 /// /// This function rotates, scales and translates a parametrized line /// ``l(t) = o + t * d`` by the Sim(3) element: /// /// 这个函数通过 Sim(3) 元素旋转、缩放和平移参数化直线 ``l(t) = o + t * d``： /// /// Origin ``o`` is rotated, scaled and translated /// 起点 ``o`` 被旋转、缩放和平移 /// Direction ``d`` is rotated /// 方向 ``d`` 被旋转 /// SOPHUS_FUNC Line operator*(Line const& l) const { Line rotatedLine = rxso3() * l; // 旋转直线 return Line(rotatedLine.origin() + translation(), rotatedLine.direction()); // 返回直线的群作用结果 }
/// Group action on planes./// 对平面的群作用。
/// This function rotates and translates a plane/// ``n.x + d = 0`` by the Sim(3) element:/// 这个函数通过 Sim(3) 元素旋转和平移一个平面 ``n.x + d = 0``。
/// Normal vector ``n`` is rotated/// 法向量 ``n`` 被旋转/// Offset ``d`` is adjusted for scale and translation/// 偏移量 ``d`` 根据缩放和平移进行调整SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const { Hyperplane const rotated = rxso3() * p; // 旋转平面 return Hyperplane(rotated.normal(), rotated.offset() - translation().dot(rotated.normal())); // 返回平面的群作用结果}
/// In-place group multiplication. This method is only valid if the return/// type of the multiplication is compatible with this SO3's Scalar type./// 就地的群乘法操作。此方法仅在乘法的返回类型与此 SO3 的 Scalar 类型兼容时有效。template <typename OtherDerived, typename = typename std::enable_if_t< std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>>SOPHUS_FUNC Sim3Base<Derived>& operator*=( Sim3Base<OtherDerived> const& other) { *static_cast<Derived*>(this) = *this * other; // 更新当前实例为乘法结果 return *this; // 返回当前实例}
/// Returns derivative of this * Sim3::exp(x) w.r.t. x at x = 0/// 返回 this * Sim3::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 J.template block<4, 3>(0, 0).setZero(); // 将矩阵块清零 J.template block<4, 4>(0, 3) = rxso3().Dx_this_mul_exp_x_at_0(); // 设置矩阵块 J.template block<3, 3>(4, 0) = rxso3().matrix(); // 设置矩阵块 J.template block<3, 4>(4, 3).setZero(); // 将矩阵块清零
return J; // 返回导数矩阵}
/// Returns derivative of log(this^{-1} * x) by x at x=this./// 返回 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 J.template block<3, 4>(0, 0).setZero(); // 将矩阵块清零 J.template block<3, 3>(0, 4) = rxso3().inverse().matrix(); // 设置矩阵块 J.template block<4, 4>(3, 0) = rxso3().Dx_log_this_inv_by_x_at_this(); // 设置矩阵块 J.template block<4, 3>(3, 4).setZero(); // 将矩阵块清零 return J; // 返回导数矩阵}
/// Returns internal parameters of Sim(3)./// 返回 Sim(3) 的内部参数。
/// It returns (q.imag[0], q.imag[1], q.imag[2], q.real, t[0], t[1], t[2]),/// with q being the quaternion, t the translation 3-vector./// 它返回 (q.imag[0], q.imag[1], q.imag[2], q.real, t[0], t[1], t[2])，SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const { Sophus::Vector<Scalar, num_parameters> p; // 定义参数向量 p p << rxso3().params(), translation(); // 设置参数向量 return p; // 返回参数向量}
/// Setter of non-zero quaternion./// 设置非零四元数。
/// Precondition: ``quat`` must not be close to zero./// 前提条件：``quat`` 不得接近于零。SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quat) { rxso3().setQuaternion(quat); // 设置四元数}
/// Accessor of quaternion./// 四元数的访问器。SOPHUS_FUNC QuaternionType const& quaternion() const { return rxso3().quaternion(); // 返回四元数}
/// Returns Rotation matrix/// 返回旋转矩阵SOPHUS_FUNC Matrix3<Scalar> rotationMatrix() const { return rxso3().rotationMatrix(); // 返回旋转矩阵}
/// Mutator of SO3 group./// SO3 群的修改器。SOPHUS_FUNC RxSO3Type& rxso3() { return static_cast<Derived*>(this)->rxso3(); // 返回 rxso3}
/// Accessor of SO3 group./// SO3 群的访问器。SOPHUS_FUNC RxSO3Type const& rxso3() const { return static_cast<Derived const*>(this)->rxso3(); // 返回常量 rxso3}
/// Returns scale./// 返回缩放因子。SOPHUS_FUNC Scalar scale() const { return rxso3().scale(); }
/// Setter of quaternion using rotation matrix ``R``, leaves scale as is./// 使用旋转矩阵 ``R`` 设置四元数，保持缩放不变。SOPHUS_FUNC void setRotationMatrix(Matrix3<Scalar> const& R) { rxso3().setRotationMatrix(R); // 设置旋转矩阵}
/// Sets scale and leaves rotation as is./// 设置缩放比例，但保持旋转不变。// Note: This function as a significant computational cost, since it has to/// call the square root twice./// 注意：此函数的计算成本较高，因为它需要调用两次平方根。///SOPHUS_FUNC void setScale(Scalar const& scale) { rxso3().setScale(scale); }
/// Setter of quaternion using scaled rotation matrix ``sR``./// 使用缩放的旋转矩阵 ``sR`` 设置四元数。// Precondition: The 3x3 matrix must be "scaled orthogonal"/// and have a positive determinant./// 前提条件：3x3 矩阵必须是“缩放正交的”，并且有一个正的行列式。///SOPHUS_FUNC void setScaledRotationMatrix(Matrix3<Scalar> const& sR) { rxso3().setScaledRotationMatrix(sR); // 使用缩放的旋转矩阵设置四元数}
/// Mutator of translation vector/// 平移向量的修改器///SOPHUS_FUNC TranslationType& translation() { return static_cast<Derived*>(this)->translation(); // 返回平移向量}
/// Accessor of translation vector/// 平移向量的访问器///SOPHUS_FUNC TranslationType const& translation() const { return static_cast<Derived const*>(this)->translation(); // 返回平移向量}};
/// Sim3 using default storage; derived from Sim3Base./// 使用默认存储的 Sim3；派生自 Sim3Base。template <class Scalar_, int Options>class Sim3 : public Sim3Base<Sim3<Scalar_, Options>> { public: using Base = Sim3Base<Sim3<Scalar_, Options>>; // 使用基类 static int constexpr DoF = Base::DoF; // 定义自由度 static int constexpr num_parameters = Base::num_parameters; // 定义参数数量
using Scalar = Scalar_; // 定义标量类型 using Transformation = typename Base::Transformation; // 定义 Transformation 类型 using Point = typename Base::Point; // 定义 Point 类型 using HomogeneousPoint = typename Base::HomogeneousPoint; // 定义 HomogeneousPoint 类型 using Tangent = typename Base::Tangent; // 定义 Tangent 类型 using Adjoint = typename Base::Adjoint; // 定义 Adjoint 类型 using RxSo3Member = RxSO3<Scalar, Options>; // 定义 RxSo3Member 类型 using TranslationMember = Vector3<Scalar, Options>; // 定义 TranslationMember 类型
using Base::operator=; // 使用基类的赋值操作符
/// Define copy-assignment operator explicitly. The definition of /// implicit copy assignment operator is deprecated in presence of a /// user-declared copy constructor (-Wdeprecated-copy in clang >= 13). /// 显式定义复制赋值操作符。由于存在用户声明的复制构造函数，隐式复制赋值操作符的定义已被弃用（在 clang >= 13 中为 -Wdeprecated-copy）。 SOPHUS_FUNC Sim3& operator=(Sim3 const& other) = default;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // 强制 Eigen 的内存对齐
/// Default constructor initializes similarity transform to the identity. /// 默认构造函数将相似变换初始化为单位变换。 /// SOPHUS_FUNC Sim3();
/// Copy constructor /// 复制构造函数 /// SOPHUS_FUNC Sim3(Sim3 const& other) = default;
/// Copy-like constructor from OtherDerived. /// 从 OtherDerived 复制的类似构造函数。 /// template <class OtherDerived> SOPHUS_FUNC Sim3(Sim3Base<OtherDerived> const& other) : rxso3_(other.rxso3()), translation_(other.translation()) { static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value, "must be same Scalar type"); // 编译时检查标量类型是否相同 }
/// Constructor from RxSO3 and translation vector /// 从 RxSO3 和平移向量构造 /// template <class OtherDerived, class D> SOPHUS_FUNC explicit Sim3(RxSO3Base<OtherDerived> const& rxso3, Eigen::MatrixBase<D> const& translation) : rxso3_(rxso3), 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"); // 编译时检查标量类型是否相同 }
/// Constructor from quaternion and translation vector. /// 从四元数和平移向量构造。 /// /// Precondition: quaternion must not be close to zero. /// 前提条件：四元数不得接近零。 /// template <class D1, class D2> SOPHUS_FUNC explicit Sim3(Eigen::QuaternionBase<D1> const& quaternion, Eigen::MatrixBase<D2> const& translation) : rxso3_(quaternion), translation_(translation) { static_assert(std::is_same<typename D1::Scalar, Scalar>::value, "must be same Scalar type"); // 编译时检查标量类型是否相同 static_assert(std::is_same<typename D2::Scalar, Scalar>::value, "must be same Scalar type"); // 编译时检查标量类型是否相同 }/// Constructor from scale factor, unit quaternion, and translation vector./// 从缩放因子、单位四元数和平移向量构造函数。// Precondition: quaternion must not be close to zero./// 前提条件：四元数不能接近零。///template <class D1, class D2>SOPHUS_FUNC explicit Sim3(Scalar const& scale, Eigen::QuaternionBase<D1> const& unit_quaternion, Eigen::MatrixBase<D2> const& translation) : Sim3(RxSO3<Scalar>(scale, unit_quaternion), translation) {}/// Constructor from 4x4 matrix/// 从4x4矩阵构造函数。// Precondition: Top-left 3x3 matrix needs to be "scaled-orthogonal" with/// positive determinant. The last row must be ``(0, 0, 0, 1)``./// 前提条件：左上角3x3矩阵必须是“缩放正交”的，且行列式为正。最后一行必须是 ``(0, 0, 0, 1)``。///SOPHUS_FUNC explicit Sim3(Matrix<Scalar, 4, 4> const& T) : rxso3_(T.template topLeftCorner<3, 3>()), translation_(T.template block<3, 1>(0, 3)) {}/// This provides unsafe read/write access to internal data. Sim(3) is/// represented by an Eigen::Quaternion (four parameters) and a 3-vector. When/// using direct write access, the user needs to take care of that the/// quaternion is not set close to zero./// 这提供了对内部数据的不安全读/写访问。Sim(3) 由 Eigen::Quaternion（四个参数）和一个3维向量表示。/// 使用直接写访问时，用户需要确保四元数不接近于零。///SOPHUS_FUNC Scalar* data() { // rxso3_ 和 translation_ 顺序排列，没有填充 return rxso3_.data();}/// Const version of data() above./// 上述 data() 的常量版本。///SOPHUS_FUNC Scalar const* data() const { // rxso3_ 和 translation_ 顺序排列，没有填充 return rxso3_.data();}/// Accessor of RxSO3/// RxSO3 的访问器。///SOPHUS_FUNC RxSo3Member& rxso3() { return rxso3_; }/// Mutator of RxSO3/// RxSO3 的修改器。///SOPHUS_FUNC RxSo3Member const& rxso3() const { return rxso3_; }/// Mutator of translation vector/// 平移向量的修改器。///SOPHUS_FUNC TranslationMember& translation() { return translation_; }/// Accessor of translation vector/// 平移向量的访问器。///SOPHUS_FUNC TranslationMember const& translation() const { return translation_;}/// Returns derivative of exp(x) wrt. x_i at x=0./// 返回 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.template block<4, 3>(0, 0).setZero(); J.template block<4, 4>(0, 3) = RxSO3<Scalar>::Dx_exp_x_at_0(); J.template block<3, 3>(4, 0).setIdentity(); J.template block<3, 4>(4, 3).setZero(); return J; } /// Returns derivative of exp(x) wrt. x./// 返回 exp(x) 对 x 的导数。///SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x( const Tangent& a) { Sophus::Matrix<Scalar, num_parameters, DoF> J; // 定义导数矩阵 J
static Matrix3<Scalar> const I = Matrix3<Scalar>::Identity(); // 定义单位矩阵 I Vector3<Scalar> const omega = a.template segment<3>(3); // 提取旋转向量 omega Vector3<Scalar> const upsilon = a.template head<3>(); // 提取平移向量 upsilon Scalar const sigma = a[6]; // 提取缩放因子 sigma Scalar const theta = omega.norm(); // 计算旋转角 theta
Matrix3<Scalar> const Omega = SO3<Scalar>::hat(omega); // 计算 omega 的反对称矩阵 Matrix3<Scalar> const Omega2 = Omega * Omega; // 计算 Omega 的平方 Vector3<Scalar> theta_domega; if (theta < Constants<Scalar>::epsilon()) { // 如果 theta 小于一个小值 theta_domega = Vector3<Scalar>::Zero(); // 将 theta_domega 设为零 } else { theta_domega = omega / theta; // 否则，计算 theta_domega } static Matrix3<Scalar> const Omega_domega[3] = { SO3<Scalar>::hat(Vector3<Scalar>::Unit(0)), SO3<Scalar>::hat(Vector3<Scalar>::Unit(1)), SO3<Scalar>::hat(Vector3<Scalar>::Unit(2))}; // 定义 Omega_domega
Matrix3<Scalar> const Omega2_domega[3] = { Omega_domega[0] * Omega + Omega * Omega_domega[0], Omega_domega[1] * Omega + Omega * Omega_domega[1], Omega_domega[2] * Omega + Omega * Omega_domega[2]}; // 定义 Omega2_domega
Matrix3<Scalar> const W = details::calcW<Scalar, 3>(Omega, theta, sigma); // 计算 W
J.template block<4, 3>(0, 0).setZero(); // 将 J 的前 4x3 块清零 J.template block<4, 4>(0, 3) = RxSO3<Scalar>::Dx_exp_x(a.template tail<4>()); // 设置 J 的 4x4 块 J.template block<3, 4>(4, 3).setZero(); // 将 J 的 3x4 块清零 J.template block<3, 3>(4, 0) = W; // 设置 J 的 3x3 块
Scalar A, B, C, A_dtheta, B_dtheta, A_dsigma, B_dsigma, C_dsigma; details::calcW_derivatives(theta, sigma, A, B, C, A_dsigma, B_dsigma, C_dsigma, A_dtheta, B_dtheta); // 计算 W 的导数
for (int i = 0; i < 3; ++i) { J.template block<3, 1>(4, 3 + i) = (A_dtheta * theta_domega[i] * Omega + A * Omega_domega[i] + B_dtheta * theta_domega[i] * Omega2 + B * Omega2_domega[i]) * upsilon; // 设置 J 的 3x1 块 }
J.template block<3, 1>(4, 6) = (A_dsigma * Omega + B_dsigma * Omega2 + C_dsigma * I) * upsilon; // 设置 J 的 3x1 块
return J; // 返回导数矩阵 J}
/// Returns derivative of exp(x) * p wrt. x_i at x=0./// 返回 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 J << Sophus::Matrix3<Scalar>::Identity(), Sophus::RxSO3<Scalar>::Dx_exp_x_times_point_at_0(point); // 设置 J return J; // 返回导数矩阵 J}
/// Returns derivative of exp(x).matrix() wrt. ``x_i at x=0``./// 返回 exp(x).matrix() 对 ``x_i 在 x=0 处的导数``。///SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) { return generator(i); // 返回生成器}
/// Group exponential/// 群指数// This functions takes in an element of tangent space and returns the/// corresponding element of the group Sim(3)./// 该函数接收切空间的一个元素并返回群 Sim(3) 的对应元素。// The first three components of ``a`` represent the translational part/// ``upsilon`` in the tangent space of Sim(3), the following three components/// of ``a`` represents the rotation vector ``omega`` and the final component/// represents the logarithm of the scaling factor ``sigma``./// To be more specific, this function computes ``expmat(hat(a))`` with/// ``expmat(.)`` being the matrix exponential and ``hat(.)`` the hat-operator/// of Sim(3), see below./// ``a`` 的前三个分量表示 Sim(3) 切空间中的平移部分 ``upsilon``，接下来的三个分量表示旋转向量 ``omega``，最后一个分量表示缩放因子的对数 ``sigma``。/// 更具体地说，该函数计算 ``expmat(hat(a))``，其中 ``expmat(.)`` 是矩阵指数，``hat(.)`` 是 Sim(3) 的 hat 运算符。///SOPHUS_FUNC static Sim3<Scalar> exp(Tangent const& a) { // For the derivation of the exponential map of Sim(3) see // H. Strasdat, "Local Accuracy and Global Consistency for Efficient Visual // SLAM", PhD thesis, 2012. // http:///hauke.strasdat.net/files/strasdat_thesis_2012.pdf (A.5, pp. 186) // 有关 Sim(3) 指数映射的推导，请参阅 H. Strasdat, "Local Accuracy and Global Consistency for Efficient Visual // SLAM", PhD 论文, 2012。 Vector3<Scalar> const upsilon = a.segment(0, 3); // 提取平移向量 upsilon Vector3<Scalar> const omega = a.segment(3, 3); // 提取旋转向量 omega Scalar const sigma = a[6]; // 提取缩放因子 sigma Scalar theta; RxSO3<Scalar> rxso3 = RxSO3<Scalar>::expAndTheta(a.template tail<4>(), &theta); // 计算指数映射和角度 theta Matrix3<Scalar> const Omega = SO3<Scalar>::hat(omega); // 计算 omega 的反对称矩阵
Matrix3<Scalar> const W = details::calcW<Scalar, 3>(Omega, theta, sigma); // 计算 W return Sim3<Scalar>(rxso3, W * upsilon); // 返回 Sim3 实例}
/// Returns the ith infinitesimal generators of Sim(3)./// 返回 Sim(3) 的第 i 个无穷小生成元。// The infinitesimal generators of Sim(3) are:/// Sim(3) 的无穷小生成元是：// ```/// | 0 0 0 1 |/// G_0 = | 0 0 0 0 |/// | 0 0 0 0 |/// | 0 0 0 0 |// | 0 0 0 0 |/// G_1 = | 0 0 0 1 |/// | 0 0 0 0 |/// | 0 0 0 0 |// | 0 0 0 0 |/// G_2 = | 0 0 0 0 |/// | 0 0 0 1 |/// | 0 0 0 0 |// | 0 0 0 0 |/// G_3 = | 0 0 -1 0 |/// | 0 1 0 0 |/// | 0 0 0 0 |// | 0 0 1 0 |/// G_4 = | 0 0 0 0 |/// | -1 0 0 0 |/// | 0 0 0 0 |// | 0 -1 0 0 |/// G_5 = | 1 0 0 0 |/// | 0 0 0 0 |/// | 0 0 0 0 |// | 1 0 0 0 |/// G_6 = | 0 1 0 0 |/// | 0 0 1 0 |/// | 0 0 0 0 |/// ```// Precondition: ``i`` must be in [0, 6]./// 前提条件：``i`` 必须在 [0, 6] 之间。///SOPHUS_FUNC static Transformation generator(int i) { SOPHUS_ENSURE(i >= 0 || i <= 6, "i should be in range [0,6]."); // 确保 i 在 [0, 6] 范围内 Tangent e; e.setZero(); // 将 e 初始化为零向量 e[i] = Scalar(1); // 设置 e 的第 i 个分量为 1 return hat(e); // 返回 hat(e)}
/// hat-operator/// hat 运算符// It takes in the 7-vector representation and returns the corresponding/// matrix representation of Lie algebra element./// 它接受 7 维向量表示并返回对应的李代数元素的矩阵表示。// Formally, the hat()-operator of Sim(3) is defined as/// 正式来说，Sim(3) 的 hat() 运算符定义为// ``hat(.): R^7 -> R^{4x4}, hat(a) = sum_i a_i * G_i`` (for i=0,...,6)/// ``hat(.): R^7 -> R^{4x4}, hat(a) = sum_i a_i * G_i`` (i=0,...,6)// with ``G_i`` being the ith infinitesimal generator of Sim(3)./// 其中 ``G_i`` 是 Sim(3) 的第 i 个无穷小生成元。// The corresponding inverse is the vee()-operator, see below./// 对应的逆运算是 vee() 运算符，见下文。///SOPHUS_FUNC static Transformation hat(Tangent const& a) { Transformation Omega; Omega.template topLeftCorner<3, 3>() = RxSO3<Scalar>::hat(a.template tail<4>()); // 设置 Omega 的左上角 3x3 块为 RxSO3 的 hat(a.tail<4>()) Omega.col(3).template head<3>() = a.template head<3>(); // 设置 Omega 的第 3 列 Omega.row(3).setZero(); // 设置 Omega 的第 3 行 return Omega; // 返回 Omega}
/// Lie bracket/// 李括号// It computes the Lie bracket of Sim(3). To be more specific, it computes/// 它计算 Sim(3) 的李括号。具体来说，它计算// ``[omega_1, omega_2]_sim3 := vee([hat(omega_1), hat(omega_2)])``/// ``[omega_1, omega_2]_sim3 := vee([hat(omega_1), hat(omega_2)])``// with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.)`` the/// hat()-operator and ``vee(.)`` the vee()-operator of Sim(3)./// 其中 ``[A,B] := AB-BA`` 是矩阵的交换子，``hat(.)`` 是 hat() 运算符，``vee(.)`` 是 Sim(3) 的 vee() 运算符。///SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) { Vector3<Scalar> const upsilon1 = a.template head<3>(); // 提取 a 的前三个分量 Vector3<Scalar> const upsilon2 = b.template head<3>(); // 提取 b 的前三个分量 Vector3<Scalar> const omega1 = a.template segment<3>(3); // 提取 a 的中间三个分量 Vector3<Scalar> const omega2 = b.template segment<3>(3); // 提取 b 的中间三个分量 Scalar sigma1 = a[6]; // 提取 a 的第七个分量 Scalar sigma2 = b[6]; // 提取 b 的第七个分量
Tangent res; res.template head<3>() = SO3<Scalar>::hat(omega1) * upsilon2 + SO3<Scalar>::hat(upsilon1) * omega2 + sigma1 * upsilon2 - sigma2 * upsilon1; // 计算前三个分量 res.template segment<3>(3) = omega1.cross(omega2); // 计算中间三个分量 res[6] = Scalar(0); // 计算第七个分量
return res; // 返回结果}
/// Draw uniform sample from Sim(3) manifold./// 从 Sim(3) 流形中绘制均匀样本。// Translations are drawn component-wise from the range [-1, 1]./// 平移是从 [-1, 1] 范围内逐个分量绘制的。/// The scale factor is drawn uniformly in log2-space from [-1, 1],/// hence the scale is in [0.5, 2]./// 缩放因子在 log2 空间中从 [-1, 1] 均匀绘制，因此缩放在 [0.5, 2] 之间。///template <class UniformRandomBitGenerator>static Sim3 sampleUniform(UniformRandomBitGenerator& generator) { std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1)); // 定义均匀分布 return Sim3(RxSO3<Scalar>::sampleUniform(generator), Vector3<Scalar>(uniform(generator), uniform(generator), uniform(generator))); // 返回 Sim3 实例}
/// vee-operator/// vee 运算符// It takes the 4x4-matrix representation ``Omega`` and maps it to the/// corresponding 7-vector representation of Lie algebra./// 它接受 4x4 矩阵表示 ``Omega`` 并将其映射到相应的李代数的 7 维向量表示。// This is the inverse of the hat()-operator, see above./// 这是 hat() 运算符的逆运算，见上文。// Precondition: ``Omega`` must have the following structure:/// 前提条件：``Omega`` 必须具有以下结构：// | g -f e a |/// | f g -d b |/// | -e d g c |/// | 0 0 0 0 |///SOPHUS_FUNC static Tangent vee(Transformation const& Omega) { Tangent upsilon_omega_sigma; upsilon_omega_sigma.template head<3>() = Omega.col(3).template head<3>(); // 提取 Omega 的第 3 列的前三个分量 upsilon_omega_sigma.template tail<4>() = RxSO3<Scalar>::vee(Omega.template topLeftCorner<3, 3>()); // 提取 Omega 的左上角 3x3 块并应用 vee 运算符 return upsilon_omega_sigma; // 返回 upsilon_omega_sigma } protected: RxSo3Member rxso3_; // 定义 RxSO3 成员变量 TranslationMember translation_; // 定义平移成员变量};
template <class Scalar, int Options>SOPHUS_FUNC Sim3<Scalar, Options>::Sim3() : translation_(TranslationMember::Zero()) { // 初始化平移成员变量为零向量 static_assert(std::is_standard_layout<Sim3>::value, "Assume standard layout for the use of offset of check below."); // 检查 Sim3 是否具有标准布局 static_assert( offsetof(Sim3, rxso3_) + sizeof(Scalar) * RxSO3<Scalar>::num_parameters == offsetof(Sim3, 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."); // 检查数据布局是否连续}
} // namespace Sophus // 结束 Sophus 命名空间
namespace Eigen { // 开始 Eigen 命名空间
/// Specialization of Eigen::Map for ``Sim3``; derived from Sim3Base./// 对 ``Sim3`` 的 Eigen::Map 的特化；派生自 Sim3Base。// Allows us to wrap Sim3 objects around POD array./// 允许我们将 Sim3 对象包装在 POD 数组中。template <class Scalar_, int Options>class Map<Sophus::Sim3<Scalar_>, Options> : public Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_>, Options>> { public: using Base = Sophus::Sim3Base<Map<Sophus::Sim3<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;
using Base::operator=; // 使用基类的赋值操作符 using Base::operator*=; // 使用基类的乘法赋值操作符 using Base::operator*; // 使用基类的乘法操作符
SOPHUS_FUNC explicit Map(Scalar* coeffs) : rxso3_(coeffs), translation_(coeffs + Sophus::RxSO3<Scalar>::num_parameters) {} // 初始化 RxSO3 和平移成员变量
/// Mutator of RxSO3 /// RxSO3 的修改器 /// SOPHUS_FUNC Map<Sophus::RxSO3<Scalar>, Options>& rxso3() { return rxso3_; }
/// Accessor of RxSO3 /// RxSO3 的访问器 /// SOPHUS_FUNC Map<Sophus::RxSO3<Scalar>, Options> const& rxso3() const { return rxso3_; }
/// Mutator of translation vector /// 平移向量的修改器 /// SOPHUS_FUNC Map<Sophus::Vector3<Scalar>, Options>& translation() { return translation_; }
/// Accessor of translation vector /// 平移向量的访问器 SOPHUS_FUNC Map<Sophus::Vector3<Scalar>, Options> const& translation() const { return translation_; }
protected: Map<Sophus::RxSO3<Scalar>, Options> rxso3_; // 定义 RxSO3 成员变量 Map<Sophus::Vector3<Scalar>, Options> translation_; // 定义平移成员变量};
/// Specialization of Eigen::Map for ``Sim3 const``; derived from Sim3Base./// 对 ``Sim3 const`` 的 Eigen::Map 的特化；派生自 Sim3Base。// Allows us to wrap RxSO3 objects around POD array./// 允许我们将 RxSO3 对象包装在 POD 数组中。template <class Scalar_, int Options>class Map<Sophus::Sim3<Scalar_> const, Options> : public Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_> const, Options>> { using Base = Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_> const, Options>>;
public: 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) : rxso3_(coeffs), translation_(coeffs + Sophus::RxSO3<Scalar>::num_parameters) {} // 初始化 RxSO3 和平移成员变量
/// Accessor of RxSO3 /// RxSO3 的访问器 /// SOPHUS_FUNC Map<Sophus::RxSO3<Scalar> const, Options> const& rxso3() const { return rxso3_; }
/// Accessor of translation vector /// 平移向量的访问器 /// SOPHUS_FUNC Map<Sophus::Vector3<Scalar> const, Options> const& translation() const { return translation_; }
protected: Map<Sophus::RxSO3<Scalar> const, Options> const rxso3_; // 定义常量 RxSO3 成员变量 Map<Sophus::Vector3<Scalar> const, Options> const translation_; // 定义常量平移成员变量};} // namespace Eigen // 结束 Eigen 命名空间这是 Sophus 库中 Sim3 类的总结。Sim3 类表示 3D 空间的相似变换，它包含旋转、缩放和平移操作。
类属性
Scalar: 表示数值类型，例如 double 或 float。
TranslationType: 表示平移向量的类型，是 Sophus::Vector3。
RxSO3Type: 表示旋转和平缩放的类型，是 Sophus::RxSO3。
DoF: 自由度，表示切空间的维度，为 7 (3 个平移，3 个旋转，1 个缩放)。
num_parameters: 内部参数个数，为 7 (4 个表示旋转的四元数，3 个表示平移)。
N: 齐次矩阵的维度，为 4。
Dim: 点的维度，为 3。
类方法
Adj(): 计算伴随变换。
cast<NewScalarType>(): 转换实例的数值类型。
inverse(): 计算变换的逆变换。
log(): 计算对数映射，即从群元素变换到切空间 (twist)。
matrix(): 返回 4x4 的齐次矩阵表示。
matrix3x4(): 返回矩阵的前三行，表示 3x4 的变换矩阵。
operator=: 赋值操作符，从其他类型转换赋值。
operator*: 群元素的乘法运算，对应旋转和平移的连接。
operator*(PointDerived): 作用于 3D 点的变换。
operator*(HPointDerived): 作用于齐次 3D 点的变换。
operator*(Line): 作用于线段的变换。
operator*(Hyperplane): 作用于平面的变换。
总结
Sim3 类用于表示和操作 3D 空间的相似变换，它结合了旋转、缩放和平移操作。这些方法可以用于机器人运动学、计算机视觉和点云处理等领域。
