cartographer源码阅读三-sensor_bridge

这个类涉及到两个类Rigid3d和TfBridgeRigid3d类实现了一个3 维刚体变换类Rigid3vector Eigen 库中的 3x1 列向量用于存储3 维平移量x、y、z 坐标QuaternionEigen 库中的四元数用于存储3 维旋转姿态相比旋转矩阵四元数更节省内存、避免万向锁问题插值更平滑AngleAxisEigen 库中的轴角表示也是一种旋转表示方式一个旋转轴 一个旋转角度可以直接转换为四元数。提供了 3 种构造方式满足不同场景的初始化需求核心是初始化两个私有成员translation_平移和rotation_旋转。后面声明了几个静态成员函数static修饰成员函数就代表即使没有实例对象也可以调用using Rigid2d Rigid2double; using Rigid2f Rigid2float; template typename FloatType class Rigid3 { public: using Vector Eigen::MatrixFloatType, 3, 1; using Quaternion Eigen::QuaternionFloatType; using AngleAxis Eigen::AngleAxisFloatType; // 默认构造函数平移为零向量旋转为单位四元数无旋转 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::arrayFloatType, 4 rotation, const std::arrayFloatType, 3 translation) { return Rigid3(Eigen::Mapconst Vector(translation.data()), Eigen::QuaternionFloatType(rotation[0], rotation[1], rotation[2], rotation[3])); } static Rigid3FloatType Identity() { return Rigid3FloatType(); } template typename OtherType Rigid3OtherType cast() const { return Rigid3OtherType(translation_.template castOtherType(), rotation_.template castOtherType()); } 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_; };