(02)Cartographer源码无死角解析-(16) SensorBridge→Rigid3(刚体变换)
本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
文末正下方中心提供了本人 联系方式， 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证
一、前言
通过上一篇博客，可以了解到，每条轨迹 (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 ) ： \color{blue}(1)： (1)： 共三个构造函数(一个默认,两个重载)，默认构造函数平移与旋转设置都为0，重载构造函数可以通过传入平移与旋转进行初始化，旋转可以使用四元数或者轴角表示。但是最终都是以四元数的格式存储的。另外还有4个创建实例化对象的静态重载函数，单独传入平移和旋转都可以生成实例(没有传入的默认为0)，另外以 std::array 格式同时传入平移与旋转也可创建实例化对象
( 2 ) ： \color{blue}(2)： (2)： 实现静态函数 Identity()，返回平移与旋转都为0的实例。 实现类中的模板函数 Rigid3
( 3 ) ： \color{blue}(3)： (3)： 欧式变换群求逆函数 Rigid3 inverse() ，推导公式如下所示(代码注解在后面)：
T = [ R t 0 1 ] 设 T − 1 = [ A b c d ] 由于 : T T − 1 = E (01) \color{Green} \tag{01} \mathbf T =\begin{bmatrix} \mathbf R& \mathbf t\\ \\ 0 & 1 \end{bmatrix}~~~~~~~~~~~~~~~设 \mathbf T^{-1}=\begin{bmatrix} \mathbf A& \mathbf b\\ \\ c & d \end{bmatrix} ~~~~~~~~~~~~由于:~~\mathbf T \mathbf T^{-1}=\mathbf E T=⎣ ⎡R0t1⎦ ⎤ 设T−1=⎣ ⎡Acbd⎦ ⎤ 由于: TT−1=E(01) 所以 { R A + c t = E c = 0 R b + d t = 0 d = 1 得 : { A = R − 1 t = − R − 1 t 所以： T − 1 = [ R − 1 − R − 1 t 0 1 ] (02) \color{Green} \tag{02}所以 \begin{cases} \mathbf R \mathbf A + c\mathbf t=\mathbf E\\ c=0\\ \mathbf R \mathbf b+d \mathbf t=0\\ d=1 \end{cases}~~~~~~~~得: \begin{cases} \mathbf A=\mathbf R^{-1} \\ \\ \mathbf t=-\mathbf R^{-1}\mathbf t\\ \end{cases}~~~~~~~所以：\mathbf T^{-1}=\begin{bmatrix} \mathbf R^{-1}& -\mathbf R^{-1}\mathbf t\\ \\ 0 & 1 \end{bmatrix} 所以⎩ ⎨ ⎧RA+ct=Ec=0Rb+dt=0d=1 得:⎩ ⎨ ⎧A=R−1t=−R−1t 所以：T−1=⎣ ⎡R−10−R−1t1⎦ ⎤(02)
( 4 ) ： \color{blue}(4)： (4)： 另外对模板类 Rigid3
template <typename FloatType>
Rigid3<FloatType> operator*(const Rigid3<FloatType>& lhs,
const Rigid3<FloatType>& rhs)
该函数主要作用为两个欧式变换群相乘法，推导过程如下：
T a = [ R a t a 0 1 ] T b = [ R b t b 0 1 ] T a T b = [ R a R b R a t b + t a 0 1 ] (03) \color{Green} \tag{03} \mathbf T_a =\begin{bmatrix} \mathbf R_a& \mathbf t_a\\ \\ 0 & 1 \end{bmatrix}~~~~~~~\mathbf T_b =\begin{bmatrix} \mathbf R_b& \mathbf t_b\\ \\ 0 & 1 \end{bmatrix}~~~~~~~\mathbf T_a\mathbf T_b=\begin{bmatrix} \mathbf R_a \mathbf R_b& \mathbf R_a \mathbf t_b+\mathbf t_a\\ \\ 0 & 1 \end{bmatrix} Ta=⎣ ⎡Ra0ta1⎦ ⎤ Tb=⎣ ⎡Rb0tb1⎦ ⎤ TaTb=⎣ ⎡RaRb0Ratb+ta1⎦ ⎤(03)
( 5 ) ： \color{blue}(5)： (5)： 另外还有一个 operator ∗ * ∗ 函数得重载：
template <typename FloatType>
typename Rigid3<FloatType>::Vector operator*(
const Rigid3<FloatType>& rigid,
const typename Rigid3<FloatType>::Vector& point)
其就是把点 p \mathbf p p 进行坐标变换，即 p n e w = R p + t \mathbf p_{new}=\mathbf R \mathbf p+ \mathbf t pnew=Rp+t
( 6 ) ： \color{blue}(6)： (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);
三、Rigid2
在了解了Rigid3之后，在来了解Rigid2就比较简单了。class Rigid2 这个模板类主要实现2维的刚性变换。三维空间中表示旋转，使用的是四元数。在2维空间表示旋转只需要一个角度就可以了，变量对应如下代码：
using Rotation2D = Eigen::Rotation2D<FloatType>;
Rotation2D rotation_;
另外，对于二变换来说来说，推导公式还是与前面一样的，只是这里的 R \mathbf R R 是 2x2 的矩阵，如下所示
T − 1 = [ R − 1 − R − 1 t 0 1 ] \mathbf T^{-1}=\begin{bmatrix} \mathbf R^{-1}& -\mathbf R^{-1}\mathbf t\\ \\ 0 & 1 \end{bmatrix} T−1=⎣ ⎡R−10−R−1t1⎦ ⎤又因为在代码中， R \mathbf R 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 进行讲解。