这个类涉及到两个类Rigid3d和TfBridge
Rigid3d类
实现了一个3 维刚体变换类Rigid3
vector ：Eigen 库中的 3x1 列向量，用于存储3 维平移量（x、y、z 坐标）
Quaternion：Eigen 库中的四元数，用于存储3 维旋转姿态（相比旋转矩阵，四元数更节省内存、避免万向锁问题，插值更平滑）
AngleAxis：Eigen 库中的轴角表示，也是一种旋转表示方式（一个旋转轴 + 一个旋转角度），可以直接转换为四元数。
提供了 3 种构造方式，满足不同场景的初始化需求，核心是初始化两个私有成员：translation_（平移）和rotation_（旋转）。
后面声明了几个静态成员函数，static修饰成员函数就代表即使没有实例对象也可以调用
using Rigid2d = Rigid2<double>; using Rigid2f = Rigid2<float>; template <typename FloatType> class Rigid3 { public: using Vector = Eigen::Matrix<FloatType, 3, 1>; using Quaternion = Eigen::Quaternion<FloatType>; using AngleAxis = Eigen::AngleAxis<FloatType>; // 默认构造函数：平移为零向量，旋转为单位四元数（无旋转） Rigid3() : translation_(Vector::Zero()), rotation_(Quaternion::Identity()) {} // 带参数构造函数：传入平移向量和四元数旋转 Rigid3(const Vector& translation, const Quaternion& rotation) : translation_(translation), rotation_(rotation) {} // 带参数构造函数：传入平移向量和轴角旋转（内部自动转换为四元数存储） Rigid3(const Vector& translation, const AngleAxis& rotation) : translation_(translation), rotation_(rotation) {} static Rigid3 Rotation(const AngleAxis& angle_axis) { return Rigid3(Vector::Zero(), Quaternion(angle_axis)); } static Rigid3 Rotation(const Quaternion& rotation) { return Rigid3(Vector::Zero(), rotation); } static Rigid3 Translation(const Vector& vector) { return Rigid3(vector, Quaternion::Identity()); } static Rigid3 FromArrays(const std::array<FloatType, 4>& rotation, const std::array<FloatType, 3>& translation) { return Rigid3(Eigen::Map<const Vector>(translation.data()), Eigen::Quaternion<FloatType>(rotation[0], rotation[1], rotation[2], rotation[3])); } static Rigid3<FloatType> Identity() { return Rigid3<FloatType>(); } template <typename OtherType> Rigid3<OtherType> cast() const { return Rigid3<OtherType>(translation_.template cast<OtherType>(), rotation_.template cast<OtherType>()); } const Vector& translation() const { return translation_; } const Quaternion& rotation() const { return rotation_; } // T = [R t] T^-1 = [R^-1 -R^-1 * t] // [0 1] [0 1 ] // R是旋转矩阵, 特殊正交群, 所以R^-1 = R^T Rigid3 inverse() const { const Quaternion rotation = rotation_.conjugate(); const Vector translation = -(rotation * translation_); return Rigid3(translation, rotation); } std::string DebugString() const { return absl::Substitute("{ t: [$0, $1, $2], q: [$3, $4, $5, $6] }", translation().x(), translation().y(), translation().z(), rotation().w(), rotation().x(), rotation().y(), rotation().z()); } bool IsValid() const { return !std::isnan(translation_.x()) && !std::isnan(translation_.y()) && !std::isnan(translation_.z()) && std::abs(FloatType(1) - rotation_.norm()) < FloatType(1e-3); } private: Vector translation_; Quaternion rotation_; };
