(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 cast(),注意调用该函数的时候,需要使用 .template 关键字。

( 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                T1= Acbd             由于:  TT1=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=R1t=R1t       所以:T1= R10R1t1 (02)

( 4 ) : \color{blue}(4): (4) 另外对模板类 Rigid3 还实现了 ‘ ∗ * ’ 操作函数,即 operator ∗ * 函数,其有两个重载函数,其一:

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} T1= R10R1t1 又因为在代码中, 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 进行讲解。

 
 
 

你可能感兴趣的:(自动驾驶,无人机,机器人,增强现实,Cartographer)