(02)Cartographer源码无死角解析-(17) SensorBridge→里程计数据坐标系变换与TfBridge分析

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

接下来会涉及到很多坐标以及部分基变换的知识，如果对方面不是很理解的朋友，请先阅读这篇博客：
待写…

阅读之后，继续分析代码，首先找到 src/cartographer_ros/cartographer_ros/cartographer_ros/sensor_bridge.h 文件，可以看到其中定义了很多处理函数，其与 node.cc 中的函数是一一对应的，其可以直接理解为订阅话题时注册的回调函数，对应关系如下：

//所有单线雷达topic回调函数
Node::HandleLaserScanMessage() =  SensorBridge::HandleLaserScanMessage() 
//所有回声波雷达topoc回调函数
Node::HandleMultiEchoLaserScanMessage() = SensorBridge::HandleMultiEchoLaserScanMessage()
//所有多线雷达topic回调函数
Node::HandlePointCloud2Message()  =  SensorBridge::HandlePointCloud2Message()
//所有IMU topic回调函数
Node::HandleImuMessage()  =  SensorBridge::HandleImuMessage() 
//所有里程计 topic回调函数
Node::HandleOdometryMessage()  =  SensorBridge::HandleOdometryMessage()
//GPS topic回调函数
Node::HandleNavSatFixMessage()  =  SensorBridge::HandleNavSatFixMessage()
//Landmar 回调函数
Node::HandleLandmarkMessage()  =  SensorBridge::HandleLandmarkMessage()  

另外 sensor_bridge.h 文件 class SensorBridge 中，可以看到有如下几个成员变量：

const int num_subdivisions_per_laser_scan_;   //一帧数据分成几段处理
const TfBridge tf_bridge_;
::cartographer::mapping::TrajectoryBuilderInterface* consttrajectory_builder_; //轨迹构建器
std::map<std::string, cartographer::common::Time>sensor_to_previous_subdivision_time_; //记录点云数据段的时间戳

回到之前讲解过的类容，在 src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc 文件中的
MapBuilderBridge::AddTrajectory() 函数中可以找到如下代码：

  // Step: 2 为这个新轨迹 添加一个SensorBridge
  sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>(
      trajectory_options.num_subdivisions_per_laser_scan, //一帧数据分成几次处理，一般设置为1
      trajectory_options.tracking_frame, //把所有数据都转换到该坐标系下 
      node_options_.lookup_transform_timeout_sec, //查找tf时的超时时间
      tf_buffer_, //用于存储 tf
      map_builder_->GetTrajectoryBuilder(trajectory_id)); // CollatedTrajectoryBuilder 轨迹构建器

这里使用 absl::make_unique<SensorBridge> 创建了，表示其返回的是一个 unique_ptr 智能指针：unique_ptr是一种定义在<memory>中的智能指针(smart pointer)。它持有对对象的独有权——两个unique_ptr不能指向一个对象，不能进行复制操作只能进行移动操作。map_builder_->GetTrajectoryBuilder() 后续会做详细讲解，其实际返回的是一个 CollatedTrajectoryBuilder 对象。
 

二、构造函数

通过上面的介绍，知道在实例化 SensorBridge 对象的时候，其会传入 5 个参数。sensor_bridge.cc 文件，其构造函数如下：

/**
 * @brief 构造函数, 并且初始化TfBridge
 * 
 * @param[in] num_subdivisions_per_laser_scan 一帧数据分成几次发送
 * @param[in] tracking_frame 数据都转换到tracking_frame
 * @param[in] lookup_transform_timeout_sec 查找tf的超时时间
 * @param[in] tf_buffer tf_buffer
 * @param[in] trajectory_builder 轨迹构建器
 */
SensorBridge::SensorBridge(
    const int num_subdivisions_per_laser_scan,
    const std::string& tracking_frame,
    const double lookup_transform_timeout_sec, tf2_ros::Buffer* const tf_buffer,
    carto::mapping::TrajectoryBuilderInterface* const trajectory_builder)
    : num_subdivisions_per_laser_scan_(num_subdivisions_per_laser_scan),
      tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer),
      trajectory_builder_(trajectory_builder) {}

代码比较简单，仅仅使用初始化列表，对 SensorBridge 的变量进行了初始化而已。
 

三、class TfBridge

构造函数初始化列表中，从上面可以看到如下代码：

tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer)

其根据输入参数实例化了一个 TfBridge 对象，然后赋值给 SensorBridge::tf_bridge_，TfBridge 的头文件路径为：src/cartographer_ros/cartographer_ros/cartographer_ros/tf_bridge.h 可以看到，其包含了三个成员变量如下：

const std::string tracking_frame_  //后续数据都转换到该坐标系下
lookup_transform_timeout_sec,  查找tf时的超时时间
tf_buffer //用于存储tf数据

另外可与看到关键字delete：

  TfBridge(const TfBridge&) = delete;
  TfBridge& operator=(const TfBridge&) = delete;

其表示删除默认拷贝构造函数以及默认赋值重载函数，也就说禁用了该类的拷贝以及等号赋值功能，强行使用的话会报错，那么再来看一下 tf_bridge.cc 文件，具体分析该类的实现。对于 TfBridge 构造函数可以说十分简单，使用初始化列表对变量进行初始化而已。然后其只实现了一个函数 TfBridge::LookupToTracking()，该函数的主要功能查找从 frame_id 坐标 到 tracking_frame_ 坐标变换矩阵，亦或者说 tracking_frame_ 坐标原点在 frame_id 坐标系下的位姿。

$注意$ 根据前面的信息，传入到 TfBridge::LookupToTracking() 函数的 tracking_frame_ 实际为 .lua 文件中的 tracking_frame = “imu_link” 参数，也就是说当前 trajectory_id 轨迹下的所有数据，都会转换到 imu_link 节点下。通常选择频率最高的传感器，因为所有的传感器都需要转换到 tracking_frame 之后再进行计算，如果选择低频传感器，那么高频传感器将会浪费大量的时间用于坐标系转换。TfBridge::LookupToTracking() 函数代码代码注释如下：

// note: LookupToTracking 查找从frame_id 到frame_id tracking_frame_ 的坐标变换
// imu_link(tracking_frame_) 在 base_link(frame_id) 上方 0.1m处
// 那这里返回的坐标变换的z是 -0.1
std::unique_ptr<::cartographer::transform::Rigid3d> TfBridge::LookupToTracking(
    const ::cartographer::common::Time time, //查询该时间点的tf数据
    const std::string& frame_id) const { //对应配置文件中的 tracking_frame 参数
  ::ros::Duration timeout(lookup_transform_timeout_sec_); //查询超时时间
  std::unique_ptr<::cartographer::transform::Rigid3d> frame_id_to_tracking;
  try {
    //查询tracking_frame_坐标系与frame_id坐标系发布时间
    //最近数据的时间戳。::ros::Time(0.)表示查询最新的数据。
    const ::ros::Time latest_tf_time = 
        buffer_
            ->lookupTransform(tracking_frame_, frame_id, ::ros::Time(0.),
                              timeout)
            .header.stamp;
    const ::ros::Time requested_time = ToRos(time);
    //查询到tf数据的时间，需要比time大，简单的说，就是有新生成
    if (latest_tf_time >= requested_time) {
      // We already have newer data, so we do not wait. Otherwise, we would wait
      // for the full 'timeout' even if we ask for data that is too old.
      //因为已经有了更新的数据，所以不等待。设置为0
      //否则如果没有新数据，将会进行等待，直到超时
      timeout = ::ros::Duration(0.);
    }
    //如果前面有新数据产生则直接获取requested_time=time时间点的tf数据
    //否则将进行等待，直到超时
    return absl::make_unique<::cartographer::transform::Rigid3d>(
        ToRigid3d(buffer_->lookupTransform(tracking_frame_, frame_id,
                                           requested_time, timeout)));
  } catch (const tf2::TransformException& ex) {//等待超时，即表示没有新数据产生
    LOG(WARNING) << ex.what();
  }
  return nullptr; //超时返回空指针
}

注意 buffer_->lookupTransform() 函数返回的是一个消息类型 geometry_msgs::TransformStamped，类似这样的消息类型可以使用如果两个指令进行查看消息内容：

rosmsg list
rosmsg info geometry_msgs/TransformStamped

本人打印如下：

然后通过 ToRigid3d() 函数，可以转换成 Rigid3d 类对象。另外，buffer_->lookupTransform() 函数返回的是 tracking_frame_ 坐标系到 frame_id 坐标系过度矩阵， 等价于 frame_id 坐标系下坐标变换到 tracking_frame_ 坐标系下的变换矩阵，亦或者说 tracking_frame_ 坐标原点 frame_id 坐标系中的位姿。
 

四、::ToOdometryData()

在了解了 TfBridge 之后，这里那么进行一个示例讲解，也就是 sensor_bridge.cc 中的 SensorBridge::ToOdometryData 函数，先设置 .lua 文件中的 $reduse\_odometry$ 为 $true$，否则该函数不会被执行。

$功能(数据类型转换)\to$ 将ros格式的里程计数据 转成tracking frame的pose, 再转成carto的里程计数据类型。

SensorBridge::ToOdometryData() 函数的输入数据为 nav_msgs::Odometry::ConstPtr& msg(这里的 nav_msgs 是属于 ROS 的一个命名空间)，通过终端执行指令 rosmsg show nav_msgs/Odometry 可以看到该msg 的信息如下：

std_msgs/Header header   //消息头  （std_msgs:标准数据类型）
  uint32 seq //消息序列
  time stamp //时间戳
  string frame_id //绑定坐标系 
string child_frame_id //数据以其为原点，或者说数据为该坐标系下的数据
geometry_msgs/PoseWithCovariance pose  //表示带有不确定性的一个空间位姿，协方差不为零
  geometry_msgs/Pose pose //几何位姿为信息  
    geometry_msgs/Point position  //位置信息，也就是平移
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation //位姿信息，也就是旋转
      float64 x
      float64 y
      float64 z
      float64 w
  float64[36] covariance //协方差
geometry_msgs/TwistWithCovariance twist //里程计传感器的线速度与角速度(Covariance表示不确定)
  geometry_msgs/Twist twist  //包含了线速度与角速度信息
    geometry_msgs/Vector3 linear //线速度
      float64 x
      float64 y
      float64 z
    geometry_msgs/Vector3 angular //角度信息
      float64 x
      float64 y
      float64 z
  float64[36] covariance  线速度与角速度协方差

其实，虽然 msg 包含了很多信息，但是只需要该信息中的时间戳以及 msg->child_frame_id 就可以了，代码如下：

// 将ros格式的里程计数据 转成tracking frame的pose, 再转成carto的里程计数据类型
std::unique_ptr<carto::sensor::OdometryData> SensorBridge::ToOdometryData(
    const nav_msgs::Odometry::ConstPtr& msg) {
  const carto::common::Time time = FromRos(msg->header.stamp);
  // 找到 tracking坐标系 到 里程计的child_frame_id 的坐标变换, 所以下方要对sensor_to_tracking取逆
  const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
      time, CheckNoLeadingSlash(msg->child_frame_id));
  if (sensor_to_tracking == nullptr) {
    
    return nullptr;
  }

其首先调用了 cartographer_ros::FromRos(const ros::Time &time) 函数。该函数主要的作用就是进行了一个时间的转换，把 ros::Time 转换成 cartographer::common::Time 格式。

在 src/cartographer_ros/cartographer_ros/cartographer_ros/time_conversion.cc 文件中，实现了 ToRos(将 ICU Universal Time 转成 ROS的时间) 与 FromRos(将 ROS的时间 转成 ICU Universal Time) 两个函数。ICU世界时标的纪元是“0001-01-01 00:00:00.0+0000”,正好比Unix纪元早719162天。因为Unix纪元是从 1971 开始的。代码注释如下：

// 将 ICU Universal Time 转成 ROS的时间
::ros::Time ToRos(::cartographer::common::Time time) {
  int64_t uts_timestamp = ::cartographer::common::ToUniversal(time);
  int64_t ns_since_unix_epoch =
      (uts_timestamp -
       ::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds *
           10000000ll) *
      100ll;
  ::ros::Time ros_time;
  ros_time.fromNSec(ns_since_unix_epoch);
  return ros_time;
}

// TODO(pedrofernandez): Write test.
// 将 ROS的时间 转成 ICU Universal Time
::cartographer::common::Time FromRos(const ::ros::Time& time) {
  // The epoch of the ICU Universal Time Scale is "0001-01-01 00:00:00.0 +0000",
  // exactly 719162 days before the Unix epoch.
  // ICU世界时标的纪元是“0001-01-01 00:00:00.0+0000”,正好比Unix纪元早719162天 
  return ::cartographer::common::FromUniversal(
      (time.sec +
       ::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds) *
          10000000ll +
      (time.nsec + 50) / 100);  // + 50 to get the rounding correct.
}

然后再调用了 TfBridge::LookupToTracking() 函数，该函数以及在上面介绍过了，传入了两个参数 time 与 msg->child_frame_id。本人的 child_frame_id 为 “footprint”。也就是说，这里 TfBridge::LookupToTracking() 查询到的数据为 child_frame_id(“footprint”) 坐标 到 tracking_frame_(“imu_link”)坐标的变换矩阵，或者说 “imu_link” 坐标系原点在 “footprint” 坐标系下的位姿。

$重点，其上的目的，都是为了实现如下目标$

$(1)$ msg 的 pose 表示的是机器人相对于世界坐标(可能是第一帧,后续再确认)，同时可以把该位姿，理解为坐标系，也就是机器人的坐标系。

$(2)$ tf_bridge_.LookupToTracking() 查询到的是 child_frame_id(“footprint”) 机器人坐标到 tracking(“imu_link”) 坐标变换，等价于 tracking(“imu_link”) 坐标系到 child_frame_id(“footprint”) 坐标系的变换。这里使用 ${\mathbf{T}}_{tf}$ 表示。

$(3)$ 可以通过对 ${\mathbf{T}}_{tf}$ 求逆，那么得到的结果为 ${\mathbf{T}}_{ft}$，其代表的含义是child_frame_id(“footprint”) 机器人坐标系到 tracking(“imu_link”) 坐标系的过渡矩阵。也就是说，通过这个矩阵，就可以把机器人的坐标系，变换成 imu 的坐标系。

$(4)$ 这里记机器人的坐标系为 $T_{wl}$，其$w$表示世界坐标系，当然，也有可能是局部地图坐标系，不过没关系，后续再确认。$[{\mathbf{T}}_w=[{\mathbf{T}}_{wf}\ast [{\mathbf{T}}_{tf}{]}^{-1}]$ 用过机器人坐标系，借助过度矩阵，求解出 imu 坐标系，显然都是先对于世界坐标系而言。

$(5)$ 值得注意的是，msg 的 pose 表示的是机器人的位姿，而不是相对于机器人的数据，该数据是相对于世界坐标系而言。

代码注释如下：

// 将ros格式的里程计数据 转成tracking frame的pose, 再转成carto的里程计数据类型
std::unique_ptr<carto::sensor::OdometryData> SensorBridge::ToOdometryData(
    const nav_msgs::Odometry::ConstPtr& msg) {

  const carto::common::Time time = FromRos(msg->header.stamp);
  // 找到 tracking坐标到里程计的child_frame_id坐标变换, 
  const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
      time, CheckNoLeadingSlash(msg->child_frame_id));
  if (sensor_to_tracking == nullptr) {
    return nullptr;
  }

  // 将里程计的footprint的pose转成tracking_frame的pose, 再转成carto的里程计数据类型
  // msg->pose.pose 为 child_frame_id 坐标系下的数据。
  return absl::make_unique<carto::sensor::OdometryData>(  
      carto::sensor::OdometryData{
          time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse()});
}

 

五、::HandleOdometryMessage()

SensorBridge::HandleOdometryMessage()函数会调用到 ToOdometryData(msg) ，该函数的注释如下：

// 调用trajectory_builder_的AddSensorData进行数据的处理
void SensorBridge::HandleOdometryMessage(
    const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) {
  // 数据类型与数据坐标系的转换
  std::unique_ptr<carto::sensor::OdometryData> odometry_data =
      ToOdometryData(msg);
  if (odometry_data != nullptr) {
    // tag: 这个trajectory_builder_是谁, 讲map_builder时候再揭晓
    trajectory_builder_->AddSensorData(
        sensor_id,
        carto::sensor::OdometryData{odometry_data->time, odometry_data->pose});
  }
}

这里的 trajectory_builder_后续再进行详细的讲解。
 

六、结语

另外这里额外的谈一下 本人源码配置 msg 中的 child_frame_id = “footprint”，总的来说机器人所有节点(如相机，IMU，雷达等)位姿数据，都要转换到 footprint 这个节点下再进行处理。简单的理解，可以认为是机器人的重心，再描述机器人位姿的时候，通常是说机器人在什么位置，朝什么方向，而不会说，机器人的相机，或者雷达在什么位置，朝什么方向，也就是说，机器人身上的某个传感器位姿，其是不能直接代替机器人的位姿，所以需要进行转换。

如果机器人 tf 树如下：
认为以 base_link 或者 footprint 为重新都是可以的，机器人上的传感器节点 GPS_back_link、GPS_front_link、Imu_llink、front_laser_link 的位置数据，都要变换到 base_link 或者 footprint 节点下，然后再交给后面的程序处理。