【C++】sophus : se3.hpp 完整的SE(3)群的实现，包括旋转和平移的组合表示、转换、映射及其相关操作 (十六)...

最新推荐文章于 2024-12-19 20:20:20 发布

十年一梦实验室

最新推荐文章于 2024-12-19 20:20:20 发布

阅读量1.4k

点赞数 33

文章标签： c++ 开发语言

本文链接：https://blog.youkuaiyun.com/cxyhjl/article/details/144573442

版权

该代码文件定义了 Sophus 库中用于表示三维空间刚性变换的SE3 类。

文件概要

该文件包含SE3 类的定义，用于表示三维空间的旋转和平移。
它依赖于so3.hpp 文件，其中定义了SO3 类，用于表示三维空间的旋转。

类定义：SE3

SE3
类是SE3Base 的模板特化，用于指定具体的数值类型 (Scalar) 和选项 (Options)。
它表示刚性变换，由旋转部分 (so3) 和平移向量 (translation) 组成。

SE3 的成员函数：

DoF
和num_parameters：分别表示自由度 (6) 和内部参数个数 (7)。
Transformation
,Point,HomogeneousPoint,Tangent,Adjoint：定义了用于表示变换矩阵、点、齐次点、切空间向量和伴随矩阵的类型。
Adj()
: 计算SE3 元素的伴随矩阵。
angleX()
,angleY(),angleZ(): 分别提取绕 X 轴、Y 轴、Z 轴的旋转角度。
cast<NewScalarType>()
: 将当前SE3 对象转换为指定数值类型NewScalarType 的新对象。
Dx_this_mul_exp_x_at_0()
: 计算this * exp(x) 在 x=0 处的导数 (相对于 x)。
Dx_log_this_inv_by_x_at_this()
: 计算log(this^-1 * x) 在 x=this 处的导数 (相对于 x)。
inverse()
: 计算SE3 元素的逆变换。
log()
: 计算SE3 元素的对数 (对应于指数映射的逆运算)。
normalize()
: 归一化SO3 元素 (通常不需要手动调用)。
matrix()
: 返回SE3 元素的 4x4 齐次变换矩阵。
matrix3x4()
: 返回变换矩阵的前三行 (旋转矩阵加上平移向量)。
赋值运算符 (=): 从另一个SE3 对象拷贝数据。
乘法运算符 (*): 计算两个SE3 元素的组合变换。
其他乘法运算符 (*)：定义了SE3 元素作用于三维点、齐次点、线段和平面的运算。
*=
: 就地更新的乘法运算 (仅当运算结果的数值类型与当前SE3 对象的数值类型一致时有效)。
rotationMatrix()
: 返回旋转矩阵。
so3()
: 获取或设置SO3 元素 (存取旋转部分)。
setQuaternion()
: 设置SO3 元素 (使用四元数表示旋转)。
setRotationMatrix()
: 设置SO3 元素 (使用旋转矩阵表示旋转)。
params()
: 返回SE3 元素的内部参数向量。
translation()
: 获取或设置平移向量。
unit_quaternion()
: 获取单位四元数 (表示旋转)。

构造函数：

SE3()
: 默认构造函数，初始化为单位变换。
其他构造函数：允许使用各种方式创建SE3 对象，例如通过SO3 对象、旋转矩阵、四元数和平移向量等。

其他成员函数：

data()
: 提供对内部数据的非安全读写访问 (需要使用者确保四元数保持归一化)。

该代码定义了SE3 类，用于方便地表示和操作三维空间的刚性变换。它提供了丰富的成员函数，可以进行变换的组合、作用于几何对象以及各种参数的存取和计算。

/// @file/// 特殊欧几里得群SE(3) - 三维空间中的旋转和平移。
#pragma once
#include "so3.hpp" // 包含SO3的头文件
namespace Sophus { // Sophus命名空间template <class Scalar_, int Options = 0>class SE3; // 定义模板类SE3using SE3d = SE3<double>; // 定义SE3d为SE3<double>的别名using SE3f = SE3<float>; // 定义SE3f为SE3<float>的别名} // namespace Sophus
namespace Eigen { // Eigen命名空间namespace internal { // 内部命名空间
template <class Scalar_, int Options>struct traits<Sophus::SE3<Scalar_, Options>> { // 定义Sophus::SE3的traits模板结构体  using Scalar = Scalar_; // 定义Scalar类型  using TranslationType = Sophus::Vector3<Scalar, Options>; // 定义平移类型  using SO3Type = Sophus::SO3<Scalar, Options>; // 定义SO3类型};
template <class Scalar_, int Options>struct traits<Map<Sophus::SE3<Scalar_>, Options>>    : traits<Sophus::SE3<Scalar_, Options>> { // 定义Sophus::SE3的Map类型的traits模板结构体  using Scalar = Scalar_; // 定义Scalar类型  using TranslationType = Map<Sophus::Vector3<Scalar>, Options>; // 定义平移类型  using SO3Type = Map<Sophus::SO3<Scalar>, Options>; // 定义SO3类型};
template <class Scalar_, int Options>struct traits<Map<Sophus::SE3<Scalar_> const, Options>>    : traits<Sophus::SE3<Scalar_, Options> const> { // 定义const类型的Map类型的traits模板结构体  using Scalar = Scalar_; // 定义Scalar类型  using TranslationType = Map<Sophus::Vector3<Scalar> const, Options>; // 定义平移类型  using SO3Type = Map<Sophus::SO3<Scalar> const, Options>; // 定义SO3类型};} // namespace internal} // namespace Eigen
namespace Sophus {
/// SE3基础类型 - 实现SE3类，但与存储无关。// SE(3)是三维空间中的旋转和平移群。它是SO(3)与三维欧几里得向量空间的半直积。该类/// 使用SO3表示旋转，用一个3维向量表示平移。// SE(3)既不是紧群，也不是交换群。// 有关三维旋转表示的更多细节，请参见SO3。///template <class Derived>class SE3Base { public:  using Scalar = typename Eigen::internal::traits<Derived>::Scalar; // 定义标量类型  using TranslationType =      typename Eigen::internal::traits<Derived>::TranslationType; // 定义平移类型  using SO3Type = typename Eigen::internal::traits<Derived>::SO3Type; // 定义SO3类型  using QuaternionType = typename SO3Type::QuaternionType; // 定义四元数类型  /// 流形的自由度，切空间中的维数  /// （平移为三维，旋转为三维）。  static int constexpr DoF = 6; // 自由度为6  /// 使用的内部参数数量（四元数的4元组，平移的3元组）。  static int constexpr num_parameters = 7; // 内部参数数量为7  /// 群变换为4x4矩阵。  static int constexpr N = 4; // 变换矩阵为4x4  /// 点为3维  static int constexpr Dim = 3; // 点的维数为3  using Transformation = Matrix<Scalar, N, N>; // 定义变换矩阵类型  using Point = Vector3<Scalar>; // 定义点类型  using HomogeneousPoint = Vector4<Scalar>; // 定义齐次点类型  using Line = ParametrizedLine3<Scalar>; // 定义参数化线类型  using Hyperplane = Hyperplane3<Scalar>; // 定义超平面类型  using Tangent = Vector<Scalar, DoF>; // 定义切向量类型  using Adjoint = Matrix<Scalar, DoF, DoF>; // 定义伴随矩阵类型
  /// 对于二元操作，返回类型由  /// Eigen的ScalarBinaryOpTraits特性确定。这允许混合具体和Map类型，  /// 以及其他兼容的标量类型，如Ceres::Jet和  /// double标量与SE3操作。  template <typename OtherDerived>  using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<      Scalar, typename OtherDerived::Scalar>::ReturnType; // 定义返回标量类型
  template <typename OtherDerived>  using SE3Product = SE3<ReturnScalar<OtherDerived>>; // 定义SE3乘积类型
  template <typename PointDerived>  using PointProduct = Vector3<ReturnScalar<PointDerived>>; // 定义点乘积类型
  template <typename HPointDerived>  using HomogeneousPointProduct = Vector4<ReturnScalar<HPointDerived>>; // 定义齐次点乘积类型
  /// 伴随变换  ///  /// 这个函数返回群元素``A``的伴随变换``Ad``，对于所有``x``，满足  /// ``hat(Ad_A * x) = A * hat(x) A^{-1}``。见下面的hat运算符。  ///  SOPHUS_FUNC Adjoint Adj() const {    Sophus::Matrix3<Scalar> const R = so3().matrix(); // 获取SO3矩阵    Adjoint res; // 定义伴随矩阵    res.block(0, 0, 3, 3) = R; // 设置伴随矩阵块    res.block(3, 3, 3, 3) = R; // 设置伴随矩阵块    res.block(0, 3, 3, 3) = SO3<Scalar>::hat(translation()) * R; // 设置伴随矩阵块    res.block(3, 0, 3, 3) = Matrix3<Scalar>::Zero(3, 3); // 设置伴随矩阵块    return res; // 返回伴随矩阵  }
  /// 提取绕X轴的旋转角度  ///  Scalar angleX() const { return so3().angleX(); } // 调用so3()对象的angleX()方法，返回绕X轴的旋转角度
  /// 提取绕Y轴的旋转角度  ///  Scalar angleY() const { return so3().angleY(); } // 调用so3()对象的angleY()方法，返回绕Y轴的旋转角度
  /// 提取绕Z轴的旋转角度  ///  Scalar angleZ() const { return so3().angleZ(); } // 调用so3()对象的angleZ()方法，返回绕Z轴的旋转角度
  /// 返回实例转换为NewScalarType类型的副本  ///  template <class NewScalarType>  SOPHUS_FUNC SE3<NewScalarType> cast() const { // 模板函数，返回NewScalarType类型的SE3对象    return SE3<NewScalarType>(so3().template cast<NewScalarType>(),                              translation().template cast<NewScalarType>()); // 转换so3和translation为NewScalarType类型  }
  /// 返回this * 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    Eigen::Quaternion<Scalar> const q = unit_quaternion(); // 获取单位四元数    Scalar const c0 = Scalar(0.5) * q.w(); // 计算常数c0    Scalar const c1 = Scalar(0.5) * q.z(); // 计算常数c1    Scalar const c2 = -c1; // 计算常数c2    Scalar const c3 = Scalar(0.5) * q.y(); // 计算常数c3    Scalar const c4 = Scalar(0.5) * q.x(); // 计算常数c4    Scalar const c5 = -c4; // 计算常数c5    Scalar const c6 = -c3; // 计算常数c6    Scalar const c7 = q.w() * q.w(); // 计算常数c7    Scalar const c8 = q.x() * q.x(); // 计算常数c8    Scalar const c9 = q.y() * q.y(); // 计算常数c9    Scalar const c10 = -c9; // 计算常数c10    Scalar const c11 = q.z() * q.z(); // 计算常数c11    Scalar const c12 = -c11; // 计算常数c12    Scalar const c13 = Scalar(2) * q.w(); // 计算常数c13    Scalar const c14 = c13 * q.z(); // 计算常数c14    Scalar const c15 = Scalar(2) * q.x(); // 计算常数c15    Scalar const c16 = c15 * q.y(); // 计算常数c16    Scalar const c17 = c13 * q.y(); // 计算常数c17    Scalar const c18 = c15 * q.z(); // 计算常数c18    Scalar const c19 = c7 - c8; // 计算常数c19    Scalar const c20 = c13 * q.x(); // 计算常数c20    Scalar const c21 = Scalar(2) * q.y() * q.z(); // 计算常数c21    J(0, 0) = 0; // 设置矩阵J的值    J(0, 1) = 0;    J(0, 2) = 0;    J(0, 3) = c0;    J(0, 4) = c2;    J(0, 5) = c3;    J(1, 0) = 0;    J(1, 1) = 0;    J(1, 2) = 0;    J(1, 3) = c1;    J(1, 4) = c0;    J(1, 5) = c5;    J(2, 0) = 0;    J(2, 1) = 0;

最低0.47元/天解锁文章