講解關(guān)于slam一系列文章匯總鏈接:史上最全slam從零開始，針對(duì)于本欄目講解(02)Cartographer源碼無(wú)死角解析-鏈接如下:
(02)Cartographer源碼無(wú)死角解析- (00)目錄_最新無(wú)死角講解：https://blog.csdn.net/weixin_43013761/article/details/127350885
?

?

一、前言

通過(guò)上一篇博客，可以了解到，每條軌跡 (trajectory_id) 都對(duì)應(yīng)一個(gè) SensorBridge 類對(duì)象，其被存儲(chǔ)于MapBuilderBridge 的成員變量 sensor_bridges_ 之中：

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

SensorBridge 的初始化位于 MapBuilderBridge::AddTrajectory() 函數(shù)之中，代碼如下：

  // Step: 2 為這個(gè)新軌跡 添加一個(gè)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 的實(shí)現(xiàn)位于 src/cartographer_ros/cartographer_ros/cartographer_ros/sensor_bridge.cc 文件中，在對(duì)齊進(jìn)行講解之前，先來(lái)看如下兩個(gè)類：

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

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

?

二、Rigid3

首先來(lái)看看其頭文件 rigid_transform.h，該中實(shí)現(xiàn)了兩個(gè)模板類

template <typename FloatType>
class Rigid3 {}

template <typename FloatType>
class Rigid2 {}

先從復(fù)雜的 Rigid3 說(shuō)起。
Rigid3主要實(shí)現(xiàn)了如下幾個(gè)接口(粗略看一下步驟即可,后面有代碼注釋)：
$(1)：$ 共三個(gè)構(gòu)造函數(shù)(一個(gè)默認(rèn),兩個(gè)重載)，默認(rèn)構(gòu)造函數(shù)平移與旋轉(zhuǎn)設(shè)置都為0，重載構(gòu)造函數(shù)可以通過(guò)傳入平移與旋轉(zhuǎn)進(jìn)行初始化，旋轉(zhuǎn)可以使用四元數(shù)或者軸角表示。但是最終都是以四元數(shù)的格式存儲(chǔ)的。另外還有4個(gè)創(chuàng)建實(shí)例化對(duì)象的靜態(tài)重載函數(shù)，單獨(dú)傳入平移和旋轉(zhuǎn)都可以生成實(shí)例(沒(méi)有傳入的默認(rèn)為0)，另外以 std::array 格式同時(shí)傳入平移與旋轉(zhuǎn)也可創(chuàng)建實(shí)例化對(duì)象

$(2)：$ 實(shí)現(xiàn)靜態(tài)函數(shù) Identity()，返回平移與旋轉(zhuǎn)都為0的實(shí)例。 實(shí)現(xiàn)類中的模板函數(shù) Rigid3<OtherType> cast()，注意調(diào)用該函數(shù)的時(shí)候，需要使用 .template 關(guān)鍵字。

$(3)：$ 歐式變換群求逆函數(shù) Rigid3 inverse() ，推導(dǎo)公式如下所示(代碼注解在后面)：
$$\mathbf{T}=?\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ & \\ 0& 1\end{array}?設(shè){\mathbf{T}}^{?1}=?\begin{array}{cc}\mathbf{A}& \mathbf{b}\\ & \\ c& d\end{array}?由于:\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}=?\begin{array}{cc}{\mathbf{R}}^{?1}& ?{\mathbf{R}}^{?1}\mathbf{t}\\ & \\ 0& 1\end{array}?\text{\hspace{1em}(02)}$$

$(4)：$ 另外對(duì)模板類 Rigid3<FloatType> 還實(shí)現(xiàn)了 ‘$?$’ 操作函數(shù)，即 operator$?$ 函數(shù)，其有兩個(gè)重載函數(shù)，其一：

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

該函數(shù)主要作用為兩個(gè)歐式變換群相乘法，推導(dǎo)過(guò)程如下：
$${\mathbf{T}}_a=?\begin{array}{cc}{\mathbf{R}}_a& {\mathbf{t}}_a\\ & \\ 0& 1\end{array}?{\mathbf{T}}_b=?\begin{array}{cc}{\mathbf{R}}_b& {\mathbf{t}}_b\\ & \\ 0& 1\end{array}?{\mathbf{T}}_a{\mathbf{T}}_b=?\begin{array}{cc}{\mathbf{R}}_a{\mathbf{R}}_b& {\mathbf{R}}_a{\mathbf{t}}_b+{\mathbf{t}}_a\\ & \\ 0& 1\end{array}?\text{\hspace{1em}(03)}$$

$(5)：$ 另外還有一個(gè) operator$?$ 函數(shù)得重載：

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

其就是把點(diǎn) $\mathbf{p}$ 進(jìn)行坐標(biāo)變換，即 ${\mathbf{p}}_{new}=\mathbf{R}\mathbf{p}+\mathbf{t}$

$(6)：$ RollPitchYaw 函數(shù)，把歐拉角轉(zhuǎn)換成四元數(shù)。
?
代碼的注釋如下：

template <typename FloatType>
class Rigid3 {
 public:
  using Vector = Eigen::Matrix<FloatType, 3, 1>; //用Vector代替表示Eigen中的旋轉(zhuǎn)矩陣
  using Quaternion = Eigen::Quaternion<FloatType>; //用Quaternion代替表示Eigen中的四元數(shù)
  using AngleAxis = Eigen::AngleAxis<FloatType>; //用AngleAxis代替表示Eigen中的軸角
  
  //默認(rèn)構(gòu)造函數(shù),對(duì)平移translation_與旋轉(zhuǎn)rotation_兩個(gè)變量通過(guò)初始化列表進(jìn)行初始化,全為0
  Rigid3() : translation_(Vector::Zero()), rotation_(Quaternion::Identity()) {}
  //構(gòu)造函數(shù)重載,傳入一個(gè)向量表示的平移translation, 與四元數(shù)表示的旋轉(zhuǎn)進(jìn)行初始化
  Rigid3(const Vector& translation, const Quaternion& rotation)
      : translation_(translation), rotation_(rotation) {}
  //構(gòu)造函數(shù)重載,傳入一個(gè)向量表示的平移translation, 與與軸角表示的旋轉(zhuǎn)
  Rigid3(const Vector& translation, const AngleAxis& rotation)
      : translation_(translation), rotation_(rotation) {}

  //聲明該為靜態(tài)函數(shù)，該函數(shù)可以通過(guò)Rigid3::Rotation()直接進(jìn)行調(diào)用，
  //而非必須創(chuàng)建實(shí)例之后才能調(diào)用,理解為python中的類函數(shù),注意其沒(méi)有this指針
  static Rigid3 Rotation(const AngleAxis& angle_axis) {
    return Rigid3(Vector::Zero(), Quaternion(angle_axis));
  }
  //該為重載函數(shù)，作用與上一函數(shù)一樣，就是根據(jù)傳入的參數(shù)創(chuàng)建一個(gè)Rigid3實(shí)例返回,
  //該實(shí)例平移初始值都為0, 旋轉(zhuǎn)使用傳入的參數(shù)進(jìn)行表示
  static Rigid3 Rotation(const Quaternion& rotation) {
    return Rigid3(Vector::Zero(), rotation);
  }
  //根據(jù)傳入的參數(shù)創(chuàng)建一個(gè)Rigid3實(shí)例返回,
  //該實(shí)例平移為傳入的vector,旋轉(zhuǎn)初始化全為0
  static Rigid3 Translation(const Vector& vector) {
    return Rigid3(vector, Quaternion::Identity());
  }
  //根據(jù)以數(shù)組形式傳入的四元素旋轉(zhuǎn)rotation，以及平移translation構(gòu)建一個(gè)實(shí)例
  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]));
  }
  //創(chuàng)建一個(gè)初始化全為0的Rigid3實(shí)例
  static Rigid3<FloatType> Identity() { return Rigid3<FloatType>(); }

  //該函數(shù)主要實(shí)現(xiàn)數(shù)據(jù)的類型轉(zhuǎn)換,把原來(lái)的數(shù)據(jù)類型轉(zhuǎn)化為OtherType
  template <typename OtherType> 
  Rigid3<OtherType> cast() const {
    //.template的用法比較簡(jiǎn)單，因?yàn)閏ast<OtherType>() 為 Eigen::Matrix 實(shí)例對(duì)象的
    //模板函數(shù)，所以使用.template聲明，告訴編譯器,接下來(lái)要調(diào)用的是一個(gè)類中實(shí)現(xiàn)的模板函數(shù)。
    //如果直接調(diào)用 translation_.cast<OtherType>() 會(huì)報(bào)錯(cuò)如下：
    //error: expected primary-expression before ‘>’ token，
    //簡(jiǎn)單的說(shuō)就是編譯器弄不清楚translation_.cast后面'<'是解析成模板還是解析成小于符號(hào)
    return Rigid3<OtherType>(translation_.template cast<OtherType>(),
                             rotation_.template cast<OtherType>());
  }

  //const修飾返回值，表示返回值不能被修改，只能賦值給其他變量
  //const修飾函數(shù)體，或者花括號(hào)，表示函數(shù)體或者花括號(hào)中，都是常量操作，
  //且其中只能調(diào)用使用const修飾的函數(shù)。另外這里返回的變量為應(yīng)勇類型
  const Vector& translation() const { return translation_; } //返回平移向量
  const Quaternion& rotation() const { return rotation_; } //返回四元數(shù)表示的旋轉(zhuǎn)

  // T = [R t]      T^-1 = [R^-1  -R^-1*t]
  //     [0 1]             [0         1  ] 
  // R是旋轉(zhuǎn)矩陣, 特殊正交群, 所以R^-1 = R^T
  Rigid3 inverse() const {
    const Quaternion rotation = rotation_.conjugate(); //共軛,等價(jià)于旋轉(zhuǎn)矩陣求逆
    const Vector translation = -(rotation * translation_);
    return Rigid3(translation, rotation); //返回歐式變換群的逆
  }

  std::string DebugString() const { //absl::Substitute 是一個(gè)高效的字符串替換函數(shù)，用于調(diào)試信息的打印
    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 { //檢測(cè)這些數(shù)據(jù)是否有效，如平移的xyz不能為nan，四元數(shù)各個(gè)元素平方和為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_; //旋轉(zhuǎn)私有成員變量
};


//實(shí)現(xiàn)模板類Rigid3的 '*' 操作，該操作為兩個(gè) Rigid3 實(shí)例進(jìn)行 '*' 運(yùn)算
//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());
}

//該函數(shù)的功能為對(duì)一個(gè)3維點(diǎn)進(jìn)行歐式變換
//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.  //實(shí)現(xiàn)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中的設(shè)計(jì)
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之后，在來(lái)了解Rigid2就比較簡(jiǎn)單了。class Rigid2 這個(gè)模板類主要實(shí)現(xiàn)2維的剛性變換。三維空間中表示旋轉(zhuǎn)，使用的是四元數(shù)。在2維空間表示旋轉(zhuǎn)只需要一個(gè)角度就可以了，變量對(duì)應(yīng)如下代碼：

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

另外，對(duì)于二變換來(lái)說(shuō)來(lái)說(shuō)，推導(dǎo)公式還是與前面一樣的，只是這里的 $\mathbf{R}$ 是 2x2 的矩陣，如下所示
$${\mathbf{T}}^{?1}=?\begin{array}{cc}{\mathbf{R}}^{?1}& ?{\mathbf{R}}^{?1}\mathbf{t}\\ & \\ 0& 1\end{array}?\text{\hspace{1em}(04)}$$又因?yàn)樵诖a中， $\mathbf{R}$ 使用 Rotation2D rotation_表示，其實(shí)際就是一個(gè)角度，所以對(duì)其求逆，就是在該角度的前面加個(gè)負(fù)號(hào)就可以，所以 Rigid2 inverse()::Rigid2 inverse() 的代碼實(shí)現(xiàn)如下：

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

其他的實(shí)現(xiàn)與 Rigid3 基本都比較類似，這里就不進(jìn)行細(xì)致的講解了。
?

四、結(jié)語(yǔ)

對(duì) /src/cartographer/cartographer/transform/rigid_transform.cc 文件中的 Rigid3(剛體變換) 進(jìn)行了詳細(xì)的簡(jiǎn)介，接下來(lái)還要對(duì) /src/cartographer_ros/cartographer_ros/cartographer_ros/tf_bridge.cc 中的 class TfBridge 進(jìn)行講解。

?
?
?文章來(lái)源地址http://www.zghlxwxcb.cn/news/detail-401373.html

到了這里，關(guān)于(02)Cartographer源碼無(wú)死角解析-(16) SensorBridge→Rigid3(剛體變換)的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！