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>( 
最低0.47元/天 解锁文章
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
