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; // 定义导数矩阵 Jstatic 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(); // 计算旋转角 thetaMatrix3<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_domegaMatrix3<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_domegaMatrix3<Scalar> const W = details::calcW<Scalar, 3>(Omega, theta, sigma); // 计算 WJ.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 空间的相似变换,它结合了旋转、缩放和平移操作。这些方法可以用于机器人运动学、计算机视觉和点云处理等领域。