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

该代码文件定义了 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;  
### 如何将 Sophus::SE3f 转换为 OpenCV 的 cv::Mat 类型 在 C++实现从 `Sophus::SE3f` 到 `cv::Mat` 的转换,可以通过以下方法完成。以下是详细的解释以及代码示例。 #### 方法概述 1. 使用 Sophus 库中的 `.matrix()` 函数获取 `Sophus::SE3f` 对象对应的齐次变换矩阵(`Eigen::Matrix4f` 类型)[^1]。 2. 借助 OpenCV 提供的工具函数 `cv::eigen2cv` 将 `Eigen::Matrix4f` 转换为 `cv::Mat` 类型[^2]。 #### 实现代码 下面是一个完整的代码片段,展示如何执行上述操作: ```cpp #include <Eigen/Core> #include <Eigen/Dense> #include <opencv2/opencv.hpp> #include <opencv2/core/eigen.hpp> // 定义变量并初始化 Sophus::SE3f Sophus::SE3f Tcw_SE3f = mpSLAM->TrackMonocular(cv_ptr->image, cv_ptr->header.stamp.toSec()); // 获取 Sophus::SE3f 对应的 4x4 齐次变换矩阵 Eigen::Matrix4f Tcw_Matrix = Tcw_SE3f.matrix(); // 将 Eigen::Matrix4f 转换为 cv::Mat cv::Mat Tcw; cv::eigen2cv(Tcw_Matrix, Tcw); ``` #### 关键点解析 - **`.matrix()` 函数的作用** `Sophus::SE3f` 是一种表示三维空间刚体运动的方式,其内部存储旋转平移信息。调用 `.matrix()` 可以返回一个 `Eigen::Matrix4f` 类型的对象,该对象包含了完整的齐次变换矩阵。 - **`cv::eigen2cv` 工具函数** 这是 OpenCV 提供的一个便捷函数,用于将 Eigen 矩阵类型转换为 OpenCV 的矩阵类型 `cv::Mat`。此函数支持多种数据类型的自动匹配转换。 - **头文件依赖** 上述代码需要引入必要的头文件 `<Eigen/Core>`、`<Eigen/Dense>` `<opencv2/core/eigen.hpp>` 来支持 Eigen OpenCV 的交互功能。 #### 错误排查建议 如果遇到转换失败的情况,请检查以下几点: 1. 是否正确安装了 Sophus、Eigen OpenCV 库,并配置好编译环境。 2. 确认 `mpSLAM->TrackMonocular(...)` 返回的结果是否有效。 3. 如果目标平台不支持浮点数精度计算,则可能需要调整为双精度版本(即使用 `Sophus::SE3d` 替代 `Sophus::SE3f`),并相应修改其他部分的数据类型。 --- ###
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值