(02)Cartographer源码无死角解析-(33) LocalTrajectoryBuilder2D: 点云数据流向、处理、消息发布等→流程复盘

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

一、前言

关于 LocalTrajectoryBuilder2D 有关于点云数据处理得部分已经讲解完成了，但是比较杂，比较乱，因为很多地方可能都是跳着讲解得。为了方便大家的理解，这里把相关的总要环节都复盘一下。首相再重述一下之前已经讲解过的数据系统构建过程。
 
$(1)$ 已经知道每新建一条轨迹就会创建一个 CreateGlobalTrajectoryBuilder2D或者 CreateGlobalTrajectoryBuilder3D对象，该对象的实例化位于 MapBuilder::AddTrajectoryBuilder() 函数中。实例化 CreateGlobalTrajectoryBuilder 对象时，会传入一个回调函数 local_slam_result_callback，该函数为 MapBuilderBridge::AddTrajectory 中的表达式：

      // lambda表达式 local_slam_result_callback_
      [this](const int trajectory_id, 
             const ::cartographer::common::Time time, 
             const Rigid3d local_pose,
             ::cartographer::sensor::RangeData range_data_in_local,
             const std::unique_ptr<
                 const ::cartographer::mapping::TrajectoryBuilderInterface::
                     InsertionResult>) {
    
    
        // 保存local slam 的结果数据 5个参数实际只用了4个
        OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
      }

暂且先放一下，只需要知道其核心时调用 OnLocalSlamResult() 函数即可。
 
$(2)$ CreateGlobalTrajectoryBuilder(2D或3D) 对象中包含着一个 LocalTrajectoryBuilder(2D或3D)对象。CreateGlobalTrajectoryBuilder 在把雷达数据分发给 LocalTrajectoryBuilder 之前，会通过阻塞的方式对所有数据按时间进行排序(fix与Landmar可以通过参数配置取消)。

阻塞排序分发数据最重要的就是 cartographer::sensor::Collator 这个类，因为 Collator 中存在变量 OrderedMultiQueue queue_; 该变量中的核心函数 OrderedMultiQueue::Dispatch() 起到了按时间分发数据的主要作用，这里的时间同步，与后续激光雷达点云数据的时间同步时不一样的，回顾之前的内容如下：

        (0, scan): {
    
          4,     }
        (0, imu):  {
    
    1,  3,   5,  }
        (0, odom): {
    
      2,       6,}

这里的时间同步，是针对于所有传感器的数据，按时间戳分发数据。而激光雷达数据的时间同步，是把多个激光雷达数据进行混合，之前的分析图如下：

点云数据时间同步
 
$(3)$ CreateGlobalTrajectoryBuilder 接收到数据之后，就会转发至 LocalTrajectoryBuilder2D 或者 LocalTrajectoryBuilder3D 如果是激光雷达数据，则是调用到 LocalTrajectoryBuilder2D(3D)::AddRangeData() 函数。
 

二、激光雷达点云数据处理

1、多雷达数据时间同步

这里主要以 LocalTrajectoryBuilder2D 处理点云数据为例，经过 CreateGlobalTrajectoryBuilder 转发到 LocalTrajectoryBuilder2D 的数据，即调用 LocalTrajectoryBuilder2D::AddRangeData() 函数进行处理的数据，已经是按时间分发的了，但是多个激光雷达的点云数据分开的，所以需要多个激光雷达传感器的点云数据进行时间同步，其核心就是上面的图示: 点云数据时间同步。主要代码位于

//src/cartographer/cartographer/mapping/internal/range_data_collator.cc
LocalTrajectoryBuilder2D::AddRangeData()
	//src/cartographer/cartographer/mapping/internal/range_data_collator.cc
	sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData()

该代码再前面已经进行过十分详细的讲解，所以这里就不再深入了。需要注意的是，这里同步之后的点云数据依旧是相对于机器人坐标系的，这里记为 $Poin{t}^{tracking}$
 

2、获取local坐标系机器人位姿

获得时间同步之后相对于机器人坐标系的点云数据 $Poin{t}^{tracking}$ 之后，就是需要对点云数据做畸变校正，其核心思想是利用每个点云数据生成时的机器人位姿(相对于local坐标系)，对每个点云进行校正。机器人某个时刻点的位姿，是使用位姿推断器推测出来，核心代码如下：

//src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc
LocalTrajectoryBuilder2D::AddRangeData(const std::string& sensor_id, const sensor::TimedPointCloudData& unsynchronized_data) 
   // 预测得到每一个时间点的位姿
   for (const auto& range : synchronized_data.ranges) {
    
    
       // Step: 2 预测出 每个点的时间戳时刻, tracking frame 在 local slam 坐标系下的位姿
       range_data_poses.push_back(extrapolator_->ExtrapolatePose(time_point).cast<float>());

也就是说，range_data_poses 中存储的就是每个点云生成时机器人相对于 local 坐标系的位姿。这里表示为 ${\mathbf{R}\mathbf{o}\mathbf{b}\mathbf{o}\mathbf{t}}_{tracking}^{local}$。
 

3、点云数据运动畸变校正

利用位姿推断器推测推测出机器人相对于 local 坐标系的位姿，那么可以利用其对点云数据进行校正，校正的过程的就是把点云数据变换到 local 坐标系下面，核心代码如下所示：

//src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc
LocalTrajectoryBuilder2D::AddRangeData(const std::string& sensor_id, const sensor::TimedPointCloudData& unsynchronized_data) 
    // Step: 3 运动畸变的去除, 将相对于tracking_frame的hit坐标 转成 local坐标系下的坐标
    // 把点云数据变换到 local slam 坐标系下
    sensor::RangefinderPoint hit_in_local =
        range_data_poses[i] * sensor::ToRangefinderPoint(hit);

其对应的公式如下：
$$Poin{t}^{local}={\mathbf{R}\mathbf{o}\mathbf{b}\mathbf{o}\mathbf{t}}_{tracking}^{local}\ast Poin{t}^{tracking}\text{\hspace{1em}(01)}$$校正过的数据，会进行积累，积累 num_accumulated_range_data 帧校正过后的数据之后，最终存储于 num_accumulated_range_data 变量中。
 

4、点云数据重力对齐

对点云数据校正之后，会进行重力对齐，重力对齐的核心代码如下所示：

//src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc
LocalTrajectoryBuilder2D::AddRangeData(const std::string& sensor_id, const sensor::TimedPointCloudData& unsynchronized_data) 
	// 获取重力对齐变换矩阵，该矩阵只包含旋转，平移为0，
    //可理解机器人坐标系的Z轴需要与重力矢量平行
    const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
        extrapolator_->EstimateGravityOrientation(time));
	TransformToGravityAlignedFrameAndFilter(
	            gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
	            accumulated_range_data_)

这里记重力对齐矩阵为 ${\mathbf{G}\mathbf{r}\mathbf{a}\mathbf{v}\mathbf{i}\mathbf{t}\mathbf{y}\mathbf{A}\mathbf{l}\mathbf{i}\mathbf{g}\mathbf{n}\mathbf{m}\mathbf{e}\mathbf{n}\mathbf{g}}_{local}^{gravity}$

$${\mathbf{G}\mathbf{r}\mathbf{a}\mathbf{v}\mathbf{i}\mathbf{t}\mathbf{y}\mathbf{A}\mathbf{l}\mathbf{i}\mathbf{g}\mathbf{n}\mathbf{m}\mathbf{e}\mathbf{n}\mathbf{g}}_{local}^{gravity}={\mathbf{G}\mathbf{r}\mathbf{a}\mathbf{v}\mathbf{i}\mathbf{t}\mathbf{y}\mathbf{A}\mathbf{l}\mathbf{i}\mathbf{g}\mathbf{n}\mathbf{m}\mathbf{e}\mathbf{n}\mathbf{g}}_{tracking}^{gravity}\ast [{\mathbf{R}\mathbf{o}\mathbf{b}\mathbf{o}\mathbf{t}}_{tracking}^{loacl}{]}^{-1}\text{\hspace{1em}(02)}$$实际上可以理解为对 local 坐标系做了一个旋转，然后把该变换矩阵应用在点云数据上，公式如下：$$Poin{t}_{}^{gravity}={\mathbf{G}\mathbf{r}\mathbf{a}\mathbf{v}\mathbf{i}\mathbf{t}\mathbf{y}\mathbf{A}\mathbf{l}\mathbf{i}\mathbf{g}\mathbf{n}\mathbf{m}\mathbf{e}\mathbf{n}\mathbf{g}}_{local}^{gravity}\ast Poin{t}_{}^{local}\text{\hspace{1em}(02)}$$ 这里就把原本相对于 local 坐标系下的数据集，变换到重力坐标系 gravity 下面了。随后还对点云进行了 Z 轴上的过滤。
 

三、点云数据匹配与滤波

1、初次滤波

经过时间同步，畸变与重力对齐过后的数据，首先会进行第一次体素滤波，核心代码如下：

//src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc
LocalTrajectoryBuilder2D::AddRangeData(const std::string& sensor_id, const sensor::TimedPointCloudData& unsynchronized_data) 
	sensor::RangeData LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter(consttransform::Rigid3f& transform_to_gravity_aligned_frame,const sensor::RangeData& range_data)
		 // Step: 6 对点云进行体素滤波
		return sensor::RangeData{
    
    
		    cropped.origin,
		    sensor::VoxelFilter(cropped.returns, options_.voxel_filter_size()), // param: voxel_filter_size
		    sensor::VoxelFilter(cropped.misses, options_.voxel_filter_size())};

其上的 sensor::VoxelFilter 函数就包含了体素滤波的主要代码，前面已经很详细的说过了，这里就不再重复了。
 

2、自适应滤波

在完成初次滤波之后，表示 LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter() 函数也完成了，那么就会返回到如下函数：

//src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc
LocalTrajectoryBuilder2D::AddRangeData(const std::string& sensor_id, const sensor::TimedPointCloudData& unsynchronized_data) 
	return AddAccumulatedRangeData(
	   time,
	   // 对点云进行重力对齐，也就是让点云的Z轴与重力方向平行
	   TransformToGravityAlignedFrameAndFilter(
	       gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
	       accumulated_range_data_),
	   gravity_alignment, sensor_duration);

下一步就是执行 AddAccumulatedRangeData() 函数，该函数依旧位于 local_trajectory_builder_2d.cc 文件中。该函数主要核心是进行点云匹配，详细的细节后面再进行讲解。其会调用到如下代码：

  // Step: 7 对 returns点云 进行自适应体素滤波，返回的点云的数据类型是PointCloud
  const sensor::PointCloud& filtered_gravity_aligned_point_cloud =
      sensor::AdaptiveVoxelFilter(gravity_aligned_range_data.returns,
                                  options_.adaptive_voxel_filter_options());
  if (filtered_gravity_aligned_point_cloud.empty()) {
    
    
    return nullptr;
  }

该部分的代码也已经进行过详细讲解了，不太记得的朋友可以回去看看一下前面的博客。
 

四、点云匹配结果保存

LocalTrajectoryBuilder2D::AddAccumulatedRangeData()

该函数执行完成之后，也就是 LocalTrajectoryBuilder2D::AddRangeData() 函数也执行完了，那么回到调用该函数的如下代码：

//src/cartographer/cartographer/mapping/internal/global_trajectory_builder.cc
GlobalTrajectoryBuilder::AddSensorData(const std::string& sensor_id, sensor::TimedPointCloudData& timed_point_cloud_data) override 

中的如下代码：

    // 通过前端进行扫描匹配, 然后返回匹配后的结果
    std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
        matching_result = local_trajectory_builder_->AddRangeData(sensor_id, timed_point_cloud_data);

可知其返回的是一个 LocalTrajectoryBuilder::MatchingResult 类型数据，该数据定于 src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h 文件中，内容如下：

  // 扫描匹配的result
  struct MatchingResult {
    
    
    common::Time time; //多雷达数据时间同步之后的数据时间
    transform::Rigid3d local_pose; //通过扫描匹配获得 local 坐标系下机器人位姿
    sensor::RangeData range_data_in_local; // 经过扫描匹配之后位姿校准之后的雷达数据
    // 'nullptr' if dropped by the motion filter.
    std::unique_ptr<const InsertionResult> insertion_result;
  };

该结果最终被回调函数 local_slam_result_callback_ 使用，回到   一、前言   中的 (1)，可以知道该回调函数的源头，并且其最终调用的的是 OnLocalSlamResult() 函数，该函数 实现于 src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc 文件中，主要功能是把结果数据保存于 MapBuilderBridge::local_slam_data_ 变量中，变量定义如下：

  std::unordered_map<int,
                     std::shared_ptr<const LocalTrajectoryData::LocalSlamData>>
      local_slam_data_ GUARDED_BY(mutex_);

五、点云匹配结果发布

存储于 local_slam_data_ 中的数据，在如下函数中被使用：

//src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc
MapBuilderBridge::GetLocalTrajectoryData() {
    
    
	   // 读取local_slam_data_要上锁
      local_slam_data = local_slam_data_.at(trajectory_id);
  	  // 填充LocalTrajectoryData
   	  local_trajectory_data[trajectory_id] = {
    
     local_slam_data,map_builder_->pose_graph()->GetLocalToGlobalTransform(trajectory_id),sensor_bridge.tf_bridge().LookupToTracking(local_slam_data->time,trajectory_options_[trajectory_id].published_frame),trajectory_options_[trajectory_id]};
   	  return local_trajectory_data;
}

那么 MapBuilderBridge::GetLocalTrajectoryData() 这个函数又是在哪里被使用的呢？在 src/cartographer_ros/cartographer_ros/cartographer_ros/node.cc 文件中，可以看到如下代码：

void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) {
    
    absl::MutexLock lock(&mutex_);
	const auto& entry : map_builder_bridge_.GetLocalTrajectoryData()
	// 获取对应轨迹id的trajectory_data
    const auto& trajectory_data = entry.second;
    // 获取local_slam_data的点云数据, 填入到point_cloud中
    for (const cartographer::sensor::RangefinderPoint point :
        trajectory_data.local_slam_data->range_data_in_local.returns) {
    
    
	    // 这里的虽然使用的是带时间戳的点云结构, 但是数据点的时间全是0.f
	    point_cloud.push_back(cartographer::sensor::ToTimedRangefinderPoint(point, 0.f /* time */));
    }
		// 先将点云转换成ROS的格式,再发布scan_matched_point_cloud点云
		scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(
		   carto::common::ToUniversal(trajectory_data.local_slam_data->time),
		   node_options_.map_frame,
		   // 将local坐标系下的点云转换成地图坐标系下的点云
		   carto::sensor::TransformTimedPointCloud(
		       point_cloud, trajectory_data.local_to_map.cast<float>())));

也就是最终调用了 scan_matched_point_cloud_publisher_.publish() 函数，把点云数据发布了出去，注意如下部分的代码：

	// 将local坐标系下的点云转换成地图坐标系下的点云carto::sensor::TransformTimedPointCloud(
	point_cloud, trajectory_data.local_to_map.cast<float>())));

也就是说，最终发布出去的点云数据，是基于 map 坐标系的。具体的细节，后面还会进行详细的讲解，大家不要着急。
 

六、结语

到目前为止，针对于 LocalTrajectoryBuilder2D 关于点云数据处理部分可以说时完成了，但是该类太过于庞大了，还有很多东西没有进行讲解。不过没有关系，后续过程中会对其完成所有的讲解。既然关于 LocalTrajectoryBuilder2D 对点云的处理已经讲解完成了，那么下面就来看看 LocalTrajectoryBuilder3D。了解 2D 处理过程之后，再了解 3D 就没有难度了。