(02)Cartographer源码无死角解析-(16) SensorBridge→回调函数之数据流向分析

讲解关于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_;
1

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
1
2
3
4
5
6
7

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
1
2
3
4
5

 

二、Rigid3

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

template <typename FloatType>
class Rigid3 {}

template <typename FloatType>
class Rigid2 {}
1
2
3
4
5

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

$(2)：$ 实现静态函数 Identity()，返回平移与旋转都为0的实例。 实现类中的模板函数 Rigid3 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 还实现了 ‘$\ast$’ 操作函数，即 operator$\ast$ 函数，其有两个重载函数，其一：

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

该函数主要作用为两个欧式变换群相乘法，推导过程如下：
$${\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) 
1
2
3
4

其就是把点 $\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() 为 Eigen::Matrix 实例对象的
    //模板函数，所以使用.template声明，告诉编译器,接下来要调用的是一个类中实现的模板函数。
    //如果直接调用 translation_.cast() 会报错如下：
    //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);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128

 

三、Rigid2

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

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

另外，对于二变换来说来说，推导公式还是与前面一样的，只是这里的 $\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);
  }
1
2
3
4
5
6
7
8

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

四、结语

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