sim2.hpp 是一个定义了相似性变换 Sim(2) 相关类和函数的头文件。以下是它的主要内容和功能的总结：
引入头文件：
包含了
rxso2.hpp和sim_details.hpp头文件，用于 RxSO2 和相似性变换的实现细节。
命名空间 Sophus：
声明了模板类
Sim2，并定义了Sim2d和Sim2f为Sim2<double>和Sim2<float>的别名。
命名空间 Eigen：
特化了
Eigen::Map模板，用于包裹Sim2对象，允许对普通数据进行包装。特化了
Eigen::internal::traits模板，用于描述Sim2类型的属性，包括标量类型、平移类型和 RxSO2 类型。
Sim2 基类：
Sim2Base类实现了 Sim2 的基础功能，包括平移、缩放、旋转的变换和计算。定义了 Sim2 的自由度、参数数量、矩阵维度、点和切向量的类型。
提供了指数映射、对数映射、伴随变换、逆变换等函数。
Sim2 类：
Sim2类继承自Sim2Base，使用默认存储方式。定义了构造函数、复制构造函数和从其他类型复制的构造函数。
提供了对内存数据的不安全读写访问。
Sim2 变换相关操作：
提供了对 RxSO2 和平移向量的访问器和修改器。
提供了计算李括号、指数映射、对数映射等函数。
提供了群运算符重载，包括群乘法、点和齐次点的变换操作。
其他工具函数：
提供了
hat算子和vee算子，进行向量与矩阵之间的变换。提供了生成无穷小生成元的函数和从 Sim(2) 流形中抽取均匀样本的函数。
总体而言，sim2.hpp 文件定义了一个用于二维空间中相似性变换的类和相关操作，主要包括旋转、缩放和平移的组合。通过这个头文件，用户可以方便地在 C++ 程序中使用和操作 Sim(2) 变换。
/// @file/// Similarity group Sim(2) - scaling, rotation and translation in 2d. /// @文件/// 相似群 Sim(2) - 2D 中的缩放、旋转和平移。
#pragma once// 防止文件被多次包含
#include "rxso2.hpp"// 包含头文件 "rxso2.hpp"
#include "sim_details.hpp"// 包含头文件 "sim_details.hpp"
namespace Sophus {// 定义命名空间 Sophustemplate <class Scalar_, int Options = 0>class Sim2;// 声明模板类 Sim2，带有类型参数 Scalar_ 和整数参数 Options（默认值为0）
using Sim2d = Sim2<double>;// 定义 Sim2d 为 Sim2<double> 的别名
using Sim2f = Sim2<float>;// 定义 Sim2f 为 Sim2<float> 的别名} // namespace Sophus// 结束命名空间 Sophus
namespace Eigen {// 定义命名空间 Eigennamespace internal {// 定义命名空间 internal
template <class Scalar_, int Options>struct traits<Sophus::Sim2<Scalar_, Options>> {// 特化模板 struct traits，用于 Sophus::Sim2 using Scalar = Scalar_;// 定义类型别名 Scalar，等于模板参数 Scalar_ using TranslationType = Sophus::Vector2<Scalar, Options>;// 定义类型别名 TranslationType，等于 Sophus::Vector2<Scalar, Options> using RxSO2Type = Sophus::RxSO2<Scalar, Options>;// 定义类型别名 RxSO2Type，等于 Sophus::RxSO2<Scalar, Options>};
template <class Scalar_, int Options>struct traits<Map<Sophus::Sim2<Scalar_>, Options>> : traits<Sophus::Sim2<Scalar_, Options>> {// 特化模板 struct traits，用于 Map 类型的 Sophus::Sim2// 并继承 traits<Sophus::Sim2<Scalar_, Options>> using Scalar = Scalar_;// 定义类型别名 Scalar，等于模板参数 Scalar_ using TranslationType = Map<Sophus::Vector2<Scalar>, Options>;// 定义类型别名 TranslationType，等于 Map 类型的 Sophus::Vector2 using RxSO2Type = Map<Sophus::RxSO2<Scalar>, Options>;// 定义类型别名 RxSO2Type，等于 Map 类型的 Sophus::RxSO2};
template <class Scalar_, int Options>struct traits<Map<Sophus::Sim2<Scalar_> const, Options>> : traits<Sophus::Sim2<Scalar_, Options> const> {// 特化模板 struct traits，用于 Map 类型且 const 的 Sophus::Sim2// 并继承 traits<Sophus::Sim2<Scalar_, Options> const> using Scalar = Scalar_;// 定义类型别名 Scalar，等于模板参数 Scalar_ using TranslationType = Map<Sophus::Vector2<Scalar> const, Options>;// 定义类型别名 TranslationType，等于 Map 类型且 const 的 Sophus::Vector2 using RxSO2Type = Map<Sophus::RxSO2<Scalar> const, Options>;// 定义类型别名 RxSO2Type，等于 Map 类型且 const 的 Sophus::RxSO2};} // namespace internal// 结束命名空间 internal} // namespace Eigen// 结束命名空间 Eigen
namespace Sophus {// 定义命名空间 Sophus
/// Sim2 base type - implements Sim2 class but is storage agnostic./// Sim2 基础类型 - 实现 Sim2 类但与存储无关。
////// Sim(2) is the group of rotations and translation and scaling in 2d. It is/// the semi-direct product of R+xSO(2) and the 2d Euclidean vector space. The/// class is represented using a composition of RxSO2 for scaling plus/// rotation and a 2-vector for translation./// Sim(2) 是在二维空间中旋转、平移和缩放的群。它是 R+xSO(2) 与二维欧几里得向量空间的半直积。/// 该类使用 RxSO2 表示缩放和旋转，并使用二维向量表示平移。
////// Sim(2) is neither compact, nor a commutative group./// Sim(2) 既不是紧致的，也不是交换群。
////// See RxSO2 for more details of the scaling + rotation representation in 2d./// 有关二维空间中缩放和旋转表示的详细信息，请参见 RxSO2。
///template <class Derived>class Sim2Base {// 声明模板类 Sim2Base
public: using Scalar = typename Eigen::internal::traits<Derived>::Scalar;// 定义类型别名 Scalar，等于 Eigen::internal::traits<Derived>::Scalar using TranslationType = typename Eigen::internal::traits<Derived>::TranslationType;// 定义类型别名 TranslationType，等于 Eigen::internal::traits<Derived>::TranslationType using RxSO2Type = typename Eigen::internal::traits<Derived>::RxSO2Type;// 定义类型别名 RxSO2Type，等于 Eigen::internal::traits<Derived>::RxSO2Type
/// Degrees of freedom of manifold, number of dimensions in tangent space /// (two for translation, one for rotation and one for scaling). static int constexpr DoF = 4; /// 流形的自由度，切空间的维度数量（平移2个，旋转1个，缩放1个）。
/// Number of internal parameters used (2-tuple for complex number, two for /// translation). static int constexpr num_parameters = 4; /// 使用的内部参数数量（复数为2元组，平移为2）
/// Group transformations are 3x3 matrices. static int constexpr N = 3; /// 群变换为3x3矩阵。
/// Points are 2-dimensional static int constexpr Dim = 2; /// 点为二维。
using Transformation = Matrix<Scalar, N, N>;// 定义类型别名 Transformation，等于 Matrix<Scalar, N, N> using Point = Vector2<Scalar>;// 定义类型别名 Point，等于 Vector2<Scalar> using HomogeneousPoint = Vector3<Scalar>;// 定义类型别名 HomogeneousPoint，等于 Vector3<Scalar> using Line = ParametrizedLine2<Scalar>;// 定义类型别名 Line，等于 ParametrizedLine2<Scalar> using Hyperplane = Hyperplane2<Scalar>;// 定义类型别名 Hyperplane，等于 Hyperplane2<Scalar> using Tangent = Vector<Scalar, DoF>;// 定义类型别名 Tangent，等于 Vector<Scalar, DoF> using Adjoint = Matrix<Scalar, DoF, DoF>;// 定义类型别名 Adjoint，等于 Matrix<Scalar, DoF, DoF>
/// For binary operations the return type is determined with the /// ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map /// types, as well as other compatible scalar types such as Ceres::Jet and /// double scalars with SIM2 operations. template <typename OtherDerived> using ReturnScalar = typename Eigen::ScalarBinaryOpTraits< Scalar, typename OtherDerived::Scalar>::ReturnType; /// 对于二元操作，返回类型由 Eigen 的 ScalarBinaryOpTraits 特性决定。 /// 这允许混合具体和 Map 类型，以及其他兼容的标量类型，如 Ceres::Jet 和 double 标量。
template <typename OtherDerived> using Sim2Product = Sim2<ReturnScalar<OtherDerived>>;// 定义模板别名 Sim2Product，返回 Sim2<ReturnScalar<OtherDerived>>
template <typename PointDerived> using PointProduct = Vector2<ReturnScalar<PointDerived>>;// 定义模板别名 PointProduct，返回 Vector2<ReturnScalar<PointDerived>>
template <typename HPointDerived> using HomogeneousPointProduct = Vector3<ReturnScalar<HPointDerived>>;// 定义模板别名 HomogeneousPointProduct，返回 Vector3<ReturnScalar<HPointDerived>>
/// Adjoint transformation /// /// This function return the adjoint transformation ``Ad`` of the group /// element ``A`` such that for all ``x`` it holds that /// ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below. /// /// 伴随变换 /// /// 此函数返回群元素 ``A`` 的伴随变换 ``Ad``，对于所有 ``x`` 都满足 /// ``hat(Ad_A * x) = A * hat(x) A^{-1}``。详见下文的帽算子。
SOPHUS_FUNC Adjoint Adj() const { Adjoint res; res.setZero(); res.template block<2, 2>(0, 0) = rxso2().matrix(); res(0, 2) = translation()[1]; res(1, 2) = -translation()[0]; res.template block<2, 1>(0, 3) = -translation();
res(2, 2) = Scalar(1); // 设置伴随矩阵 res 的 (2, 2) 元素为 1
res(3, 3) = Scalar(1); // 设置伴随矩阵 res 的 (3, 3) 元素为 1 return res; // 返回计算出的伴随矩阵 res }
/// Returns copy of instance casted to NewScalarType. /// 返回实例的副本，并将其转换为 NewScalarType 类型。 template <class NewScalarType> SOPHUS_FUNC Sim2<NewScalarType> cast() const { return Sim2<NewScalarType>(rxso2().template cast<NewScalarType>(), translation().template cast<NewScalarType>()); // 返回一个新的 Sim2<NewScalarType> 实例，并将内部的 rxso2 和 translation 成员转换为 NewScalarType 类型。 }
/// Returns group inverse. /// 返回群的逆元素。 SOPHUS_FUNC Sim2<Scalar> inverse() const { RxSO2<Scalar> invR = rxso2().inverse(); // 计算 rxso2() 的逆，并存储在 invR 中 return Sim2<Scalar>(invR, invR * (translation() * Scalar(-1))); // 返回一个新的 Sim2 实例，其中 rxso2 为 invR，translation 为 invR 乘以 -translation() }
/// 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(2). /// 具体来说，这个函数计算 ``vee(logmat(.))``，其中 ``logmat(.)`` 是矩阵对数，``vee(.)`` 是 Sim(2) 的对算子。 /// SOPHUS_FUNC Tangent log() const { /// The derivation of the closed-form Sim(2) logarithm for is done /// analogously to the closed-form solution of the SE(2) 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(2) 对数的闭形式推导类似于 SE(2) 对数的闭形式解，参见 /// J. Gallier, D. Xu, "计算斜对称矩阵的指数和正交矩阵的对数", IJRA 2002. /// https:///pdfs.semanticscholar.org/cfe3/e4b39de63c8cabd89bf3feff7f5449fc981d.pdf /// (第6部分，第8页) Tangent res; // 定义 Tangent 类型的变量 res Vector2<Scalar> const theta_sigma = rxso2().log(); // 计算 rxso2() 的对数，并将结果存储在 theta_sigma 中 Scalar const theta = theta_sigma[0]; // 提取 theta_sigma 的第一个元素作为 theta Scalar const sigma = theta_sigma[1]; // 提取 theta_sigma 的第二个元素作为 sigma Matrix2<Scalar> const Omega = SO2<Scalar>::hat(theta); // 计算 theta 的 SO2 帽算子 Omega Matrix2<Scalar> const W_inv = details::calcWInv<Scalar, 2>(Omega, theta, sigma, scale()); // 计算 W 的逆矩阵 W_inv
res.segment(0, 2) = W_inv * translation(); // 计算 W_inv 乘以 translation() 的结果，并赋值给 res 的前两个元素 res[2] = theta; // 将 theta 赋值给 res 的第三个元素 res[3] = sigma; // 将 sigma 赋值给 res 的第四个元素 return res; // 返回计算结果 res }
/// Returns 3x3 matrix representation of the instance. /// 返回实例的 3x3 矩阵表示形式。 /// /// It has the following form: /// 它具有以下形式： /// /// | s*R t | /// | o 1 | /// /// where ``R`` is a 2x2 rotation matrix, ``s`` a scale factor, ``t`` a /// translation 2-vector and ``o`` a 2-column vector of zeros. /// 其中 ``R`` 是一个 2x2 旋转矩阵，``s`` 是一个缩放因子，``t`` 是一个平移二维向量，``o`` 是一个包含两个零的列向量。 /// SOPHUS_FUNC Transformation matrix() const { Transformation homogeneous_matrix; // 定义 Transformation 类型的变量 homogeneous_matrix homogeneous_matrix.template topLeftCorner<2, 3>() = matrix2x3(); // 将 matrix2x3() 的结果赋值给 homogeneous_matrix 的左上角 2x3 子矩阵 homogeneous_matrix.row(2) = Matrix<Scalar, 3, 1>(Scalar(0), Scalar(0), Scalar(1)); // 将矩阵 homogeneous_matrix 的第三行设置为 (0, 0, 1) return homogeneous_matrix; // 返回计算结果 homogeneous_matrix }
/// Returns the significant first two rows of the matrix above. /// 返回上述矩阵的前两行。 /// SOPHUS_FUNC Matrix<Scalar, 2, 3> matrix2x3() const { Matrix<Scalar, 2, 3> matrix; // 定义 Matrix<Scalar, 2, 3> 类型的变量 matrix matrix.template topLeftCorner<2, 2>() = rxso2().matrix(); // 将 rxso2().matrix() 的结果赋值给 matrix 的左上角 2x2 子矩阵 matrix.col(2) = translation(); // 将 translation() 的结果赋值给 matrix 的第三列 return matrix; // 返回计算结果 matrix }
/// Assignment-like operator from OtherDerived. /// 从 OtherDerived 类型的赋值运算符。 /// template <class OtherDerived> SOPHUS_FUNC Sim2Base<Derived>& operator=( Sim2Base<OtherDerived> const& other) { rxso2() = other.rxso2(); // 将 other.rxso2() 的结果赋值给当前对象的 rxso2() translation() = other.translation(); // 将 other.translation() 的结果赋值给当前对象的 translation() return *this; // 返回当前对象 }
/// Group multiplication, which is rotation plus scaling concatenation. /// 群乘法，即旋转加缩放连接。 /// /// Note: That scaling is calculated with saturation. See RxSO2 for /// details. /// 注意：缩放是饱和计算的。详见 RxSO2。 /// template <typename OtherDerived> SOPHUS_FUNC Sim2Product<OtherDerived> operator*( Sim2Base<OtherDerived> const& other) const { return Sim2Product<OtherDerived>( rxso2() * other.rxso2(), translation() + rxso2() * other.translation()); // 返回一个新的 Sim2Product<OtherDerived> 实例，其中 rxso2 为当前对象的 rxso2() 乘以 other.rxso2()，translation 为当前对象的 translation() 加上 rxso2() 乘以 other.translation() }
/// Group action on 2-points. /// 对二维点的群操作。 /// /// This function rotates, scales and translates a two dimensional point /// ``p`` by the Sim(2) element ``(bar_sR_foo, t_bar)`` (= similarity /// transformation): /// 该函数通过 Sim(2) 元素 ``(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, 2>::value>> SOPHUS_FUNC PointProduct<PointDerived> operator*( Eigen::MatrixBase<PointDerived> const& p) const { return rxso2() * p + translation(); // 返回当前对象的 rxso2() 乘以 p 的结果加上 translation() }
/// Group action on homogeneous 2-points. See above for more details. /// 对齐次二维点的群操作。详见上文。 /// template <typename HPointDerived, typename = typename std::enable_if_t< IsFixedSizeVector<HPointDerived, 3>::value>> SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*( Eigen::MatrixBase<HPointDerived> const& p) const { const PointProduct<HPointDerived> tp = rxso2() * p.template head<2>() + p(2) * translation(); // 计算 rxso2() 乘以 p 的前两个元素的结果加上 p(2) 乘以 translation()，并存储在 tp 中 return HomogeneousPointProduct<HPointDerived>(tp(0), tp(1), p(2)); // 返回一个新的 HomogeneousPointProduct<HPointDerived> 实例，包含 tp 的第一个元素，tp 的第二个元素和 p(2) }
/// Group action on lines. /// 对直线的群操作。 /// /// This function rotates, scales and translates a parametrized line /// ``l(t) = o + t * d`` by the Sim(2) element: /// 该函数通过 Sim(2) 元素旋转、缩放和平移参数化直线 ``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 = rxso2() * l; // 定义变量 rotatedLine，将 l 通过 rxso2() 旋转缩放变换 return Line(rotatedLine.origin() + translation(), rotatedLine.direction()); // 返回新的 Line 对象，其中原点为 rotatedLine 的原点加上平移，方向为 rotatedLine 的方向 }
/// Group action on hyper-planes. /// 对超平面的群操作。 /// /// This function rotates a hyper-plane ``n.x + d = 0`` by the Sim2 /// element: /// 该函数通过 Sim2 元素旋转超平面 ``n.x + d = 0``。 /// /// Normal vector ``n`` is rotated /// 法向量 ``n`` 被旋转 /// Offset ``d`` is scaled and adjusted for translation /// 偏移 ``d`` 被缩放并调整平移 /// /// Note that in 2d-case hyper-planes are just another parametrization of /// lines /// 注意在二维情况下，超平面只是直线的另一种参数化表示 /// SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const { Hyperplane const rotated = rxso2() * p; // 定义常量变量 rotated，将 p 通过 rxso2() 旋转变换 return Hyperplane(rotated.normal(), rotated.offset() - translation().dot(rotated.normal())); // 返回新的 Hyperplane 对象，其中法向量为 rotated 的法向量，偏移为 rotated 的偏移减去平移向量与 rotated 法向量的点积 }
/// Returns internal parameters of Sim(2). /// 返回 Sim(2) 的内部参数。 /// /// It returns (c[0], c[1], t[0], t[1]), /// 它返回 (c[0], c[1], t[0], t[1])， /// with c being the complex number, t the translation 3-vector. /// 其中 c 是复数，t 是平移向量。 /// SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const { Sophus::Vector<Scalar, num_parameters> p; // 定义 Sophus::Vector 类型的变量 p p << rxso2().params(), translation(); // 将 rxso2() 的参数和平移向量赋值给 p return p; // 返回 p }
/// In-place group multiplication. This method is only valid if the return /// type of the multiplication is compatible with this SO2's Scalar type. /// 就地群乘法。仅当乘法的返回类型与此 SO2 的标量类型兼容时，此方法才有效。 /// template <typename OtherDerived, typename = typename std::enable_if_t< std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>> SOPHUS_FUNC Sim2Base<Derived>& operator*=( Sim2Base<OtherDerived> const& other) { *static_cast<Derived*>(this) = *this * other; // 使用 other 更新当前对象 return *this; // 返回当前对象 }
/// Returns derivative of this * Sim2::exp(x) wrt. x at x=0. /// 返回 this * Sim2::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; // 定义 Matrix 类型的变量 J J.template block<2, 2>(0, 0).setZero(); // 将 J 的 (0,0) 到 (1,1) 块置零 J.template block<2, 2>(0, 2) = rxso2().Dx_this_mul_exp_x_at_0(); // 将 rxso2() 对 this * Sim2::exp(x) 的导数赋值给 J 的 (0,2) 到 (1,3) 块 J.template block<2, 2>(2, 2).setZero(); // 将 J 的 (2,2) 到 (3,3) 块置零 J.template block<2, 2>(2, 0) = rxso2().matrix(); // 将 rxso2() 的矩阵赋值给 J 的 (2,0) 到 (3,1) 块 return J; // 返回 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, num_parameters, DoF> J; // 定义 Matrix 类型的变量 J J.template block<2, 2>(0, 0).setZero(); // 将 J 的 (0,0) 到 (1,1) 块置零 J.template block<2, 2>(0, 2) = rxso2().inverse().matrix(); // 将 rxso2() 的逆矩阵赋值给 J 的 (0,2) 到 (1,3) 块 J.template block<2, 2>(2, 0) = rxso2().Dx_log_this_inv_by_x_at_this(); // 将 rxso2() 对 log(this^{-1} * x) 的导数赋值给 J 的 (2,0) 到 (3,1) 块 J.template block<2, 2>(2, 2).setZero(); // 将 J 的 (2,2) 到 (3,3) 块置零 return J; // 返回 J }
/// Setter of non-zero complex number. /// 非零复数的设置函数。 /// /// Precondition: ``z`` must not be close to zero. /// 前提条件：``z`` 必须不接近零。 /// SOPHUS_FUNC void setComplex(Vector2<Scalar> const& z) { rxso2().setComplex(z); // 设置 rxso2() 的复数为 z }
/// Accessor of complex number. /// 复数的访问器。 /// SOPHUS_FUNC typename Eigen::internal::traits<Derived>::RxSO2Type::ComplexType const& complex() const { return rxso2().complex(); // 返回 rxso2() 的复数 }
/// Returns Rotation matrix /// 返回旋转矩阵 /// SOPHUS_FUNC Matrix2<Scalar> rotationMatrix() const { return rxso2().rotationMatrix(); // 返回 rxso2() 的旋转矩阵 }
/// Mutator of SO2 group. /// SO2 群的修改器。 /// SOPHUS_FUNC RxSO2Type& rxso2() { return static_cast<Derived*>(this)->rxso2(); // 返回当前对象的 rxso2 }
/// Accessor of SO2 group. /// SO2 群的访问器。 /// SOPHUS_FUNC RxSO2Type const& rxso2() const { return static_cast<Derived const*>(this)->rxso2(); // 返回当前对象的 rxso2 }
/// Returns scale. /// 返回缩放比例。 /// SOPHUS_FUNC Scalar scale() const { return rxso2().scale(); } // 返回 rxso2() 的缩放比例
/// Setter of complex number using rotation matrix ``R``, leaves scale as is. /// 使用旋转矩阵 ``R`` 设置复数，保留缩放比例。 /// SOPHUS_FUNC void setRotationMatrix(Matrix2<Scalar> const& R) { rxso2().setRotationMatrix(R); // 使用 R 设置 rxso2() 的旋转矩阵 }
/// 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) { rxso2().setScale(scale); } // 设置缩放比例，并保持旋转不变
/// Setter of complex number using scaled rotation matrix ``sR``. /// 使用缩放旋转矩阵 ``sR`` 设置复数。 /// /// Precondition: The 2x2 matrix must be "scaled orthogonal" /// and have a positive determinant. /// 前提条件：2x2 矩阵必须是“缩放正交”的，并且行列式为正。 /// SOPHUS_FUNC void setScaledRotationMatrix(Matrix2<Scalar> const& sR) { rxso2().setScaledRotationMatrix(sR); // 使用 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(); // 返回当前对象的平移向量 }};
/// Sim2 using default storage; derived from Sim2Base./// 使用默认存储的 Sim2；从 Sim2Base 派生。template <class Scalar_, int Options>class Sim2 : public Sim2Base<Sim2<Scalar_, Options>> { public: using Base = Sim2Base<Sim2<Scalar_, Options>>; // 定义 Base 类型，等于 Sim2Base<Sim2<Scalar_, Options>> using Scalar = Scalar_; // 定义 Scalar 类型，等于模板参数 Scalar_ using Transformation = typename Base::Transformation; // 定义 Transformation 类型，等于 Base::Transformation using Point = typename Base::Point; // 定义 Point 类型，等于 Base::Point using HomogeneousPoint = typename Base::HomogeneousPoint; // 定义 HomogeneousPoint 类型，等于 Base::HomogeneousPoint using Tangent = typename Base::Tangent; // 定义 Tangent 类型，等于 Base::Tangent using Adjoint = typename Base::Adjoint; // 定义 Adjoint 类型，等于 Base::Adjoint using RxSo2Member = RxSO2<Scalar, Options>; // 定义 RxSo2Member 类型，等于 RxSO2<Scalar, Options> using TranslationMember = Vector2<Scalar, Options>; // 定义 TranslationMember 类型，等于 Vector2<Scalar, Options>
using Base::operator=; // 使用 Base 的赋值运算符
/// 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 Sim2& operator=(Sim2 const& other) = default; // 默认复制赋值运算符
static int constexpr DoF = Base::DoF; // 定义 DoF，等于 Base::DoF static int constexpr num_parameters = Base::num_parameters; // 定义 num_parameters，等于 Base::num_parameters
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // 保证内存对齐的宏
/// Default constructor initializes similarity transform to the identity. /// 默认构造函数将相似变换初始化为单位变换。 /// SOPHUS_FUNC Sim2();
/// Copy constructor /// 复制构造函数 /// SOPHUS_FUNC Sim2(Sim2 const& other) = default;
/// Copy-like constructor from OtherDerived. /// 从 OtherDerived 复制构造函数。 /// template <class OtherDerived> SOPHUS_FUNC Sim2(Sim2Base<OtherDerived> const& other) : rxso2_(other.rxso2()), translation_(other.translation()) { static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value, "must be same Scalar type"); // 静态断言，确保 OtherDerived::Scalar 与 Scalar 类型相同 }
/// Constructor from RxSO2 and translation vector /// 从 RxSO2 和平移向量构造函数 /// template <class OtherDerived, class D> SOPHUS_FUNC Sim2(RxSO2Base<OtherDerived> const& rxso2, Eigen::MatrixBase<D> const& translation) : rxso2_(rxso2), translation_(translation) { static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value, "must be same Scalar type"); // 静态断言，确保 OtherDerived::Scalar 与 Scalar 类型相同 static_assert(std::is_same<typename D::Scalar, Scalar>::value, "must be same Scalar type"); // 静态断言，确保 D::Scalar 与 Scalar 类型相同 }
/// Constructor from complex number and translation vector. /// 从复数和平移向量构造函数。 /// /// Precondition: complex number must not be close to zero. /// 前提条件：复数不得接近零。 /// template <class D> SOPHUS_FUNC Sim2(Vector2<Scalar> const& complex_number, Eigen::MatrixBase<D> const& translation) : rxso2_(complex_number), translation_(translation) { static_assert(std::is_same<typename D::Scalar, Scalar>::value, "must be same Scalar type"); // 静态断言，确保 D::Scalar 与 Scalar 类型相同 }
/// Constructor from 3x3 matrix /// 从 3x3 矩阵构造函数 /// /// Precondition: Top-left 2x2 matrix needs to be "scaled-orthogonal" with /// positive determinant. The last row must be ``(0, 0, 1)``. /// 前提条件：左上角 2x2 矩阵必须是“缩放正交”的，且行列式为正。最后一行必须是 ``(0, 0, 1)``。 /// SOPHUS_FUNC explicit Sim2(Matrix<Scalar, 3, 3> const& T) : rxso2_((T.template topLeftCorner<2, 2>()).eval()), translation_(T.template block<2, 1>(0, 2)) {}
/// This provides unsafe read/write access to internal data. Sim(2) is /// represented by a complex number (two parameters) and a 2-vector. When /// using direct write access, the user needs to take care of that the /// complex number is not set close to zero. /// 这提供了对内部数据的不安全读写访问。Sim(2) 由一个复数（两个参数）和一个二维向量表示。 /// 在使用直接写访问时，用户需要确保复数不接近零。 /// SOPHUS_FUNC Scalar* data() { // rxso2_ 和 translation_ 按顺序排列，没有填充 return rxso2_.data(); }
/// Const version of data() above. /// 上面 data() 的常量版本。 /// SOPHUS_FUNC Scalar const* data() const { // rxso2_ 和 translation_ 按顺序排列，没有填充 return rxso2_.data(); // 返回 rxso2_ 的数据 }
/// Accessor of RxSO2 /// RxSO2 的访问器 /// SOPHUS_FUNC RxSo2Member& rxso2() { return rxso2_; } // 返回 rxso2_ 成员
/// Mutator of RxSO2 /// RxSO2 的修改器 /// SOPHUS_FUNC RxSo2Member const& rxso2() const { return rxso2_; } // 返回 rxso2_ 成员（常量版本）
/// Mutator of translation vector /// 平移向量的修改器 /// SOPHUS_FUNC TranslationMember& translation() { return translation_; } // 返回 translation_ 成员
/// Accessor of translation vector /// 平移向量的访问器 /// SOPHUS_FUNC TranslationMember const& translation() const { return translation_; // 返回 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; // 定义 Sophus::Matrix 类型的变量 J J.template block<2, 2>(0, 0).setZero(); // 将 J 的 (0,0) 到 (1,1) 块置零 J.template block<2, 2>(0, 2) = RxSO2<Scalar>::Dx_exp_x_at_0(); // 将 RxSO2 对 exp(x) 的导数赋值给 J 的 (0,2) 到 (1,3) 块 J.template block<2, 2>(2, 0).setIdentity(); // 将 J 的 (2,0) 到 (3,1) 块设为单位矩阵 J.template block<2, 2>(2, 2).setZero(); // 将 J 的 (2,2) 到 (3,3) 块置零 return J; // 返回 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) { static Matrix2<Scalar> const I = Matrix2<Scalar>::Identity(); // 定义静态常量 I，为 2x2 单位矩阵 static Scalar const one(1.0); // 定义静态常量 one，值为 1.0
Scalar const theta = a[2]; // 提取 a 的第三个元素作为 theta Scalar const sigma = a[3]; // 提取 a 的第四个元素作为 sigma
Matrix2<Scalar> const Omega = SO2<Scalar>::hat(theta); // 计算 theta 的 SO2 帽算子 Omega Matrix2<Scalar> const Omega_dtheta = SO2<Scalar>::hat(one); // 计算 theta 的导数 Omega_dtheta Matrix2<Scalar> const Omega2 = Omega * Omega; // 计算 Omega 的平方 Omega2 Matrix2<Scalar> const Omega2_dtheta = Omega_dtheta * Omega + Omega * Omega_dtheta; // 计算 Omega2 的导数 Omega2_dtheta Matrix2<Scalar> const W = details::calcW<Scalar, 2>(Omega, theta, sigma); // 计算 W Vector2<Scalar> const upsilon = a.segment(0, 2); // 提取 a 的前两个元素作为 upsilon
Sophus::Matrix<Scalar, num_parameters, DoF> J; // 定义 Sophus::Matrix 类型的变量 J J.template block<2, 2>(0, 0).setZero(); // 将 J 的 (0,0) 到 (1,1) 块置零 J.template block<2, 2>(0, 2) = RxSO2<Scalar>::Dx_exp_x(a.template tail<2>()); // 将 RxSO2 对 exp(x) 的导数赋值给 J 的 (0,2) 到 (1,3) 块 J.template block<2, 2>(2, 0) = W; // 将 W 赋值给 J 的 (2,0) 到 (3,1) 块
Scalar A, B, C, A_dtheta, B_dtheta, A_dsigma, B_dsigma, C_dsigma; // 定义标量变量 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 的导数
J.template block<2, 1>(2, 2) = (A_dtheta * Omega + A * Omega_dtheta + B_dtheta * Omega2 + B * Omega2_dtheta) * upsilon; // 计算并赋值 J 的 (2,2) 到 (3,2) 块 J.template block<2, 1>(2, 3) = (A_dsigma * Omega + B_dsigma * Omega2 + C_dsigma * I) * upsilon; // 计算并赋值 J 的 (2,3) 到 (3,3) 块
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, 2, DoF> Dx_exp_x_times_point_at_0( Point const& point) { Sophus::Matrix<Scalar, 2, DoF> J; // 定义 Sophus::Matrix 类型的变量 J J << Sophus::Matrix2<Scalar>::Identity(), Sophus::RxSO2<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); // 返回生成器函数的结果 }
/// Derivative of Lie bracket with respect to first element. /// Lie 括号相对于第一个元素的导数。 /// /// This function returns ``D_a [a, b]`` with ``D_a`` being the /// differential operator with respect to ``a``, ``[a, b]`` being the lie /// bracket of the Lie algebra sim(2). /// 这个函数返回 ``D_a [a, b]``，其中 ``D_a`` 是相对于 ``a`` 的微分算子， /// ``[a, b]`` 是 sim(2) 的 Lie 括号。 /// See ``lieBracket()`` below. /// 详见下文的 ``lieBracket()``。
/// Group exponential /// 群指数 /// /// This functions takes in an element of tangent space and returns the /// corresponding element of the group Sim(2). /// 这个函数接受一个切空间的元素，并返回对应的 Sim(2) 群元素。 /// /// The first two components of ``a`` represent the translational part /// ``upsilon`` in the tangent space of Sim(2), the following two components /// of ``a`` represents the rotation ``theta`` and the final component /// represents the logarithm of the scaling factor ``sigma``. /// ``a`` 的前两个分量代表 Sim(2) 切空间的平移部分 ``upsilon``，接下来的两个分量 /// 代表旋转 ``theta``，最后一个分量代表缩放因子的对数 ``sigma``。
/// To be more specific, this function computes ``expmat(hat(a))`` with /// ``expmat(.)`` being the matrix exponential and ``hat(.)`` the hat-operator /// of Sim(2), see below. /// 具体来说，这个函数计算 ``expmat(hat(a))``，其中 ``expmat(.)`` 是矩阵指数，``hat(.)`` 是 Sim(2) 的帽算子，详见下文。 /// SOPHUS_FUNC static Sim2<Scalar> exp(Tangent const& a) { // For the derivation of the exponential map of Sim(N) 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(N) 指数映射的推导，请参见 // H. Strasdat, "Local Accuracy and Global Consistency for Efficient Visual // SLAM", PhD 论文, 2012. // http:///hauke.strasdat.net/files/strasdat_thesis_2012.pdf (A.5, 第 186 页) Vector2<Scalar> const upsilon = a.segment(0, 2); // 提取 a 的前两个元素作为 upsilon Scalar const theta = a[2]; // 提取 a 的第三个元素作为 theta Scalar const sigma = a[3]; // 提取 a 的第四个元素作为 sigma RxSO2<Scalar> rxso2 = RxSO2<Scalar>::exp(a.template tail<2>()); // 计算 a 的后两个元素的指数，并存储在 rxso2 中 Matrix2<Scalar> const Omega = SO2<Scalar>::hat(theta); // 计算 theta 的 SO2 帽算子 Omega Matrix2<Scalar> const W = details::calcW<Scalar, 2>(Omega, theta, sigma); // 计算 W return Sim2<Scalar>(rxso2, W * upsilon); // 返回一个新的 Sim2<Scalar> 实例，其中 rxso2 为 rxso2，translation 为 W 乘以 upsilon }
/// Returns the ith infinitesimal generators of Sim(2). /// 返回 Sim(2) 的第 i 个无穷小生成元。 /// /// The infinitesimal generators of Sim(2) are: /// Sim(2) 的无穷小生成元是： /// /// ``` /// | 0 0 1 | /// G_0 = | 0 0 0 | /// | 0 0 0 | /// /// | 0 0 0 | /// G_1 = | 0 0 1 | /// | 0 0 0 | /// /// | 0 -1 0 | /// G_2 = | 1 0 0 | /// | 0 0 0 | /// /// | 1 0 0 | /// G_3 = | 0 1 0 | /// | 0 0 0 | /// ``` /// /// Precondition: ``i`` must be in [0, 3]. /// 前提条件：``i`` 必须在 [0, 3] 范围内。 /// SOPHUS_FUNC static Transformation generator(int i) { SOPHUS_ENSURE(i >= 0 || i <= 3, "i should be in range [0,3]."); // 确保 i 在 [0, 3] 范围内 Tangent e; // 定义 Tangent 类型的变量 e e.setZero(); // 将 e 置零 e[i] = Scalar(1); // 将 e 的第 i 个分量设为 1 return hat(e); // 返回 hat(e) 的结果 }
/// hat-operator /// hat 算子 /// /// It takes in the 4-vector representation and returns the corresponding /// matrix representation of Lie algebra element. /// 接受 4 维向量表示，并返回相应的李代数元素的矩阵表示。 /// /// Formally, the hat()-operator of Sim(2) is defined as /// 正式地，Sim(2) 的 hat() 算子定义为 /// /// ``hat(.): R^4 -> R^{3x3}, hat(a) = sum_i a_i * G_i`` (for i=0,...,6) /// ``hat(.): R^4 -> R^{3x3}, hat(a) = sum_i a_i * G_i`` (对于 i=0,...,6) /// /// with ``G_i`` being the ith infinitesimal generator of Sim(2). /// 其中 ``G_i`` 是 Sim(2) 的第 i 个无穷小生成元。 /// /// The corresponding inverse is the vee()-operator, see below. /// 相应的逆算子是 vee()，见下文。 /// SOPHUS_FUNC static Transformation hat(Tangent const& a) { Transformation Omega; // 定义 Transformation 类型的变量 Omega Omega.template topLeftCorner<2, 2>() = RxSO2<Scalar>::hat(a.template tail<2>()); // 将 a 的后两个元素的 hat 结果赋值给 Omega 的左上角 2x2 子矩阵 Omega.col(2).template head<2>() = a.template head<2>(); // 将 a 的前两个元素赋值给 Omega 的第三列的前两个元素 Omega.row(2).setZero(); // 将 Omega 的第三行置零 return Omega; // 返回 Omega }
/// Lie bracket /// 李括号 /// /// It computes the Lie bracket of Sim(2). To be more specific, it computes /// 计算 Sim(2) 的李括号。具体来说，它计算 /// /// ``[omega_1, omega_2]_sim2 := vee([hat(omega_1), hat(omega_2)])`` /// ``[omega_1, omega_2]_sim2 := 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(2). /// 其中 ``[A,B] := AB-BA`` 是矩阵交换子，``hat(.)`` 是 hat 算子，``vee(.)`` 是 Sim(2) 的 vee 算子。 /// SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) { Vector2<Scalar> const upsilon1 = a.template head<2>(); // 提取 a 的前两个元素作为 upsilon1 Vector2<Scalar> const upsilon2 = b.template head<2>(); // 提取 b 的前两个元素作为 upsilon2 Scalar const theta1 = a[2]; // 提取 a 的第三个元素作为 theta1 Scalar const theta2 = b[2]; // 提取 b 的第三个元素作为 theta2 Scalar const sigma1 = a[3]; // 提取 a 的第四个元素作为 sigma1 Scalar const sigma2 = b[3]; // 提取 b 的第四个元素作为 sigma2
Tangent res; // 定义 Tangent 类型的变量 res res[0] = -theta1 * upsilon2[1] + theta2 * upsilon1[1] + sigma1 * upsilon2[0] - sigma2 * upsilon1[0]; // 计算 res 的第一个分量 res[1] = theta1 * upsilon2[0] - theta2 * upsilon1[0] + sigma1 * upsilon2[1] - sigma2 * upsilon1[1]; // 计算 res 的第二个分量 res[2] = Scalar(0); // 设置 res 的第三个分量为 0 res[3] = Scalar(0); // 设置 res 的第四个分量为 0
return res; // 返回 res }
/// Draw uniform sample from Sim(2) manifold. /// 从 Sim(2) 流形中抽取均匀样本。 /// /// 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 Sim2 sampleUniform(UniformRandomBitGenerator& generator) { std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1)); // 定义均匀分布，范围为 [-1, 1] return Sim2(RxSO2<Scalar>::sampleUniform(generator), Vector2<Scalar>(uniform(generator), uniform(generator))); // 返回一个新的 Sim2 实例，其中 RxSO2 由 generator 均匀抽取，平移向量由均匀分布生成 }
/// vee-operator /// vee 算子 /// /// It takes the 3x3-matrix representation ``Omega`` and maps it to the /// corresponding 4-vector representation of Lie algebra. /// 它接受 3x3 矩阵表示 ``Omega`` 并将其映射到相应的 4 维向量表示的李代数。 /// /// This is the inverse of the hat()-operator, see above. /// 这是 hat 算子的逆操作，详见上文。 /// /// Precondition: ``Omega`` must have the following structure: /// 前提条件：``Omega`` 必须具有以下结构： /// /// | d -c a | /// | c d b | /// | 0 0 0 | /// SOPHUS_FUNC static Tangent vee(Transformation const& Omega) { Tangent upsilon_omega_sigma; // 定义 Tangent 类型的变量 upsilon_omega_sigma upsilon_omega_sigma.template head<2>() = Omega.col(2).template head<2>(); // 将 Omega 的第三列的前两个元素赋值给 upsilon_omega_sigma 的前两个元素 upsilon_omega_sigma.template tail<2>() = RxSO2<Scalar>::vee(Omega.template topLeftCorner<2, 2>()); // 将 Omega 的左上角 2x2 子矩阵的 vee 结果赋值给 upsilon_omega_sigma 的后两个元素 return upsilon_omega_sigma; // 返回 upsilon_omega_sigma }
protected: RxSo2Member rxso2_; // 定义保护成员变量 rxso2_ TranslationMember translation_; // 定义保护成员变量 translation_};
template <class Scalar, int Options>SOPHUS_FUNC Sim2<Scalar, Options>::Sim2() : translation_(TranslationMember::Zero()) { // 构造函数初始化 translation_ 为零向量 static_assert(std::is_standard_layout<Sim2>::value, "Assume standard layout for the use of offset of check below."); // 静态断言，确保 Sim2 是标准布局类型 static_assert( offsetof(Sim2, rxso2_) + sizeof(Scalar) * RxSO2<Scalar>::num_parameters == offsetof(Sim2, 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."); // 静态断言，确保 rxso2_ 和 translation_ 的内存布局是紧密排列的}
} // namespace Sophus// 结束命名空间 Sophus
namespace Eigen {// 定义命名空间 Eigen
/// Specialization of Eigen::Map for ``Sim2``; derived from Sim2Base./// ``Sim2`` 的 Eigen::Map 特化；从 Sim2Base 派生。
////// Allows us to wrap Sim2 objects around POD array./// 允许我们将 Sim2 对象包装在 POD 数组中。
template <class Scalar_, int Options>class Map<Sophus::Sim2<Scalar_>, Options> : public Sophus::Sim2Base<Map<Sophus::Sim2<Scalar_>, Options>> {// 声明模板类 Map，继承自 Sophus::Sim2Base public: using Base = Sophus::Sim2Base<Map<Sophus::Sim2<Scalar_>, Options>>;// 定义类型别名 Base，等于 Sophus::Sim2Base<Map<Sophus::Sim2<Scalar_>, Options>> using Scalar = Scalar_;// 定义类型别名 Scalar，等于模板参数 Scalar_ using Transformation = typename Base::Transformation;// 定义类型别名 Transformation，等于 Base::Transformation using Point = typename Base::Point;// 定义类型别名 Point，等于 Base::Point using HomogeneousPoint = typename Base::HomogeneousPoint;// 定义类型别名 HomogeneousPoint，等于 Base::HomogeneousPoint using Tangent = typename Base::Tangent;// 定义类型别名 Tangent，等于 Base::Tangent using Adjoint = typename Base::Adjoint;// 定义类型别名 Adjoint，等于 Base::Adjoint
using Base::operator=;// 使用 Base 的赋值运算符 using Base::operator*=;// 使用 Base 的乘法赋值运算符 using Base::operator*;// 使用 Base 的乘法运算符
SOPHUS_FUNC explicit Map(Scalar* coeffs) : rxso2_(coeffs), translation_(coeffs + Sophus::RxSO2<Scalar>::num_parameters) {}// 显式定义构造函数，初始化 rxso2_ 和 translation_
/// Mutator of RxSO2 /// RxSO2 的修改器 /// SOPHUS_FUNC Map<Sophus::RxSO2<Scalar>, Options>& rxso2() { return rxso2_; }// 返回 rxso2_ 的引用
/// Accessor of RxSO2 /// RxSO2 的访问器 /// SOPHUS_FUNC Map<Sophus::RxSO2<Scalar>, Options> const& rxso2() const { return rxso2_; }// 返回 rxso2_ 的常量引用
/// Mutator of translation vector /// 平移向量的修改器 /// SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options>& translation() { return translation_; }// 返回 translation_ 的引用
/// Accessor of translation vector /// 平移向量的访问器 SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options> const& translation() const { return translation_; }// 返回 translation_ 的常量引用
protected: Map<Sophus::RxSO2<Scalar>, Options> rxso2_;// 定义保护成员变量 rxso2_ Map<Sophus::Vector2<Scalar>, Options> translation_;// 定义保护成员变量 translation_};
/// Specialization of Eigen::Map for ``Sim2 const``; derived from Sim2Base./// ``Sim2 const`` 的 Eigen::Map 特化；从 Sim2Base 派生。
////// Allows us to wrap RxSO2 objects around POD array./// 允许我们将 RxSO2 对象包装在 POD 数组中。
template <class Scalar_, int Options>class Map<Sophus::Sim2<Scalar_> const, Options> : public Sophus::Sim2Base<Map<Sophus::Sim2<Scalar_> const, Options>> {// 声明模板类 Map，继承自 Sophus::Sim2Base public: using Base = Sophus::Sim2Base<Map<Sophus::Sim2<Scalar_> const, Options>>;// 定义类型别名 Base，等于 Sophus::Sim2Base<Map<Sophus::Sim2<Scalar_> const, Options>> using Scalar = Scalar_;// 定义类型别名 Scalar，等于模板参数 Scalar_ using Transformation = typename Base::Transformation;// 定义类型别名 Transformation，等于 Base::Transformation using Point = typename Base::Point;// 定义类型别名 Point，等于 Base::Point using HomogeneousPoint = typename Base::HomogeneousPoint;// 定义类型别名 HomogeneousPoint，等于 Base::HomogeneousPoint using Tangent = typename Base::Tangent;// 定义类型别名 Tangent，等于 Base::Tangent using Adjoint = typename Base::Adjoint;// 定义类型别名 Adjoint，等于 Base::Adjoint
using Base::operator*=;// 使用 Base 的乘法赋值运算符 using Base::operator*;// 使用 Base 的乘法运算符
SOPHUS_FUNC explicit Map(Scalar const* coeffs) : rxso2_(coeffs), translation_(coeffs + Sophus::RxSO2<Scalar>::num_parameters) {}// 显式定义构造函数，初始化 rxso2_ 和 translation_
/// Accessor of RxSO2 /// RxSO2 的访问器 /// SOPHUS_FUNC Map<Sophus::RxSO2<Scalar> const, Options> const& rxso2() const { return rxso2_; }// 返回 rxso2_ 的常量引用
/// Accessor of translation vector /// 平移向量的访问器 /// SOPHUS_FUNC Map<Sophus::Vector2<Scalar> const, Options> const& translation() const { return translation_; }// 返回 translation_ 的常量引用
protected: Map<Sophus::RxSO2<Scalar> const, Options> const rxso2_;// 定义保护常量成员变量 rxso2_ Map<Sophus::Vector2<Scalar> const, Options> const translation_;// 定义保护常量成员变量 translation_};} // namespace Eigen// 结束命名空间 Eigen该代码文件定义了 Sim(2) 类，用于表示二维平面的相似变换。相似变换包括缩放、旋转和平移。
主要内容：
- Sim2Base 类
：这是 Sim(2) 的基类，定义了相似变换的一些基本属性和操作，例如自由度、参数个数、矩阵表示、对点和线的变换等等。
- Sim2 类
：这是 Sim(2) 的具体实现类，继承自 Sim2Base 类。它提供了构造函数、赋值运算符、各种存取数据的成员函数以及一些特定的操作，例如求对数、指数映射、雅可比矩阵等。
代码细节：
Sophus::Vector2<Scalar>： 表示二维向量，其中
Scalar为数值类型（例如 double 或 float）。RxSO2： 表示旋转和平缩放的组合，内部使用复数表示。
Transformation： 表示 3x3 的变换矩阵。
Point： 表示二维点。
HomogeneousPoint： 表示齐次坐标系下的三维点。
Tangent： 表示切空间的向量，用于表示微小的变换。
Adjoint： 表示伴随矩阵。
常用函数：
matrix(): 返回相似变换的 3x3 矩阵表示。
log(): 计算相似变换的对数映射，即从群元素 (变换矩阵) 得到切空间中的向量 (微小变换)。
inverse(): 求解相似变换的逆变换。
operator*： 定义相似变换的组合运算 (例如，
A * B表示先应用变换 B 再应用变换 A)。rxso2(): 获取内部的 RxSO2 对象，用于访问旋转和平缩放信息。
translation(): 获取内部的平移向量。
总而言之，这段代码实现了二维相似变换的表示和各种操作，可用于机器人定位、图像处理等领域。
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
