(02)Cartographer源码无死角解析-(16) SensorBridge→Rigid3(刚体变换)

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

通过上一篇博客，可以了解到，每条轨迹 (trajectory_id) 都对应一个 SensorBridge 类对象，其被存储于MapBuilderBridge 的成员变量 sensor_bridges_ 之中：

std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;

SensorBridge 的初始化位于 MapBuilderBridge::AddTrajectory() 函数之中，代码如下：

  // Step: 2 为这个新轨迹 添加一个SensorBridge
  sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>(
      trajectory_options.num_subdivisions_per_laser_scan,
      trajectory_options.tracking_frame,
      node_options_.lookup_transform_timeout_sec, 
      tf_buffer_,
      map_builder_->GetTrajectoryBuilder(trajectory_id)); // CollatedTrajectoryBuilder

SensorBridge 的实现位于 src/cartographer_ros/cartographer_ros/cartographer_ros/sensor_bridge.cc 文件中，在对齐进行讲解之前，先来看如下两个类：

//src/cartographer_ros/cartographer_ros/cartographer_ros/tf_bridge.cc
class TfBridge

//src/cartographer/cartographer/transform/rigid_transform.cc
class Rigid3

 

二、Rigid3

首先来看看其头文件 rigid_transform.h，该中实现了两个模板类

template <typename FloatType>
class Rigid3 {}

template <typename FloatType>
class Rigid2 {}

先从复杂的 Rigid3 说起。
Rigid3主要实现了如下几个接口(粗略看一下步骤即可,后面有代码注释)：
$(1)：$ 共三个构造函数(一个默认,两个重载)，默认构造函数平移与旋转设置都为0，重载构造函数可以通过传入平移与旋转进行初始化，旋转可以使用四元数或者轴角表示。但是最终都是以四元数的格式存储的。另外还有4个创建实例化对象的静态重载函数，单独传入平移和旋转都可以生成实例(没有传入的默认为0)，另外以 std::array 格式同时传入平移与旋转也可创建实例化对象

$(2)：$ 实现静态函数 Identity()，返回平移与旋转都为0的实例。 实现类中的模板函数 Rigid3<OtherType> cast()，注意调用该函数的时候，需要使用 .template 关键字。

$(3)：$ 欧式变换群求逆函数 Rigid3 inverse() ，推导公式如下所示(代码注解在后面)：
$$\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ & \\ 0& 1\end{array}\right]设{\mathbf{T}}^{-1}=\left[\begin{array}{cc}\mathbf{A}& \mathbf{b}\\ & \\ c& d\end{array}\right]由于:\mathbf{T}{\mathbf{T}}^{-1}=\mathbf{E}\text{\hspace{1em}(01)}$$$$所以\{\begin{array}{l}\mathbf{R}\mathbf{A}+c\mathbf{t}=\mathbf{E}\\ c=0\\ \mathbf{R}\mathbf{b}+d\mathbf{t}=0\\ d=1\end{array}得:\{\begin{array}{l}\mathbf{A}={\mathbf{R}}^{-1}\\ \\ \mathbf{t}=-{\mathbf{R}}^{-1}\mathbf{t}\end{array}所以：{\mathbf{T}}^{-1}=\left[\begin{array}{cc}{\mathbf{R}}^{-1}& -{\mathbf{R}}^{-1}\mathbf{t}\\ & \\ 0& 1\end{array}\right]\text{\hspace{1em}(02)}$$

$(4)：$ 另外对模板类 Rigid3<FloatType> 还实现了 ‘$\ast$’ 操作函数，即 operator$\ast$ 函数，其有两个重载函数，其一：

template <typename FloatType>
Rigid3<FloatType> operator*(const Rigid3<FloatType>& lhs,
                            const Rigid3<FloatType>& rhs) 

该函数主要作用为两个欧式变换群相乘法，推导过程如下：
$${\mathbf{T}}_a=\left[\begin{array}{cc}{\mathbf{R}}_a& {\mathbf{t}}_a\\ & \\ 0& 1\end{array}\right]{\mathbf{T}}_b=\left[\begin{array}{cc}{\mathbf{R}}_b& {\mathbf{t}}_b\\ & \\ 0& 1\end{array}\right]{\mathbf{T}}_a{\mathbf{T}}_b=\left[\begin{array}{cc}{\mathbf{R}}_a{\mathbf{R}}_b& {\mathbf{R}}_a{\mathbf{t}}_b+{\mathbf{t}}_a\\ & \\ 0& 1\end{array}\right]\text{\hspace{1em}(03)}$$

$(5)：$ 另外还有一个 operator$\ast$ 函数得重载：

template <typename FloatType>
typename Rigid3<FloatType>::Vector operator*(
    const Rigid3<FloatType>& rigid,
    const typename Rigid3<FloatType>::Vector& point) 

其就是把点 $\mathbf{p}$ 进行坐标变换，即 ${\mathbf{p}}_{new}=\mathbf{R}\mathbf{p}+\mathbf{t}$

$(6)：$ RollPitchYaw 函数，把欧拉角转换成四元数。
 
代码的注释如下：

template <typename FloatType>
class Rigid3 {
 public:
  using Vector = Eigen::Matrix<FloatType, 3, 1>; //用Vector代替表示Eigen中的旋转矩阵
  using Quaternion = Eigen::Quaternion<FloatType>; //用Quaternion代替表示Eigen中的四元数
  using AngleAxis = Eigen::AngleAxis<FloatType>; //用AngleAxis代替表示Eigen中的轴角
  
  //默认构造函数,对平移translation_与旋转rotation_两个变量通过初始化列表进行初始化,全为0
  Rigid3() : translation_(Vector::Zero()), rotation_(Quaternion::Identity()) {}
  //构造函数重载,传入一个向量表示的平移translation, 与四元数表示的旋转进行初始化
  Rigid3(const Vector& translation, const Quaternion& rotation)
      : translation_(translation), rotation_(rotation) {}
  //构造函数重载,传入一个向量表示的平移translation, 与与轴角表示的旋转
  Rigid3(const Vector& translation, const AngleAxis& rotation)
      : translation_(translation), rotation_(rotation) {}

  //声明该为静态函数，该函数可以通过Rigid3::Rotation()直接进行调用，
  //而非必须创建实例之后才能调用,理解为python中的类函数,注意其没有this指针
  static Rigid3 Rotation(const AngleAxis& angle_axis) {
    return Rigid3(Vector::Zero(), Quaternion(angle_axis));
  }
  //该为重载函数，作用与上一函数一样，就是根据传入的参数创建一个Rigid3实例返回,
  //该实例平移初始值都为0, 旋转使用传入的参数进行表示
  static Rigid3 Rotation(const Quaternion& rotation) {
    return Rigid3(Vector::Zero(), rotation);
  }
  //根据传入的参数创建一个Rigid3实例返回,
  //该实例平移为传入的vector,旋转初始化全为0
  static Rigid3 Translation(const Vector& vector) {
    return Rigid3(vector, Quaternion::Identity());
  }
  //根据以数组形式传入的四元素旋转rotation，以及平移translation构建一个实例
  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]));
  }
  //创建一个初始化全为0的Rigid3实例
  static Rigid3<FloatType> Identity() { return Rigid3<FloatType>(); }

  //该函数主要实现数据的类型转换,把原来的数据类型转化为OtherType
  template <typename OtherType> 
  Rigid3<OtherType> cast() const {
    //.template的用法比较简单，因为cast<OtherType>() 为 Eigen::Matrix 实例对象的
    //模板函数，所以使用.template声明，告诉编译器,接下来要调用的是一个类中实现的模板函数。
    //如果直接调用 translation_.cast<OtherType>() 会报错如下：
    //error: expected primary-expression before ‘>’ token，
    //简单的说就是编译器弄不清楚translation_.cast后面'<'是解析成模板还是解析成小于符号
    return Rigid3<OtherType>(translation_.template cast<OtherType>(),
                             rotation_.template cast<OtherType>());
  }

  //const修饰返回值，表示返回值不能被修改，只能赋值给其他变量
  //const修饰函数体，或者花括号，表示函数体或者花括号中，都是常量操作，
  //且其中只能调用使用const修饰的函数。另外这里返回的变量为应勇类型
  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 { //absl::Substitute 是一个高效的字符串替换函数，用于调试信息的打印
    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 { //检测这些数据是否有效，如平移的xyz不能为nan，四元数各个元素平方和为1。
    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_; //旋转私有成员变量
};


//实现模板类Rigid3的 '*' 操作，该操作为两个 Rigid3 实例进行 '*' 运算
//lhs(Left Hand Side)表示乘法操作的左值,  rhs(Right Hand Side)表示乘法操作的右值
// Tlhs=[Rl tl]   Trhs = [Rr  tr]    Tlhs*Trhs=[Rl*Rr      Rl*tr+tl]
//      [0  1 ]          [0   1 ]              [0            1     ]
// Tlhs 与 Trhs 都是欧式变换群
template <typename FloatType>
Rigid3<FloatType> operator*(const Rigid3<FloatType>& lhs,
                            const Rigid3<FloatType>& rhs) {
  return Rigid3<FloatType>(
      lhs.rotation() * rhs.translation() + lhs.translation(),
      (lhs.rotation() * rhs.rotation()).normalized());
}

//该函数的功能为对一个3维点进行欧式变换
//p_new = R*p + t 
template <typename FloatType>
typename Rigid3<FloatType>::Vector operator*(
    const Rigid3<FloatType>& rigid,
    const typename Rigid3<FloatType>::Vector& point) {
  return rigid.rotation() * point + rigid.translation();
}

// This is needed for gmock.  //实现cout打印与输出功能
template <typename T>
std::ostream& operator<<(std::ostream& os,
                         const cartographer::transform::Rigid3<T>& rigid) {
  os << rigid.DebugString();
  return os;
}

using Rigid3d = Rigid3<double>;  //类似于Eigen中的设计
using Rigid3f = Rigid3<float>;

// Converts (roll, pitch, yaw) to a unit length quaternion. Based on the URDF
// specification http://wiki.ros.org/urdf/XML/joint.
Eigen::Quaterniond RollPitchYaw(double roll, double pitch, double yaw);

// Returns an transform::Rigid3d given a 'dictionary' containing 'translation'
// (x, y, z) and 'rotation' which can either we an array of (roll, pitch, yaw)
// or a dictionary with (w, x, y, z) values as a quaternion.
Rigid3d FromDictionary(common::LuaParameterDictionary* dictionary);

 

三、Rigid2

在了解了Rigid3之后，在来了解Rigid2就比较简单了。class Rigid2 这个模板类主要实现2维的刚性变换。三维空间中表示旋转，使用的是四元数。在2维空间表示旋转只需要一个角度就可以了，变量对应如下代码：

  using Rotation2D = Eigen::Rotation2D<FloatType>;
  Rotation2D rotation_;

另外，对于二变换来说来说，推导公式还是与前面一样的，只是这里的 $\mathbf{R}$ 是 2x2 的矩阵，如下所示
$${\mathbf{T}}^{-1}=\left[\begin{array}{cc}{\mathbf{R}}^{-1}& -{\mathbf{R}}^{-1}\mathbf{t}\\ & \\ 0& 1\end{array}\right]\text{\hspace{1em}(04)}$$又因为在代码中， $\mathbf{R}$ 使用 Rotation2D rotation_表示，其实际就是一个角度，所以对其求逆，就是在该角度的前面加个负号就可以，所以 Rigid2 inverse()::Rigid2 inverse() 的代码实现如下：

  // T = [R t] T^-1 = [R^-1  -R^-1 * t]
  //     [0 1]        [0         1    ] 
  // R是旋转矩阵, 特殊正交群, 所以R^-1 = R^T
  Rigid2 inverse() const {
    const Rotation2D rotation = rotation_.inverse();
    const Vector translation = -(rotation * translation_);
    return Rigid2(translation, rotation);
  }

其他的实现与 Rigid3 基本都比较类似，这里就不进行细致的讲解了。
 

四、结语

对 /src/cartographer/cartographer/transform/rigid_transform.cc 文件中的 Rigid3(刚体变换) 进行了详细的简介，接下来还要对 /src/cartographer_ros/cartographer_ros/cartographer_ros/tf_bridge.cc 中的 class TfBridge 进行讲解。