rxso2.hpp 文件主要实现了一个描述二维空间中旋转和缩放组合的数学对象RxSO2。这个对象结合了旋转群 SO(2) 和正实数缩放群 R+，可以用于表示和操作二维变换。以下是该文件的主要内容总结：
类模板定义：
template <class Scalar_, int Options = 0> class RxSO2：定义了RxSO2模板类，用于描述带缩放的旋转矩阵。using RxSO2d = RxSO2<double>和using RxSO2f = RxSO2<float>：分别定义了double和float类型的RxSO2。
类和方法实现：
RxSO2Base：一个模板基类，实现了大部分RxSO2的功能，包括旋转矩阵和缩放因子的计算、读取和设置。RxSO2：派生类，继承自RxSO2Base，提供了具体的数据存储和操作。
辅助函数和操作：
复数访问器：提供了对内部复数表示的读写访问，如
complex()、data()。矩阵变换和旋转：实现了旋转矩阵和缩放因子的计算，如
rotationMatrix()、scale()。指数和对数映射：实现了李群和李代数之间的转换，如
exp()和log()。群操作：实现了对点、直线和超平面的群操作，如
operator*()。
特化和映射：
Eigen::Map特化：实现了对RxSO2和RxSO2 const的特化，使其可以与Eigen库的其他部分配合使用。
保护机制：
确保缩放因子在合理范围内，以避免数值不稳定。
总体来说，rxso2.hpp 文件实现了一个用于描述二维旋转和缩放的数学工具类，可以广泛用于计算机图形学、机器人学等领域。文件通过模板和特化的方式实现了灵活且高效的数值计算功能。
/// R X SO(2) 直积 - 在二维空间中的旋转和缩放。
#pragma once
#include "so2.hpp" // 包含定义 SO2 类的头文件
namespace Sophus { template <class Scalar_, int Options = 0> class RxSO2; // 定义模板类 RxSO2 using RxSO2d = RxSO2<double>; // RxSO2 类的 double 类型特化 using RxSO2f = RxSO2<float>; // RxSO2 类的 float 类型特化} // namespace Sophus
namespace Eigen { namespace internal { template <class Scalar_, int Options_> struct traits<Sophus::RxSO2<Scalar_, Options_>> { static constexpr int Options = Options_; // 定义 Options 常量 using Scalar = Scalar_; // 定义标量类型 using ComplexType = Sophus::Vector2<Scalar, Options>; // 定义复数类型 };
template <class Scalar_, int Options_> struct traits<Map<Sophus::RxSO2<Scalar_>, Options_>> : traits<Sophus::RxSO2<Scalar_, Options_>> { static constexpr int Options = Options_; // 定义 Options 常量 using Scalar = Scalar_; // 定义标量类型 using ComplexType = Map<Sophus::Vector2<Scalar>, Options>; // 定义复数类型 };
template <class Scalar_, int Options_> struct traits<Map<Sophus::RxSO2<Scalar_> const, Options_>> : traits<Sophus::RxSO2<Scalar_, Options_> const> { static constexpr int Options = Options_; // 定义 Options 常量 using Scalar = Scalar_; // 定义标量类型 using ComplexType = Map<Sophus::Vector2<Scalar> const, Options>; // 定义复数类型 }; } // namespace internal} // namespace Eigen
namespace Sophus { /// RxSO2 基类型 - 实现了 RxSO2 类但不包含存储特定信息 /// /// 这个类实现了群 ``R+ x SO(2)``, 即正实数群与二维特殊正交群 SO(2) 的直积。 /// 从几何上看，它是二维空间中的旋转和缩放的群。 /// 作为矩阵群，R+ x SO(2) 由形如 ``s * R`` 的矩阵组成，其中 ``R`` 是行列式为 1 的正交矩阵，``s > 0`` 是正实数。 /// 特别是，它具有如下形式： /// /// | s * cos(theta) s * -sin(theta) | /// | s * sin(theta) s * cos(theta) | /// /// 其中 ``theta`` 是旋转角度。在内部，它由旋转矩阵的第一列表示，或者说由一个非零的复数表示。 /// /// R+ x SO(2) 不是紧群，但它是交换群。首先，它不是紧群，因为缩放因子不受限。其次，它是交换群，因为 /// ``sR(alpha, s1) * sR(beta, s2) = sR(beta, s2) * sR(alpha, s1)``, 只是因为 ``alpha + beta = beta + alpha`` 和 ``s1 * s2 = s2 * s1`` /// 其中 ``alpha`` 和 ``beta`` 是旋转角度，``s1`` 和 ``s2`` 是缩放因子。 /// /// 这个类有一个显式的类不变性，即缩放 ``s`` 不会过于接近零或无穷大。严格来说，必须满足以下条件： /// /// ``complex().norm() >= Constants::epsilon()`` 并且 /// ``1. / complex().norm() >= Constants::epsilon()``。 /// /// 为了遵守这个条件，群乘法通过饱和实现，使得乘积始终具有不小于这个阈值的缩放比例。
template <class Derived> class RxSO2Base { public: static constexpr int Options = Eigen::internal::traits<Derived>::Options; // 定义 Options 常量 using Scalar = typename Eigen::internal::traits<Derived>::Scalar; // 定义标量类型 using ComplexType = typename Eigen::internal::traits<Derived>::ComplexType; // 定义复数类型 using ComplexTemporaryType = Sophus::Vector2<Scalar, Options>; // 定义临时复数类型
/// 流形的自由度，即切空间的维数 (一个用于旋转，一个用于缩放)。 static int constexpr DoF = 2;
/// 使用的内部参数数量 (复数是一个元组)。 static int constexpr num_parameters = 2;
/// 群变换是 2x2 矩阵。 static int constexpr N = 2;
/// 点是二维的。 static int constexpr Dim = 2;
using Transformation = Matrix<Scalar, N, N>; // 定义变换矩阵类型 using Point = Vector2<Scalar>; // 定义点的类型 using HomogeneousPoint = Vector3<Scalar>; // 定义齐次点的类型 using Line = ParametrizedLine2<Scalar>; // 定义参数化线的类型 using Hyperplane = Hyperplane2<Scalar>; // 定义超平面的类型 using Tangent = Vector<Scalar, DoF>; // 定义切向量的类型 using Adjoint = Matrix<Scalar, DoF, DoF>; // 定义伴随矩阵的类型
/// 对于二元运算，返回类型由 Eigen 的 ScalarBinaryOpTraits 特性决定。 /// 这允许混合具体类型和 Map 类型，以及其他兼容的标量类型，例如 Ceres::Jet 和 double 标量与 RxSO2 运算。 template <typename OtherDerived> using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<Scalar, typename OtherDerived::Scalar>::ReturnType;
template <typename OtherDerived> using RxSO2Product = RxSO2<ReturnScalar<OtherDerived>>; // 定义 RxSO2 乘积的类型
template <typename PointDerived> using PointProduct = Vector2<ReturnScalar<PointDerived>>; // 定义点乘积的类型
template <typename HPointDerived> using HomogeneousPointProduct = Vector3<ReturnScalar<HPointDerived>>; // 定义齐次点乘积的类型
/// 伴随变换////// 这个函数返回群元素 ``A`` 的伴随变换 ``Ad``，使得对于所有 ``x`` 均满足/// ``hat(Ad_A * x) = A * hat(x) A^{-1}``。参见下面的帽操作符。////// 对于 RxSO(2)，它仅返回单位矩阵。///SOPHUS_FUNC Adjoint Adj() const { return Adjoint::Identity(); }
/// 返回旋转角度。///SOPHUS_FUNC Scalar angle() const { return SO2<Scalar>(complex()).log(); }
/// 返回转换为 NewScalarType 类型的实例的副本。///template <class NewScalarType>SOPHUS_FUNC RxSO2<NewScalarType> cast() const { typename RxSO2<NewScalarType>::ComplexType c = complex().template cast<NewScalarType>(); return RxSO2<NewScalarType>(c);}
/// 提供对内部数据的不安全读写访问。RxSO(2) 由一个复数（两个参数）表示。/// 使用直接写访问时，用户需要注意复数不能设置得太接近零。////// 注意：第一个参数表示实部，而第二个参数表示虚部。///SOPHUS_FUNC Scalar* data() { return complex_nonconst().data(); }
/// 上面 data() 的常量版本。///SOPHUS_FUNC Scalar const* data() const { return complex().data(); }
/// 返回群逆。///SOPHUS_FUNC RxSO2<Scalar> inverse() const { Scalar squared_scale = complex().squaredNorm(); Vector2<Scalar> xy = complex() / squared_scale; return RxSO2<Scalar>(xy.x(), -xy.y());}
/// 对数映射////// 计算对数，即群指数的逆运算，将群元素（缩放的旋转矩阵）映射到切空间的元素（旋转向量加上缩放因子的对数）。////// 具体来说，这个函数计算 ``vee(logmat(.))``，其中 ``logmat(.)`` 是矩阵对数，``vee(.)`` 是 RxSO2 的对偶操作符。///SOPHUS_FUNC Tangent log() const { using std::log; Tangent theta_sigma; theta_sigma[1] = log(scale()); theta_sigma[0] = SO2<Scalar>(complex()).log(); return theta_sigma;}
/// 返回实例的 2x2 矩阵表示。////// 对于 RxSO2，矩阵表示是一个缩放的正交矩阵 ``sR``，其中 ``det(R)=s^2``，因此是一个具有缩放 ``s`` 的旋转矩阵 ``R``。///SOPHUS_FUNC Transformation matrix() const { Transformation sR; // clang-format off sR << complex()[0], -complex()[1], complex()[1], complex()[0]; // clang-format on return sR;}
/// 赋值操作符。///template <class OtherDerived>SOPHUS_FUNC RxSO2Base<Derived>& operator=( RxSO2Base<OtherDerived> const& other) { complex_nonconst() = other.complex(); return *this;}
/// 群乘法，即旋转合并和缩放相乘。////// 注意：此函数对接近零的乘积进行饱和处理，以确保类不变性。///template <typename OtherDerived>SOPHUS_FUNC RxSO2Product<OtherDerived> operator*( RxSO2Base<OtherDerived> const& other) const { using ResultT = ReturnScalar<OtherDerived>;
Scalar lhs_real = complex().x(); Scalar lhs_imag = complex().y(); typename OtherDerived::Scalar const& rhs_real = other.complex().x(); typename OtherDerived::Scalar const& rhs_imag = other.complex().y(); /// 复数乘法 typename RxSO2Product<OtherDerived>::ComplexType result_complex( lhs_real * rhs_real - lhs_imag * rhs_imag, lhs_real * rhs_imag + lhs_imag * rhs_real);
const ResultT squared_scale = result_complex.squaredNorm();
if (squared_scale < Constants<ResultT>::epsilon() * Constants<ResultT>::epsilon()) { /// 饱和处理以确保类不变性。 result_complex.normalize(); result_complex *= Constants<ResultT>::epsilonPlus(); } if (squared_scale > Scalar(1.) / (Constants<ResultT>::epsilon() * Constants<ResultT>::epsilon())) { /// 饱和处理以确保类不变性。 result_complex.normalize(); result_complex /= Constants<ResultT>::epsilonPlus(); } return RxSO2Product<OtherDerived>(result_complex);}
/// 对二维点进行群操作。////// 这个函数通过 SO2 元素 ``bar_R_foo``（即旋转矩阵）旋转一个二维点 ``p``，并按缩放因子 ``s`` 缩放：////// ``p_bar = s * (bar_R_foo * p_foo)``。///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 matrix() * p;}
/// 对齐次二维点进行群操作。详情请参见上文。///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 auto rsp = *this * p.template head<2>(); return HomogeneousPointProduct<HPointDerived>(rsp(0), rsp(1), p(2));}
/// 对直线进行群操作。////// 这个函数通过 SO2 元素旋转参数化直线 ``l(t) = o + t * d``，并按缩放因子缩放。////// 原点 ``o`` 被旋转和缩放/// 方向 ``d`` 被旋转（保持其范数）SOPHUS_FUNC Line operator*(Line const& l) const { return Line((*this) * l.origin(), (*this) * l.direction() / scale());}
/// 对超平面进行群操作。////// 这个函数通过 SO2 元素旋转超平面 ``n.x + d = 0``，并按缩放因子缩放偏移量。////// 法向量 ``n`` 被旋转/// 偏移量 ``d`` 被缩放////// 注意，在二维情况下，超平面只是直线的另一种参数化表示///SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const { const auto this_scale = scale(); return Hyperplane((*this) * p.normal() / this_scale, this_scale * p.offset());}
/// 就地进行群乘法。如果乘法的返回类型与此 SO2 的标量类型兼容，则此方法有效。////// 注意：这个函数对接近零的乘积进行饱和处理，以确保类不变性。///template <typename OtherDerived, typename = typename std::enable_if_t< std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>>SOPHUS_FUNC RxSO2Base<Derived>& operator*=( RxSO2Base<OtherDerived> const& other) { *static_cast<Derived*>(this) = *this * other; return *this;}
/// 返回 this * RxSO2::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 << -complex().y(), complex().x(), complex().x(), complex().y(); return J;}
/// 返回 log(this^{-1} * x) 对 x 在 x=this 处的导数。///SOPHUS_FUNC Matrix<Scalar, DoF, num_parameters> Dx_log_this_inv_by_x_at_this() const { Matrix<Scalar, DoF, num_parameters> J; const Scalar norm_sq_inv = Scalar(1.) / complex().squaredNorm(); J << -complex().y(), complex().x(), complex().x(), complex().y(); return J * norm_sq_inv;}
/// 返回 RxSO(2) 的内部参数。////// 它返回 (c[0], c[1])，其中 c 是复数。///SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const { return complex();}
/// 设置非零复数。////// 前提条件：``z`` 不能太接近零或无穷大。SOPHUS_FUNC void setComplex(Vector2<Scalar> const& z) { SOPHUS_ENSURE(z.squaredNorm() > Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon(), "Scale factor must be greater-equal epsilon."); SOPHUS_ENSURE(z.squaredNorm() < Scalar(1.) / (Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon()), "Inverse scale factor must be greater-equal epsilon."); static_cast<Derived*>(this)->complex_nonconst() = z;}
/// 访问复数。///SOPHUS_FUNC ComplexType const& complex() const { return static_cast<Derived const*>(this)->complex();}
/// 返回旋转矩阵。///SOPHUS_FUNC Transformation rotationMatrix() const { ComplexTemporaryType norm_quad = complex(); norm_quad.normalize(); return SO2<Scalar>(norm_quad).matrix();}
/// 返回缩放因子。///SOPHUS_FUNC Scalar scale() const { using std::hypot; return hypot(complex().x(), complex().y());}
/// 设置旋转角度，保持缩放不变。///SOPHUS_FUNC void setAngle(Scalar const& theta) { setSO2(SO2<Scalar>(theta));}
/// 使用旋转矩阵 ``R`` 设置复数，保持缩放不变。////// 前提条件：``R`` 必须是行列式为一的正交矩阵。///SOPHUS_FUNC void setRotationMatrix(Transformation const& R) { setSO2(SO2<Scalar>(R));}
/// 设置缩放因子，保持旋转不变。///SOPHUS_FUNC void setScale(Scalar const& scale) { using std::sqrt; complex_nonconst().normalize(); complex_nonconst() *= scale;}
/// 使用缩放的旋转矩阵 ``sR`` 设置复数。////// 前提条件：这个 2x2 矩阵必须是“缩放正交”的并且具有正行列式。///SOPHUS_FUNC void setScaledRotationMatrix(Transformation const& sR) { SOPHUS_ENSURE(isScaledOrthogonalAndPositive(sR), "sR must be scaled orthogonal:\n {}", (sR)); complex_nonconst() = sR.col(0);}
/// 设置 SO(2) 旋转，保持缩放不变。///SOPHUS_FUNC void setSO2(SO2<Scalar> const& so2) { using std::sqrt; Scalar saved_scale = scale(); complex_nonconst() = so2.unit_complex(); complex_nonconst() *= saved_scale;}
SOPHUS_FUNC SO2<Scalar> so2() const { return SO2<Scalar>(complex());}
private:/// 修改复数（私有）以确保类不变性。///SOPHUS_FUNC ComplexType& complex_nonconst() { return static_cast<Derived*>(this)->complex_nonconst();}};
/// 使用存储的 RxSO2；继承自 RxSO2Base。template <class Scalar_, int Options>class RxSO2 : public RxSO2Base<RxSO2<Scalar_, Options>> {public: using Base = RxSO2Base<RxSO2<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 ComplexMember = Eigen::Matrix<Scalar, 2, 1, Options>;
/// ``Base`` 是友元类，因此可以从 ``Base`` 访问 complex_nonconst。 friend class RxSO2Base<RxSO2<Scalar_, Options>>;
using Base::operator=;
/// 明确定义拷贝赋值操作符。如果存在用户声明的拷贝构造函数，则隐式拷贝赋值操作符的定义已被弃用（clang >= 13 中的 -Wdeprecated-copy）。 SOPHUS_FUNC RxSO2& operator=(RxSO2 const& other) = default;
static int constexpr DoF = Base::DoF; static int constexpr num_parameters = Base::num_parameters;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// 默认构造函数将复数初始化为单位旋转，缩放为 1。 /// SOPHUS_FUNC RxSO2() : complex_(Scalar(1), Scalar(0)) {}
/// 拷贝构造函数 /// SOPHUS_FUNC RxSO2(RxSO2 const& other) = default;
/// 从 OtherDerived 复制构造。///template <class OtherDerived>SOPHUS_FUNC RxSO2(RxSO2Base<OtherDerived> const& other) : complex_(other.complex()) {}
/// 使用缩放旋转矩阵的构造函数////// 前提条件：旋转矩阵需要是缩放正交矩阵，行列式为 ``s^2``。///SOPHUS_FUNC explicit RxSO2(Transformation const& sR) { this->setScaledRotationMatrix(sR);}
/// 使用缩放因子和旋转矩阵 ``R`` 的构造函数。////// 前提条件：旋转矩阵 ``R`` 必须是行列式为 1 的正交矩阵，/// 并且 ``scale`` 不能太接近零或无穷大。///SOPHUS_FUNC RxSO2(Scalar const& scale, Transformation const& R) : RxSO2((scale * SO2<Scalar>(R).unit_complex()).eval()) {}
/// 使用缩放因子和 SO2 的构造函数////// 前提条件：``scale`` 不能太接近零或无穷大。///SOPHUS_FUNC RxSO2(Scalar const& scale, SO2<Scalar> const& so2) : RxSO2((scale * so2.unit_complex()).eval()) {}
/// 使用复数的构造函数。////// 前提条件：复数不能太接近零或无穷大。///SOPHUS_FUNC explicit RxSO2(Vector2<Scalar> const& z) : complex_(z) { SOPHUS_ENSURE( complex_.squaredNorm() >= Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon(), "Scale factor must be greater-equal epsilon: {} vs {}", (complex_.squaredNorm()), (Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon())); SOPHUS_ENSURE( complex_.squaredNorm() <= Scalar(1.) / (Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon()), "Inverse scale factor must be greater-equal epsilon: {} vs {}", (Scalar(1.) / complex_.squaredNorm()), (Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon()));}
/// 使用复数的构造函数。////// 前提条件：复数不能太接近零或无穷大。///SOPHUS_FUNC explicit RxSO2(Scalar const& real, Scalar const& imag) : RxSO2(Vector2<Scalar>(real, imag)) {}
/// 访问复数。///SOPHUS_FUNC ComplexMember const& complex() const { return complex_; }
/// 返回 exp(x) 对 ``x`` 的导数///SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x( Tangent const& a) { using std::cos; using std::exp; using std::sin; Scalar const theta = a[0]; Scalar const sigma = a[1];
Sophus::Matrix<Scalar, num_parameters, DoF> J; J << -sin(theta), cos(theta), cos(theta), sin(theta); return J * exp(sigma);}
/// 返回 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; static Scalar const i(1.); static Scalar const o(0.); J << o, i, i, o; return J;}
/// 返回 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; j << Sophus::SO2<Scalar>::Dx_exp_x_times_point_at_0(point), point; return j;}
/// 返回 exp(x).matrix() 对 ``x_i 在 x=0`` 的导数。///SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) { return generator(i);}
/// 群指数////// 这个函数接收切空间的一个元素（= 旋转角加上缩放的对数），并返回对应的群 RxSO2 的元素。////// 更具体地说，这个函数计算 ``expmat(hat(theta))``，其中 ``expmat(.)`` 是矩阵指数，``hat(.)`` 是 RSO2 的 hat() 操作符。///SOPHUS_FUNC static RxSO2<Scalar> exp(Tangent const& a) { using std::exp; using std::max; using std::min;
Scalar const theta = a[0]; Scalar const sigma = a[1]; Scalar s = exp(sigma); // 确保缩放因子的正确性 s = max(s, Constants<Scalar>::epsilonPlus()); s = min(s, Scalar(1.) / Constants<Scalar>::epsilonPlus()); Vector2<Scalar> z = SO2<Scalar>::exp(theta).unit_complex(); z *= s; return RxSO2<Scalar>(z);}
/// 返回 ``R+ x SO(2)`` 的第 i 个无穷小生成元。////// RxSO2 的无穷小生成元为：////// ```/// | 0 -1 |/// G_0 = | 1 0 |////// | 1 0 |/// G_1 = | 0 1 |/// ```////// 前提条件：``i`` 必须是 0 或 1。///SOPHUS_FUNC static Transformation generator(int i) { SOPHUS_ENSURE(i >= 0 && i <= 1, "i should be 0 or 1."); Tangent e; e.setZero(); e[i] = Scalar(1); return hat(e);}
/// hat 操作符////// 它接收二维向量表示 ``a``（= 旋转角加上缩放的对数），返回对应的李代数元素的矩阵表示。////// 正式定义，RxSO2 的 hat 操作符为：////// ``hat(.): R^2 -> R^{2x2}, hat(a) = sum_i a_i * G_i`` (对 i=0,1,2)////// 其中 ``G_i`` 是 RxSO2 的第 i 个无穷小生成元。////// 对应的逆操作符是 vee()，参见下面。///SOPHUS_FUNC static Transformation hat(Tangent const& a) { Transformation A; // clang-format off A << a(1), -a(0), a(0), a(1); // clang-format on return A;}
/// 李括号////// 它计算 RxSO(2) 的李括号。更具体地说，它计算////// ``[omega_1, omega_2]_rxso2 := vee([hat(omega_1), hat(omega_2)])``////// 其中 ``[A,B] := AB-BA`` 是矩阵对易子，``hat(.)`` 是 hat 操作符，``vee(.)`` 是 RxSO2 的 vee 操作符。///SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) { Vector2<Scalar> res; res.setZero(); return res;}
/// 从 RxSO(2) 流形中绘制均匀样本。////// 缩放因子在 log2 空间中从 [-1, 1] 均匀绘制，因此缩放在 [0.5, 2]。///template <class UniformRandomBitGenerator>static RxSO2 sampleUniform(UniformRandomBitGenerator& generator) { std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1)); using std::exp2; return RxSO2(exp2(uniform(generator)), SO2<Scalar>::sampleUniform(generator));}
/// vee 操作符////// 它接收 2x2 矩阵表示 ``Omega``，并将其映射到相应的李代数向量表示。////// 这是 hat 操作符的逆操作，参见上文。////// 前提条件：``Omega`` 必须具有以下结构：////// | d -x |/// | x d |///SOPHUS_FUNC static Tangent vee(Transformation const& Omega) { using std::abs; return Tangent(Omega(1, 0), Omega(0, 0));}
protected:SOPHUS_FUNC ComplexMember& complex_nonconst() { return complex_; }
ComplexMember complex_;};
} // namespace Sophus
namespace Eigen {
/// Eigen::Map 针对 ``RxSO2`` 的特化；继承自 RxSO2Base。////// 允许我们将 RxSO2 对象包装在 POD 数组（例如外部 z 样式复数）周围。template <class Scalar_, int Options>class Map<Sophus::RxSO2<Scalar_>, Options> : public Sophus::RxSO2Base<Map<Sophus::RxSO2<Scalar_>, Options>> { using Base = Sophus::RxSO2Base<Map<Sophus::RxSO2<Scalar_>, 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;
/// ``Base`` 是友元类，因此可以从 ``Base`` 访问 complex_nonconst。 friend class Sophus::RxSO2Base<Map<Sophus::RxSO2<Scalar_>, Options>>;
using Base::operator=; using Base::operator*=; using Base::operator*;
SOPHUS_FUNC explicit Map(Scalar* coeffs) : complex_(coeffs) {}
/// 访问复数。 /// SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options> const& complex() const { return complex_; }
protected: SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options>& complex_nonconst() { return complex_; }
Map<Sophus::Vector2<Scalar>, Options> complex_;};
/// Eigen::Map 针对 ``RxSO2 const`` 的特化；继承自 RxSO2Base。////// 允许我们将 RxSO2 对象包装在 POD 数组（例如外部 z 样式复数）周围。template <class Scalar_, int Options>class Map<Sophus::RxSO2<Scalar_> const, Options> : public Sophus::RxSO2Base<Map<Sophus::RxSO2<Scalar_> const, Options>> { public: using Base = Sophus::RxSO2Base<Map<Sophus::RxSO2<Scalar_> const, Options>>; using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using Adjoint = typename Base::Adjoint;
using Base::operator*=; using Base::operator*;
SOPHUS_FUNC explicit Map(Scalar const* coeffs) : complex_(coeffs) {}
/// 访问复数。 /// SOPHUS_FUNC Map<Sophus::Vector2<Scalar> const, Options> const& complex() const { return complex_; }
protected: Map<Sophus::Vector2<Scalar> const, Options> const complex_;};} // namespace Eigen这段代码定义了 Sophus 库中的RxSO2 类，用于表示二维空间中的旋转和缩放变换。RxSO2 是正实数群 R+ 和二维特殊正交群 SO(2) 的直积。
以下是代码的中文总结：
- 功能:
RxSO2类表示二维空间中的旋转和缩放变换。它本质上是一个复数，其中复数的模表示缩放比例，复数的辐角表示旋转角度。
- 数学背景:
RxSO2对应于数学上的 R+ x SO(2) 群，即正实数群和二维特殊正交群的直积。它描述了在二维平面上先进行旋转，再进行缩放的变换。
- 内部表示:
RxSO2内部使用一个复数来表示。这个复数存储在
complex_成员变量中。 - 类结构:
RxSO2Base: 这是一个基类，实现了
RxSO2的基本功能，但不包含存储。它使用模板参数Derived来实现静态多态性（CRTP）。RxSO2: 这是主要的类，继承自
RxSO2Base，并包含一个ComplexMember类型的成员变量complex_用于存储复数。Eigen::Map: 提供了将
RxSO2对象映射到外部内存的功能，允许使用例如 C 风格的数组来表示RxSO2对象。这对于与其他库的接口非常有用。
- 主要方法:
angle(): 返回旋转角度。
inverse(): 返回逆变换。
log(): 计算对数映射，将
RxSO2元素映射到李代数。exp(): 计算指数映射，将李代数元素映射回
RxSO2群。matrix(): 返回 2x2 的变换矩阵。
operator*(): 重载了乘法运算符，用于群乘法（变换的复合）和变换点/向量。
hat(): 帽算子，将李代数向量映射到李代数矩阵。
vee(): 楔算子，帽算子的逆运算，将李代数矩阵映射到李代数向量。
generator(): 返回李代数的生成元。
setAngle(),
setScale(),setRotationMatrix(),setScaledRotationMatrix(),setSO2(): 设置旋转角度、缩放比例、旋转矩阵等。
- 关键特性:
- 饱和:
在群乘法中，为了保证数值稳定性，对缩放比例进行了饱和处理，避免接近零或无穷大。
- 李代数:
提供了李代数相关的操作，如
log()、exp()、hat()、vee()和lieBracket()，这对于优化和插值等应用非常重要。 - Eigen 集成:
使用 Eigen 库进行矩阵和向量运算，并提供了
Eigen::Map的特化，方便与其他使用 Eigen 的代码集成。
- 饱和:
- 模板:
使用模板允许使用不同的标量类型（例如
float或double）。
总而言之，这段代码提供了一个高效且功能完善的RxSO2 类，用于在二维空间中进行旋转和缩放变换，并提供了李代数相关的操作，方便进行更高级的几何计算。
扫一扫
751
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
