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

这篇具有很好参考价值的文章主要介绍了(02)Cartographer源码无死角解析-(16) SensorBridge→Rigid3(刚体变换)。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示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主要实现了如下几个接口(粗略看一下步骤即可,后面有代码注释)：
$\color{blue}(1)：$ 共三个构造函数(一个默认,两个重载)，默认构造函数平移与旋转设置都为0，重载构造函数可以通过传入平移与旋转进行初始化，旋转可以使用四元数或者轴角表示。但是最终都是以四元数的格式存储的。另外还有4个创建实例化对象的静态重载函数，单独传入平移和旋转都可以生成实例(没有传入的默认为0)，另外以 std::array 格式同时传入平移与旋转也可创建实例化对象

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

$\color{blue}(3)：$ 欧式变换群求逆函数 Rigid3 inverse() ，推导公式如下所示(代码注解在后面)：
$\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$ $\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}$

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

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

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

$\color{blue}(5)：$ 另外还有一个 operator $*$ 函数得重载：

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$

$\color{blue}(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 的矩阵，如下所示
$\color{Green} \tag{04} \mathbf T^{-1}=\begin{bmatrix} \mathbf R^{-1}& -\mathbf R^{-1}\mathbf t\\ \\ 0 & 1 \end{bmatrix}$ 又因为在代码中， $\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 进行讲解。

文章来源地址https://www.toymoban.com/news/detail-401373.html

到了这里，关于(02)Cartographer源码无死角解析-(16) SensorBridge→Rigid3(刚体变换)的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！