Cartographer学习之sensor_bridge-Rigid3d

就不告诉你噢

已于 2023-04-13 09:15:54 修改

阅读量452

点赞数

文章标签：学习

于 2023-04-12 20:41:08 首次发布

本文链接：https://blog.csdn.net/m0_55879913/article/details/130106124

版权

上篇文章主要介绍了map_builder_bridge_,其中提到了sensor_bridge。将在本篇文章中详细介绍,来到sensor_bridge.cc文件，具体代码段如下：

namespace cartographer_ros {

namespace carto = ::cartographer;

using carto::transform::Rigid3d;

namespace {

// 检查frame_id是否带有'/',如果带则报错
const std::string& CheckNoLeadingSlash(const std::string& frame_id) {
  if (frame_id.size() > 0) {
    CHECK_NE(frame_id[0], '/') << "The frame_id " << frame_id
                               << " should not start with a /. See 1.7 in "
                                  "http://wiki.ros.org/tf2/Migration.";
  }
  return frame_id;
}

首先申明需要使用的命名空间，检查frame_id格式是否符合要求，如果其中带有“/”便会报错误信息。接下来我们先了解一下Rigid3d,转到rigid_transform.h，二维变化省略，直接看到三维刚体的坐标变换实现，逐行来进行分析：

template <typename FloatType>
class Rigid3 {
 public:
  using Vector = Eigen::Matrix<FloatType, 3, 1>;
  using Quaternion = Eigen::Quaternion<FloatType>;
  using AngleAxis = Eigen::AngleAxis<FloatType>;

 private:
  Vector translation_;
  Quaternion rotation_;
};

这里使用了template调用模板类的模板成员函数，该函数主要实现数据的类型转换,把原来的数据类型转化为FloatType，当要使用模板类中的模板函数时, 如果同时满足下面两个条件:

1.如果模板类的模板参数是不确定类型时（int和非模板类等是确定类型）

2.显式提供模板函数的模板参数, 不管模板参数是确定类型还是不确定类型

需要在模板函数前加template关键字。Rigid3这个类首先申明了三个Eigen类型的参数，其中Vector表示一个三行一列的列向量，Quaternion表示一个四元数以及AngleAxis代表的的轴角。然后定义了平移向量translation为Vector类型，且旋转矩阵rotation定义为QUaternion类型。

接着继续往下看：

 Rigid3() : translation_(Vector::Zero()), rotation_(Quaternion::Identity()) {}
  Rigid3(const Vector& translation, const Quaternion& rotation)
      : translation_(translation), rotation_(rotation) {}
  Rigid3(const Vector& translation, const AngleAxis& rotation)
      : translation_(translation), rotation_(rotation) {}

第一个是默认构造函数,对平移translation_与旋转rotation_两个变量通过初始化列表进行初始化，后面两个则是构造重载函数,第二个函数，传入一个平移向量translation, 与旋转矩阵进行初始化，第三个函数则是传入一个向量表示的平移translation, 与轴角表示的旋转进行初始化。

  static Rigid3 Rotation(const AngleAxis& angle_axis) {
    return Rigid3(Vector::Zero(), Quaternion(angle_axis));
  }

  static Rigid3 Rotation(const Quaternion& rotation) {
    return Rigid3(Vector::Zero(), rotation);
  }

  static Rigid3 Translation(const Vector& vector) {
    return Rigid3(vector, Quaternion::Identity());
  }

  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>(); }

接着申明了几个静态变量，根据传入的参数创建一个Rigid3实例返回，前两个初始平移分量皆为0，轴角以及旋转都根据传入的参数值进行表示。第三个平移向量根据传入的表示，其中旋转矩阵的初值为0。第四个根据以数组形式传入的四元数旋转矩阵rotation，以及平移translation构建一个实例。

  template <typename OtherType>
  Rigid3<OtherType> cast() const {
    return Rigid3<OtherType>(translation_.template cast<OtherType>(),
                             rotation_.template cast<OtherType>());
  }

  const Vector& translation() const { return translation_; }
  const Quaternion& rotation() const { return rotation_; }

如上文所述，该函数主要实现数据的类型转换,把原来的数据类型转化为OtherType，因为cast<OtherType>() 为 Eigen::Matrix 实例对象的模板函数，所以使用.template声明，告诉编译器。接下来要调用的是一个类中实现的模板函数。const修饰返回值，表示返回值不能被修改，只能赋值给其他变量，const修饰函数体，或者花括号，表示函数体或者花括号中，都是常量操作，
且其中只能调用使用const修饰的函数。

  Rigid3 inverse() const {
    const Quaternion rotation = rotation_.conjugate();
    const Vector translation = -(rotation * translation_);
    return Rigid3(translation, rotation);
  }

Rigid3 inverse()函数为欧式变换群求逆函数：

${\color{Red} T= \bigl(\begin{smallmatrix} R & &t\\ 0& & 1 \end{smallmatrix}\bigr)}$ 设 ${\color{Red} T^{-1}= \bigl(\begin{smallmatrix} A & &b\\ c& & d \end{smallmatrix}\bigr)}$ 由 $TT^{_{-1}}=E$ 可得

$c=0$ $d=1$ $RA+ct=E$ $Rb+dt=0$ 得 $A=R^{_{-1}}$ $t=-R^{_{-1}}t$

即 $T^{-1}=\bigl(\begin{smallmatrix} R^{-1} & & R^{-1} t\\ 0 & & 1 \end{smallmatrix}\bigr){\color{Red} }$ ,conjugate函数是用来取共轭，等价于旋转矩阵求逆。最后返回欧式变换群的逆。

 std::string DebugString() const {
    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());
  }

其中absl命名空间中的Substitute是一个高效的字符串替换函数，用于调试信息的打印，将平移向量以及旋转矩阵的信息打印出来。

 bool IsValid() const {
    return !std::isnan(translation_.x()) && !std::isnan(translation_.y()) &&
           !std::isnan(translation_.z()) &&
           std::abs(FloatType(1) - rotation_.norm()) < FloatType(1e-3);
  }

再用IsValid函数来判断数据是否有效，std::isnan全局函数用于检查其参数是否是非数字值。如果 x 是特殊的非数字值 NaN（或者能被转换为这样的值），返回的值就是 true。如果 x 是其他值,则返回 false。

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());
}

lhs是全局坐标系下的位姿, rhs是全局坐标系下的位姿变动量，描述了两个坐标系下的变换关系。也可以理解为两个欧式变换群的乘法：
$T_{a}$ 表示在a坐标系下的旋转以及平移，而 $T_{b}$ 则是在b坐标系下的变换矩阵

$T_{a}=\bigl(\begin{smallmatrix} R_{a} & t_{a}\\ 0& 1 \end{smallmatrix}\bigr)$ $T_{b}=\bigl(\begin{smallmatrix} R_{b} & t_{b}\\ 0& 1 \end{smallmatrix}\bigr)$

$T_{a}T_{b}=\bigl(\begin{smallmatrix} R_{a} R_{b}&R_{a} t_{b}+t_{a}\\ 0& 1 \end{smallmatrix}\bigr)$

template <typename FloatType>
typename Rigid3<FloatType>::Vector operator*(
    const Rigid3<FloatType>& rigid,
    const typename Rigid3<FloatType>::Vector& point) {
  return rigid.rotation() * point + rigid.translation();
}

实现了一个点欧式变换 $P_{_{new}}=RP+t$

// This is needed for gmock.
template <typename T>
std::ostream& operator<<(std::ostream& os,
                         const cartographer::transform::Rigid3<T>& rigid) {
  os << rigid.DebugString();
  return os;
}

using Rigid3d = Rigid3<double>;
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);

}  // namespace transform
}  // namespace cartographer

#endif  // CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_H_