该代码文件定义了 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;