这段代码定义了一个名为Cartesian 的类，它将欧几里得向量空间（即笛卡尔坐标系）实现为一个李群。它旨在用于教育目的，展示李群如何推广欧几里得向量空间，并用于针对李群接口实现的模板/通用算法。
以下是代码的详细解释：
1. 命名空间和基本定义：
定义了
Sophus命名空间，并在其中定义了Cartesian类模板，以及Cartesian2、Cartesian3、Cartesian2d和Cartesian3d等常用类型的别名。
2. Eigen traits 特化：
为
Sophus::Cartesian及其Map版本特化了Eigen::traits，以便 Eigen 库能够正确处理Cartesian对象，并将其视为具有特定标量类型和参数类型的对象。这允许Cartesian对象与 Eigen 的其他类型无缝集成。
3.CartesianBase 类模板：
这是Cartesian 类的基类，包含所有核心功能。
- 类型定义：
定义了
Scalar、ParamsType、DoF（自由度）、num_parameters（参数数量）、N（变换矩阵的维度）、Dim（向量维度）、Transformation（变换矩阵类型）、Point（点类型）、HomogeneousPoint（齐次点类型）、Tangent（切向量类型）、Adjoint（伴随矩阵类型）等类型别名。 - 伴随变换 (
Adj)：对于交换群（如笛卡尔群），伴随变换始终是单位矩阵。
- 类型转换 (
cast)：将
Cartesian对象转换为具有不同标量类型的对象。 - 导数计算：
提供了计算
this * exp(x)在 x=0 处的导数 (Dx_this_mul_exp_x_at_0) 和log(this^{-1} * x)在 x=this 处关于 x 的导数 (Dx_log_this_inv_by_x_at_this) 的函数。 - 求逆 (
inverse)：计算群的逆元，对于笛卡尔群，即向量的加法逆元。
- 对数映射 (
log)：对于欧几里得向量空间，对数映射只是恒等映射，或者更准确地说，它只是从 NxN 矩阵中提取重要的 M 维向量。
- 矩阵表示 (
matrix)：返回实例的 (M+1)x(M+1) 矩阵表示，形式为：
| I p |，| o 1 |。 - 群乘法 (
operator*)：对于笛卡尔群，群乘法即向量加法。
- 群作用于点 (
operator*)：定义了群作用于点和齐次点的操作，本质上也是向量加法。
- 群作用于直线和平面：
定义了群作用于直线和超平面的操作。
- 原地群乘法 (
operator*=)：定义了原地群乘法操作。
- 参数访问 (
params)：提供了访问内部参数向量的接口。
4.Cartesian 类模板：
这是Cartesian 类的主要实现，继承自CartesianBase。
- 构造函数：
提供了默认构造函数（初始化为零向量）、拷贝构造函数、从其他
CartesianBase类型的构造函数，以及从 M 维向量或 (M+1)x(M+1) 矩阵构造的构造函数。 - 数据访问 (
data)：提供了对内部数据的直接读写访问。
- 指数映射的导数 (
Dx_exp_x_at_0,Dx_exp_x,Dx_exp_x_times_point_at_0,Dxi_exp_x_matrix_at_0)：提供了计算指数映射及其相关导数的函数。
- 生成器 (
generator)：返回
Cartesian(M)的第 i 个无穷小生成器。 - 指数映射 (
exp)：对于欧几里得向量空间，指数映射只是构造 (M+1)x(M+1) 齐次矩阵表示。
- hat 算子 (
hat)：定义了 hat 算子，将切向量映射到李代数。
- 李括号 (
lieBracket)：对于交换群，李括号始终为 0。
- 均匀采样 (
sampleUniform)：在每个坐标的 [-1, 1] 范围内进行均匀采样。
- vee 算子 (
vee)：定义了 vee 算子，是 hat 算子的逆运算。
5.Eigen::Map 特化：
为Cartesian 和Cartesian const 特化了Eigen::Map，允许将Cartesian 对象包装在原始数据数组周围，而无需进行复制。
总结：
这段代码将欧几里得向量空间实现为一个李群Cartesian。它提供了李群相关的操作，例如指数映射、对数映射、伴随变换、李括号等。通过继承和模板特化，代码实现了代码复用和类型安全。此外，通过与Eigen::Map 的集成，代码可以高效地处理原始数据数组，避免不必要的复制。这个类的主要目的是为了教学和在通用算法中使用，因为它本质上只是对向量加法的一种更复杂的表示。
/// Cartesian - 欧几里得向量空间作为李群
#pragma once // 防止头文件被重复包含#include <sophus/types.hpp> // 包含 Sophus 库中的类型定义
namespace Sophus { // 定义命名空间 Sophustemplate <class Scalar_, int M, int Options = 0>class Cartesian; // 声明 Cartesian 模板类
template <class Scalar_>using Cartesian2 = Cartesian<Scalar_, 2>; // 定义 Cartesian2 类型别名
template <class Scalar_>using Cartesian3 = Cartesian<Scalar_, 3>; // 定义 Cartesian3 类型别名
using Cartesian2d = Cartesian2<double>; // 定义双精度 Cartesian2 类型别名using Cartesian3d = Cartesian3<double>; // 定义双精度 Cartesian3 类型别名
} // namespace Sophus
namespace Eigen { // 定义命名空间 Eigennamespace internal { // 定义内部命名空间 internal
template <class Scalar_, int M, int Options>struct traits<Sophus::Cartesian<Scalar_, M, Options>> { using Scalar = Scalar_; // 定义标量类型 using ParamsType = Sophus::Vector<Scalar, M, Options>; // 定义参数类型};
template <class Scalar_, int M, int Options>struct traits<Map<Sophus::Cartesian<Scalar_, M>, Options>> : traits<Sophus::Cartesian<Scalar_, M, Options>> { using Scalar = Scalar_; // 定义标量类型 using ParamsType = Map<Sophus::Vector<Scalar, M>, Options>; // 定义参数类型};
template <class Scalar_, int M, int Options>struct traits<Map<Sophus::Cartesian<Scalar_, M> const, Options>> : traits<Sophus::Cartesian<Scalar_, M, Options> const> { using Scalar = Scalar_; // 定义标量类型 using ParamsType = Map<Sophus::Vector<Scalar, M> const, Options>; // 定义参数类型};} // namespace internal} // namespace Eigen
namespace Sophus { // 继续定义命名空间 Sophus
/// Cartesian 基类 - 实现 Cartesian 类但与存储无关。////// 欧几里得向量空间作为李群。////// 李群可以看作是欧几里得向量空间 R^M 的广义。这里，一个 N 维向量 ``p`` 表示为/// (M+1) x (M+1) 齐次矩阵:////// | I p |/// | o 1 |////// 另一方面，Cartesian(M) 可以看作是 SE(M) 的特例，具有恒等旋转，因此表示纯平移。////// 此类的目的有两个:/// - 用于教育目的，突出李群如何广义化欧几里得向量空间。/// - 用于模板化/泛型算法（如 Sophus::Spline）/// 这些算法是根据李群接口实现的。////// 显然，Cartesian(M) 也可以表示为一个 M 元组。////// Cartesian 不是紧凑的，但它是一个交换群。对于向量加法，满足 `a+b = b+a`。////// 参见下面的 Cartesian 类了解更多详细信息。///template <class Derived, int M>class CartesianBase { // 定义 CartesianBase 模板类 public: using Scalar = typename Eigen::internal::traits<Derived>::Scalar; // 定义标量类型 using ParamsType = typename Eigen::internal::traits<Derived>::ParamsType; // 定义参数类型 /// 流形的自由度，等于笛卡尔坐标的数量。 static int constexpr DoF = M; /// 使用的内部参数数量，同样是 M。 static int constexpr num_parameters = M; /// 群变换是 (M+1)x(M+1) 矩阵。 static int constexpr N = M + 1; static int constexpr Dim = M;
using Transformation = Sophus::Matrix<Scalar, N, N>; // 定义 Transformation 类型 using Point = Sophus::Vector<Scalar, M>; // 定义 Point 类型 using HomogeneousPoint = Sophus::Vector<Scalar, N>; // 定义 HomogeneousPoint 类型 using Line = ParametrizedLine<Scalar, M>; // 定义 Line 类型 using Hyperplane = Eigen::Hyperplane<Scalar, M>; // 定义 Hyperplane 类型 using Tangent = Sophus::Vector<Scalar, DoF>; // 定义 Tangent 类型 using Adjoint = Matrix<Scalar, DoF, DoF>; // 定义 Adjoint 类型
/// 对于二元运算，返回类型由 Eigen 的 ScalarBinaryOpTraits 特性确定。 /// 这允许混合具体类型和 Map 类型，以及其他兼容的标量类型，如 Ceres::Jet 和 /// 用于笛卡尔运算的双精度标量。 template <typename OtherDerived> using ReturnScalar = typename Eigen::ScalarBinaryOpTraits< Scalar, typename OtherDerived::Scalar>::ReturnType;
template <typename OtherDerived> using CartesianSum = Cartesian<ReturnScalar<OtherDerived>, M>; // 定义 CartesianSum 类型
template <typename PointDerived> using PointProduct = Sophus::Vector<ReturnScalar<PointDerived>, M>; // 定义 PointProduct 类型
template <typename HPointDerived> using HomogeneousPointProduct = Sophus::Vector<ReturnScalar<HPointDerived>, N>; // 定义 HomogeneousPointProduct 类型
/// 伴随变换 /// /// 始终是交换群的单位矩阵。 SOPHUS_FUNC Adjoint Adj() const { return Adjoint::Identity(); }
/// 返回实例的复制并转换为新的标量类型。 /// template <class NewScalarType> SOPHUS_FUNC Cartesian<NewScalarType, M> cast() const { return Cartesian<NewScalarType, M>(params().template cast<NewScalarType>()); }
/// 返回 this * exp(x) 在 x=0 处相对于 x 的导数。 /// SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_this_mul_exp_x_at_0() const { Sophus::Matrix<Scalar, num_parameters, DoF> m; m.setIdentity(); return m; }
/// 返回 log(this^{-1} * x) 在 x=this 处相对于 x 的导数。 /// SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_log_this_inv_by_x_at_this() const { Matrix<Scalar, DoF, num_parameters> m; m.setIdentity(); return m; }
/// 返回群的逆元。 /// /// 加性逆元。 /// SOPHUS_FUNC Cartesian<Scalar, M> inverse() const { return Cartesian<Scalar, M>(-params()); }
/// 对数映射 /// /// 对于欧几里得向量空间，直接为单位映射。更准确地说， /// 它只从 NxN 矩阵中提取出重要的 M 向量。 /// SOPHUS_FUNC Tangent log() const { return params(); }
/// 返回实例的 4x4 矩阵表示。 /// /// 形式如下: /// /// | I p | /// | o 1 | /// SOPHUS_FUNC Transformation matrix() const { Sophus::Matrix<Scalar, N, N> matrix; matrix.setIdentity(); matrix.col(M).template head<M>() = params(); return matrix; }
/// 群的乘法，即向量加法。 /// template <class OtherDerived> SOPHUS_FUNC CartesianBase<Derived, M>& operator=( CartesianBase<OtherDerived, M> const& other) { params() = other.params(); return *this; }
/// 群的乘法，即向量加法。 /// template <typename OtherDerived> SOPHUS_FUNC CartesianSum<OtherDerived> operator*( CartesianBase<OtherDerived, M> const& other) const { return CartesianSum<OtherDerived>(params() + other.params()); }
/// 群对点的作用，仍然是向量加法。 /// template <typename PointDerived, typename = typename std::enable_if_t< IsFixedSizeVector<PointDerived, M>::value>> SOPHUS_FUNC PointProduct<PointDerived> operator*( Eigen::MatrixBase<PointDerived> const& p) const { return PointProduct<PointDerived>(params() + p); }
/// 群对齐次点的作用。见上面的更多详细信息。 /// template <typename HPointDerived, typename = typename std::enable_if_t< IsFixedSizeVector<HPointDerived, N>::value>> SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*( Eigen::MatrixBase<HPointDerived> const& p) const { const auto rp = *this * p.template head<M>(); HomogeneousPointProduct<HPointDerived> r; r << rp, p(M); return r; }/// 群对直线的作用。 /// SOPHUS_FUNC Line operator*(Line const& l) const { return Line((*this) * l.origin(), l.direction()); // 返回变换后的直线 }
/// 群对平面的作用。 /// SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const { return Hyperplane(p.normal(), p.offset() - params().dot(p.normal())); // 返回变换后的超平面 }
/// 就地群乘法。此方法仅在乘法的返回类型与此笛卡尔的标量类型兼容时有效。 /// template <typename OtherDerived, typename = typename std::enable_if_t< std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>> SOPHUS_FUNC CartesianBase<Derived, M>& operator*=( CartesianBase<OtherDerived, M> const& other) { *static_cast<Derived*>(this) = *this * other; return *this; }
/// 修改 params 向量。 /// SOPHUS_FUNC ParamsType& params() { return static_cast<Derived*>(this)->params(); // 返回 params 向量 }
/// 访问 params 向量 /// SOPHUS_FUNC ParamsType const& params() const { return static_cast<Derived const*>(this)->params(); // 返回常量 params 向量 }};
/// 使用默认存储的 Cartesian；派生自 CartesianBase。template <class Scalar_, int M, int Options>class Cartesian : public CartesianBase<Cartesian<Scalar_, M, Options>, M> { using Base = CartesianBase<Cartesian<Scalar_, M, Options>, M>;
public: static int constexpr DoF = Base::DoF; // 自由度 static int constexpr num_parameters = Base::num_parameters; // 参数数量 static int constexpr N = Base::N; static int constexpr Dim = Base::Dim;
using Scalar = Scalar_; using Transformation = typename Base::Transformation; using Point = typename Base::Point; using HomogeneousPoint = typename Base::HomogeneousPoint; using Tangent = typename Base::Tangent; using ParamsMember = Sophus::Vector<Scalar, M, Options>;
using Base::operator=;
/// 显式定义复制赋值运算符。在存在用户声明的复制构造函数的情况下，隐式复制赋值运算符的定义已弃用（在 clang >= 13 中为 -Wdeprecated-copy）。 SOPHUS_FUNC Cartesian& operator=(Cartesian const& other) = default;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// 默认构造函数初始化为零向量。 /// SOPHUS_FUNC Cartesian() { params_.setZero(); }
/// 复制构造函数 /// SOPHUS_FUNC Cartesian(Cartesian const& other) = default;
/// 类似复制的构造函数 /// template <class OtherDerived> SOPHUS_FUNC Cartesian(CartesianBase<OtherDerived, M> const& other) : params_(other.params()) { static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value, "must be same Scalar type"); // 确保标量类型相同 }
/// 接受 M 向量或 (M+1)x(M+1) 矩阵。 /// template <class D> explicit SOPHUS_FUNC Cartesian(Eigen::MatrixBase<D> const& m) { static_assert( std::is_same<typename Eigen::MatrixBase<D>::Scalar, Scalar>::value, ""); // 确保矩阵基类型的标量类型相同 if (m.rows() == DoF && m.cols() == 1) { // 编译时的技巧 params_ = m.template block<M, 1>(0, 0); } else if (m.rows() == N && m.cols() == N) { params_ = m.template block<M, 1>(0, M); } else { SOPHUS_ENSURE(false, "{} {}", m.rows(), m.cols()); } }
/// 提供对内部数据的不安全读写访问。 /// SOPHUS_FUNC Scalar* data() { return params_.data(); }
/// 上面的 const 版本。 /// SOPHUS_FUNC Scalar const* data() const { return params_.data(); }
/// 返回 exp(x) 相对于 x 的导数。 /// SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x_at_0() { Sophus::Matrix<Scalar, num_parameters, DoF> m; m.setIdentity(); return m; }
/// 返回 exp(x) 相对于 x_i 在 x=0 处的导数。///SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x( Tangent const&) { return Dx_exp_x_at_0(); // 调用 Dx_exp_x_at_0 函数}
/// 返回 exp(x) * p 对 x_i 在 x=0 处的导数。///SOPHUS_FUNC static Sophus::Matrix<Scalar, Dim, DoF> Dx_exp_x_times_point_at_0( Point const&) { Sophus::Matrix<Scalar, Dim, DoF> J; // 创建矩阵 J J.setIdentity(); // 将矩阵 J 设为单位矩阵 return J; // 返回单位矩阵 J}
/// 返回 exp(x).matrix() 对 ``x_i 在 x=0 处的导数。///SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) { return generator(i); // 返回第 i 个生成元}
/// 修改参数向量///SOPHUS_FUNC ParamsMember& params() { return params_; // 返回参数向量}
/// 访问参数向量///SOPHUS_FUNC ParamsMember const& params() const { return params_; // 返回参数向量}
/// 返回 Cartesian(M) 的第 i 个无穷小生成元。////// 例如，三维情况下的无穷小生成元:////// ```/// | 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 |/// ```////// 前置条件: ``i`` 必须在 [0, M-1] 范围内。///SOPHUS_FUNC static Transformation generator(int i) { SOPHUS_ENSURE(i >= 0 && i <= M, "i 应该在 [0, M-1] 范围内。"); // 检查 i 的范围 Tangent e; // 创建切线向量 e e.setZero(); // 将 e 置零 e[i] = Scalar(1); // 设置 e 的第 i 个元素为 1 return hat(e); // 返回 hat(e)}
/// 群指数////// 对于欧几里得向量空间，仅为单位映射。更准确地说，/// 它只是从 M 向量构造 (M+1)x(M+1) 的齐次矩阵表示。///SOPHUS_FUNC static Cartesian<Scalar, M> exp(Tangent const& a) { return Cartesian<Scalar, M>(a); // 返回 Cartesian 对象}
/// hat-运算符////// 正式地说，Cartesian(M) 的 hat() 运算符定义为////// ``hat(.): R^M -> R^{M+1xM+1}, hat(a) = sum_i a_i * G_i``/// (对于 i=0,...,M-1)////// 其中 ``G_i`` 是 Cartesian(M) 的第 i 个无穷小生成元。////// 对应的逆运算是 vee() 运算符，见下文。///SOPHUS_FUNC static Transformation hat(Tangent const& a) { Transformation Omega; // 创建变换矩阵 Omega Omega.setZero(); // 将 Omega 置零 Omega.col(M).template head<M>() = a.template head<M>(); // 设置 Omega 的最后一列的前 M 个元素 return Omega; // 返回变换矩阵 Omega}
/// 李括号////// 对于交换群，总是 0。SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) { return Tangent::Zero(); // 返回零向量}
/// 在每个坐标范围内绘制 [-1, 1] 的均匀样本。///template <class UniformRandomBitGenerator>static Cartesian sampleUniform(UniformRandomBitGenerator& generator) { std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1)); // 创建均匀分布对象 Vector<Scalar, M> v; // 创建向量 v for (int i = 0; i < M; ++i) { v[i] = uniform(generator); // 为向量 v 生成均匀样本 } return Cartesian(v); // 返回生成的 Cartesian 对象}
/// vee-运算符////// 这是 hat() 运算符的逆运算，见上文。///SOPHUS_FUNC static Tangent vee(Transformation const& m) { return m.col(M).template head<M>(); // 返回变换矩阵 m 的最后一列的前 M 个元素}
protected: ParamsMember params_; // 定义参数成员变量};
} // namespace Sophus
namespace Eigen { // 定义命名空间 Eigen
/// Eigen::Map 的 ``Cartesian`` 特化; 派生自 CartesianBase。////// 允许我们将 Cartesian 对象包装在 POD 数组周围。template <class Scalar_, int M, int Options>class Map<Sophus::Cartesian<Scalar_, M>, Options> : public Sophus::CartesianBase<Map<Sophus::Cartesian<Scalar_, M>, Options>, M> { public: using Base = Sophus::CartesianBase<Map<Sophus::Cartesian<Scalar_, M>, Options>, M>; // 定义基类类型 using Scalar = Scalar_; // 定义标量类型 using Transformation = typename Base::Transformation; // 定义变换类型 using Point = typename Base::Point; // 定义点类型 using HomogeneousPoint = typename Base::HomogeneousPoint; // 定义齐次点类型 using Tangent = typename Base::Tangent; // 定义切向量类型
using Base::operator=; // 继承基类的赋值操作符 using Base::operator*=; // 继承基类的乘法赋值操作符 using Base::operator*; // 继承基类的乘法操作符
SOPHUS_FUNC explicit Map(Scalar* coeffs) : params_(coeffs) {} // 显式构造函数，使用参数初始化
/// 修改参数向量 /// SOPHUS_FUNC Map<Sophus::Vector<Scalar, M, Options>>& params() { return params_; // 返回参数向量的引用 }
/// 访问参数向量 /// SOPHUS_FUNC Map<Sophus::Vector<Scalar, M, Options>> const& params() const { return params_; // 返回参数向量的常量引用 }
protected: Map<Sophus::Vector<Scalar, M>, Options> params_; // 参数成员变量};
/// Eigen::Map 的 ``Cartesian const`` 特化; 派生自 CartesianBase。////// 允许我们将 Cartesian 对象包装在 POD 数组周围。template <class Scalar_, int M, int Options>class Map<Sophus::Cartesian<Scalar_, M> const, Options> : public Sophus::CartesianBase<Map<Sophus::Cartesian<Scalar_, M> const, Options>, M> { public: using Base = Sophus::CartesianBase<Map<Sophus::Cartesian<Scalar_, M> const, Options>, M>; // 定义基类类型 using Scalar = Scalar_; // 定义标量类型 using Transformation = typename Base::Transformation; // 定义变换类型 using Point = typename Base::Point; // 定义点类型 using HomogeneousPoint = typename Base::HomogeneousPoint; // 定义齐次点类型 using Tangent = typename Base::Tangent; // 定义切向量类型
using Base::operator*; // 继承基类的乘法操作符
SOPHUS_FUNC Map(Scalar const* coeffs) : params_(coeffs) {} // 构造函数，使用常量参数初始化
/// 访问参数向量 /// SOPHUS_FUNC Map<Sophus::Vector<Scalar, M> const, Options> const& params() const { return params_; // 返回参数向量的常量引用 }
protected: Map<Sophus::Vector<Scalar, M> const, Options> const params_; // 常量参数成员变量};} // namespace Eigen
扫一扫
1785
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
