cartographer源码阅读三-sensor_bridge

这个类涉及到两个类Rigid3d和TfBridge

Rigid3d类

实现了一个3 维刚体变换类Rigid3

vector ：Eigen 库中的 3x1 列向量，用于存储3 维平移量（x、y、z 坐标）

Quaternion：Eigen 库中的四元数，用于存储3 维旋转姿态（相比旋转矩阵，四元数更节省内存、避免万向锁问题，插值更平滑）

AngleAxis：Eigen 库中的轴角表示，也是一种旋转表示方式（一个旋转轴 + 一个旋转角度），可以直接转换为四元数。

提供了 3 种构造方式，满足不同场景的初始化需求，核心是初始化两个私有成员：translation_（平移）和rotation_（旋转）。

后面声明了几个静态成员函数，static修饰成员函数就代表即使没有实例对象也可以调用

cpp 复制代码

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_;
};