Cartographer前端之local_trajectory_builder_2d.cc

 

其中红色框圈起来的则是local_trajectory_builder_2d所处理的所有内容和流程，其输入仅为激光数据（点云）、odometry，Imu DATA。而经过一系列处理后输出结果为cartographer定义的InsertionResult结果类，包含了时间戳、当前机器人位置、submap及在世界坐标的位置、激光点云结合等。即经过前端处理可以得到一系列的submap，如此后续其他模块可将submap进行后端处理拼接形成整个地图。

整体处理流程如下：

1.激光原始数据预处理融合转换成cartographer使用的点云格式；

2.激光点云数据经过两次滤波后进行真正使用；

3.odometry、imu及其匹配后的精确位置构建位置估计器实时估计下刻位置；

4.根据预估位置和点云数据进行scan match获取优化后更精确的位置；

5.经过运动滤波器（目的是降采样）后的位置及其对应的点云进行维护和更新submap；

6.当submap满足一定数量的激光帧时，输出MatchingResult类型结果；

综述

local_trajectory_builder_2d做前端 SLAM，主要实现位姿估计和扫描匹配，生成子图，但不支持闭环检测。 

扫描匹配器(Scan Matiching)，以激光雷达的扫描数据和位姿估计为输入， 使用Ceres库完成扫描匹配，输出位姿的观测值。

一方面反馈给位姿估计器用于修正估计值，另一方面提供给运动滤波器(Motion Filter)用于判定机器人是否产生了运动。

如果产生了运动，则将此时的点云数据插入到当前正在维护的子图中，同时输出插入的结果，包括时间、位姿、子图、扫描数据等信息

一、各部分代码简介

1. scanmatch

主要是将当前的传感器数据与当前维护的子图进行匹配，寻找一个位姿估计使得传感器数据能够尽可能的与地图匹配上。 这是一个最优化的问题，Cartographer主要通过ceres库求解。

三个参数，分别记录了参考时间、位姿估计器预测的位姿、经过重力修正之后的扫描数据。

  在函数的一开始先获取对象active_submaps_中维护的用于扫描匹配的旧图。

1. 得到ukf预测的位姿和协方差, 相当于滤波器中的预测位姿
2. 在ukf的预测位姿的基础上，通过scanmatch匹配的位姿叫做观测位姿。

下面贴代码：

std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
  const common::Time time, const transform::Rigid2d& pose_prediction, const sensor::PointCloud& filtered_gravity_aligned_point_cloud)
{
  if (active_submaps_.submaps().empty())
  {
    return absl::make_unique<transform::Rigid2d>(pose_prediction);
  }
  std::shared_ptr<const Submap2D> matching_submap =
    active_submaps_.submaps().front();
  // The online correlative scan matcher will refine the initial estimate for
  // the Ceres scan matcher.

// 以输入的位姿估计器预测的位姿作为扫描匹配器迭代的初始位姿
  transform::Rigid2d initial_ceres_pose = pose_prediction;
// 这里pose_prediction 传入位姿预估器的位置，输出 real_time_correlative_scan_matcher_ 计算出的位置 initial_ceres_pose
  if (options_.use_online_correlative_scan_matching())
  {
    const double score = real_time_correlative_scan_matcher_.Match(
      pose_prediction, filtered_gravity_aligned_point_cloud,
      *matching_submap->grid(), &initial_ceres_pose);
    kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);
// LOG(WARNING) << "real_time_correlative_scan_matcher_.Match score is :   " <<  score
// <<  "  pose x :    " <<initial_ceres_pose.translation_(0, 0)
// <<  "  pose y :    " <<initial_ceres_pose.translation_(1, 0);
  }
/*********
 *给定了迭代初值之后，就可以通过对象ceres_scan_matcher_把扫描匹配问题描述成为一个最小二乘的问题，使用优化库ceres提供的工具寻优，
 * 得到一个使观测数据出现的概率最大化的位姿估计。 该估计将被当作机器人的实际位姿参与后序的计算。寻优的结果将保存在对象pose_observation中，
 * 临时对象summary则记录了优化迭代的过程。
 * ***********/
  auto pose_observation = absl::make_unique<transform::Rigid2d>();
  ceres::Solver::Summary summary;
  // 其中 pose_prediction.translation()表示上一个周期的位姿平移
  // initial_ceres_pose为本次优化的初始位姿输入
  // filtered_gravity_aligned_point_cloud 为输入二维点云
  // *matching_submap->grid() 为对应的概率地图
  // pose_observation.get()为输出 ceres_scan_matcher_.Match的位置
  ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
                            filtered_gravity_aligned_point_cloud,
                            *matching_submap->grid(), pose_observation.get(),
                            &summary);
// LOG(WARNING) << "ceres_scan_matcher_.Match is :   "
// <<  "  pose x :    " <<pose_observation->translation_(0, 0)
// <<  "  pose y :    " <<pose_observation->translation_(1, 0);
/**********
 * 如果我们成功的求解了扫描匹配问题，那么就计算一下残差通过判据对象kCeresScanMatcherCostMetric，
 * kScanMatcherResidualDistanceMetric和kScanMatcherResidualAngleMetric检查一下计算结果
 * *******/
  if (pose_observation)
  {
    kCeresScanMatcherCostMetric->Observe(summary.final_cost);
    const double residual_distance =
      (pose_observation->translation() - pose_prediction.translation())
      .norm();
    kScanMatcherResidualDistanceMetric->Observe(residual_distance);
    const double residual_angle =
      std::abs(pose_observation->rotation().angle() -
               pose_prediction.rotation().angle());
    kScanMatcherResidualAngleMetric->Observe(residual_angle);
  }
  return pose_observation;
}

2.AddRangeData

AddRangeData为激光点云输入接口，也是每帧处理的核心流程，包含了结构图中的local slam虚框中的处理过程，即使没有odometer和imu两个接口，local slam依然可以正常运行。其具体处理步骤如下：

多激光传感器数据基于时间戳同步融合

开启位置估计器

根据位置估计器，针对每个点云point的时间戳进行预测位置进行畸变校准

校准后的点云转换成scanmath和map 更新使用的range格式，并将miss和hit分类

获取校准后的origin pose

获取预测的重力加速度方向

根据重力加速度方向投影点云

降采样滤波，并抛弃设置高度范围外的所有点云

然后调用匹配方法

插入并更新submap

获取匹配后的结果内容，包括时间，轨迹节点位置，节点位置对应点云，submap

下面贴代码

std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>  // 添加激光雷达数据
LocalTrajectoryBuilder2D::AddRangeData(
  const std::string& sensor_id, const sensor::TimedPointCloudData& unsynchronized_data)
{
// 进行多个传感器的数据同步    在函数的一开始，先把输入的索引和数据记录在数据收集器range_data_collator_中，同时我们将得到一个做了时间同步的扫描数据
  auto synchronized_data =
    range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);
  if (synchronized_data.ranges.empty())
  {
    LOG(INFO) << "Range data collator filling buffer.";
    return nullptr;
  }
  // 步时间
  const common::Time& time = synchronized_data.time;
  // Initialize extrapolator now if we do not ever use an IMU.
  if (!options_.use_imu_data())
  {
    InitializeExtrapolator(time);  // 构造位姿估计器，不用imu就直接初始化位姿推测器
  }
  // 位置估计器初始化，即第一次或者没有imu数据时应该先初始化位置估计器。由于位置估计器提供匹配的预测位置，因此未完成初始化无需后续的匹配等处理；
  if (extrapolator_ == nullptr)
  {
    // Until we've initialized the extrapolator with our first IMU message, we
    // cannot compute the orientation of the rangefinder.
    LOG(INFO) << "Extrapolator not yet initialized.";
    return nullptr;
  }

  CHECK(!synchronized_data.ranges.empty());
  // TODO(gaschler): Check if this can strictly be 0.
// 检查扫描数据的时间确保最后一个数据的时间偏移量大于等于0
// 然后获取第一个数据点的绝对时间，将它与位姿估计器的时间对比来判定位姿估计器是否完成了初始化操作
  CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f);
  const common::Time time_first_point =
    time +
    common::FromSeconds(synchronized_data.ranges.front().point_time.time);
  if (time_first_point < extrapolator_->GetLastPoseTime())
  {
    LOG(INFO) << "Extrapolator is still initializing.";
    return nullptr;
  }
// 激光的运动补偿
// 开辟一个新的vector存储所有当前雷达传感器点云每个点对应的位置信息，其位置信息由估计器预测而来
  std::vector<transform::Rigid3f> range_data_poses;
  range_data_poses.reserve(synchronized_data.ranges.size());
  bool warned = false;
// 该函数在一个for循环中通过位姿估计器获取各个扫描点对应的机器人位姿。在循环的一开始，先计算测量时间，并与位姿估计器的时间对比。
// 如果时间滞后，在给出了警告信息后，
// 盲目的调整时间，以防止因果倒置的问题。如果一切正常，我们就可以通过对象extrapolator_和测量时间获取对应的位姿。
  for (const auto& range : synchronized_data.ranges)
  {
    common::Time time_point = time + common::FromSeconds(range.point_time.time);
    // 遍历每一个点云点的时间戳，理论上应晚于估计器上刻位置时间戳，否则说明传感器采集时间错误
    if (time_point < extrapolator_->GetLastExtrapolatedTime())
    {
      if (!warned)
      {
        LOG(ERROR)
          << "Timestamp of individual range data point jumps backwards from "
          << extrapolator_->GetLastExtrapolatedTime() << " to " << time_point;
        warned = true;
      }
      time_point = extrapolator_->GetLastExtrapolatedTime();
    }
    // 根据每个点的时间戳估计点云点对应的位置并进行缓存
    range_data_poses.push_back(
      extrapolator_->ExtrapolatePose(time_point).cast<float>());
  }
// 如果历史没有累积数据，就用成员变量accumulation_started_记录下当前的时间，作为跟踪轨迹的开始时刻。
  if (num_accumulated_ == 0)
  {
    // 'accumulated_range_data_.origin' is uninitialized until the last
    // accumulation.
    accumulated_range_data_ = sensor::RangeData {{}, {}, {}};
  }

  // Drop any returns below the minimum range and convert returns beyond the
  // maximum range into misses.
/*******************************
 * 在接下来的for循环中，抛弃掉所有测量距离小于配置min_range的hit点，并把那些超过配置max_range的测量点划归miss点。
 * 在第208和211行中， 通过左乘机器人位姿，将机器人坐标系下的传感器位姿，和激光测量的hit点位姿，转换到局部地图的坐标系下。
 * 然后在第213和214行中计算测量距离， 实际上这个计算有点重复了，因为ROS系统中原始的激光数据就是距离信息，这里来回转换了一遍有点多余。
 * 最后在215到226行的if-else语句中，完成hit点和miss点的筛选工作。 遍历所有点之后，累加计数num_accumulated_。
 * ******************************/
  for (size_t i = 0; i < synchronized_data.ranges.size(); ++i)
  {
    // 提取每一个点云点的pose（包含时间戳）
    const sensor::TimedRangefinderPoint& hit =
      synchronized_data.ranges[i].point_time;
    // 提取此点云对应的原点坐标pose，并进行畸变矫正
    const Eigen::Vector3f origin_in_local =
      range_data_poses[i] *
      synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
    // 对此点进行畸变矫正，并转换为pose，不包含时间戳
    sensor::RangefinderPoint hit_in_local =
      range_data_poses[i] * sensor::ToRangefinderPoint(hit);
    // 计算点到原点距离
    const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;
    const float           range = delta.norm();
    // 距离满足一定范围内保留，否则丢弃
    if (range >= options_.min_range())
    {
      if (range <= options_.max_range())
      {
        accumulated_range_data_.returns.push_back(hit_in_local);
      }
      else
      {
        // 超出设置最远距离，则放入miss队列中，并且距离全部调整为配置值
        hit_in_local.position =
          origin_in_local +
          options_.missing_data_ray_length() / range * delta;
        accumulated_range_data_.misses.push_back(hit_in_local);
      }
    }
  }

// 以上代码即为上节分析过的激光雷达点云数据的畸变矫正过程，其目的是提高点云的准确性。由于估计器可估计出任意时刻的大约位置，而雷达点云第一个采样点到最后一个点必然存在时间间隔，其时间戳已经存储在输入的点云中。故可获取点云中每个点激光原点位置估计值，以此进行修正。
// 在遍历每个点同时，将点云重新进行封装，即真正意义上点云集合，包括原点坐标和点云坐标。同时为后续的submap更新准备，将点云按照到激光原点的距离分成有效点return和miss两类。实际就是限制map更新激光测距有效距离。
// 注意：激光雷达原数据本身就是距离值，直接可以划分return和miss两类。在cartographer_ros转换成了点云，然后再反转成距离判断，显然降低了效率。而其他slam算法如gmapping接口输入直接为distance格式，因此可直接判断；
// 思考和意义：1.由于cartographer算法并非针对某一种传感器、某一类并且个数也不一定，因此cartographer的核心代码采用更加通用的点云格式作为输入。
// 2.因此只有深入了解cartographer内部技术细节，可针对自己构建的机器人进行优化和修改，更加适配自身的传感器类型

  // 激光点云累积个数
  ++num_accumulated_;
// 以上的这些工作无非是将Cartographer系统中原始的激光点云数据转换成占用栅格和插入器需要的RangeData类型的数据。
// 扫描匹配、运动过滤、更新子图的工作都是在下面的代码片段中实现的。

// 每当累积的传感器数据数量超过了配置值num_accumulated_range_data，就会调用函数AddAccumulatedRangeData完成Local SLAM的几项核心任务，
// 并返回记录了子图插入结果的扫描匹配结果。 在调用该函数之前，先将计数清零，并通过位姿估计器获取重力的方向，记录下当前的累积位姿。
// 函数TransformToGravityAlignedFrameAndFilter   主要是以重力方向为参考修正传感器数据后进行体素化滤波
  if (num_accumulated_ >= options_.num_accumulated_range_data())
  {
    // 取最新时间戳
    const common::Time               current_sensor_time = synchronized_data.time;
    absl::optional<common::Duration> sensor_duration;
    if (last_sensor_time_.has_value())
    {
      sensor_duration = current_sensor_time - last_sensor_time_.value();
    }
    last_sensor_time_ = current_sensor_time;
    num_accumulated_  = 0;
    // 获取估计的重力加速度方向，格式为旋转向量
    const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
      extrapolator_->EstimateGravityOrientation(time));
    // TODO(gaschler): This assumes that 'range_data_poses.back()' is at time
    // 'time'.
    // 估计的最后一个点的预测位置作为矫正后的点云的原点坐标
    accumulated_range_data_.origin = range_data_poses.back().translation();
    // 进行重力加速度方向上的投影，同时进行降采样滤波
    return AddAccumulatedRangeData(
      time,
      TransformToGravityAlignedFrameAndFilter(
        gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
        accumulated_range_data_),
      gravity_alignment, sensor_duration);
  }
  return nullptr;
}

3.TransformToGravityAlignedFrameAndFilter

这里根据重力加速度方向进行旋转投影，仅保留一定高度，通过体素滤波。

下面贴代码：

sensor::RangeData LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter(
  const transform::Rigid3f& transform_to_gravity_aligned_frame, const sensor::RangeData& range_data) const
{
  const sensor::RangeData cropped =
    sensor::CropRangeData(sensor::TransformRangeData(
                            range_data, transform_to_gravity_aligned_frame),
                          options_.min_z(), options_.max_z());
  return sensor::RangeData {
           cropped.origin,
           sensor::VoxelFilter(cropped.returns, options_.voxel_filter_size()),
           sensor::VoxelFilter(cropped.misses, options_.voxel_filter_size())
  };
}

由于为2d slam故只需要水平方向的点云，投影后仅保留固定高度范围内的点云，然后采用立体素的方法进行降采样，其基本原理即在一个立方体内仅保留一个点，且为这个立方体内部所有点云的均值坐标，是pcl库中最基本降采样方法；

4.AddAccumulatedRangeData

下面贴代码：

std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
  const common::Time time, const sensor::RangeData& gravity_aligned_range_data, const transform::Rigid3d& gravity_alignment, const absl::optional<common::Duration>& sensor_duration)
{
  if (gravity_aligned_range_data.returns.empty())
  {
    LOG(WARNING) << "Dropped empty horizontal range data.";
    return nullptr;
  }

  // Computes a gravity aligned pose prediction.
// 计算重力修正之后的机器人位姿，然后通过函数ScanMatch进行扫描匹配，并返回新的位姿估计
  const transform::Rigid3d non_gravity_aligned_pose_prediction =
    extrapolator_->ExtrapolatePose(time);
  // 经过重力方向计算投影后的2d位置
  const transform::Rigid2d pose_prediction = transform::Project2D(
    non_gravity_aligned_pose_prediction * gravity_alignment.inverse());
// LOG(WARNING) << " BEFORE ADD POINT CLOUD  POSE X     " <<  pose_prediction.translation_(0, 0)
// << " POSE Y   " <<    pose_prediction.translation_(1, 0);  //这里是直接靠位姿推测器出来的位置
// 经过立体像素滤波获取点云
// 默认size为0.5m， 最小个数200个，最远距离50m
  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;
  }

  // local map frame <- gravity-aligned frame
  // 采用预测位置作为初始位置和滤波后的点云进行相关匹配获得的位置
  std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
    ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);
// LOG(WARNING) << " AFTER ADD POINT CLOUD  POSE X     " <<  pose_estimate_2d->translation_(0, 0)
// << " POSE Y   " <<    pose_estimate_2d->translation_(1, 0);  // 这里是靠scan_match出来的位置
  if (pose_estimate_2d == nullptr)
  {
    LOG(WARNING) << "Scan matching failed.";
    return nullptr;
  }
  // 然后将新的位姿估计反馈给位姿估计器对象extrapolator_。
  // 转换位置类型
  // gravity_alignment 为水平投影系数，假设平地则为1
  const transform::Rigid3d pose_estimate =
    transform::Embed3D(*pose_estimate_2d) * gravity_alignment;
  // 将此刻匹配后的准确位置加入估计值， 即更新估计器
  extrapolator_->AddPose(time, pose_estimate);
// 接着将传感器的数据转换到新的位姿下，通过函数InsertIntoSubmap将新的扫描数据插入到子图中。
  sensor::RangeData range_data_in_local =
    TransformRangeData(gravity_aligned_range_data,
                       transform::Embed3D(pose_estimate_2d->cast<float>()));
  // 根据真实的机器人位置和真实的点云坐标进行submap更新和维护
  std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
    time, range_data_in_local, filtered_gravity_aligned_point_cloud,
    pose_estimate, gravity_alignment.rotation());

  const auto wall_time = std::chrono::steady_clock::now();
  if (last_wall_time_.has_value())
  {
    const auto wall_time_duration = wall_time - last_wall_time_.value();
    kLocalSlamLatencyMetric->Set(common::ToSeconds(wall_time_duration));
    if (sensor_duration.has_value())
    {
      kLocalSlamRealTimeRatio->Set(common::ToSeconds(sensor_duration.value()) /
                                   common::ToSeconds(wall_time_duration));
    }
  }
  const double thread_cpu_time_seconds = common::GetThreadCpuTimeSeconds();
  if (last_thread_cpu_time_seconds_.has_value())
  {
    const double thread_cpu_duration_seconds =
      thread_cpu_time_seconds - last_thread_cpu_time_seconds_.value();
    if (sensor_duration.has_value())
    {
      kLocalSlamCpuRealTimeRatio->Set(
        common::ToSeconds(sensor_duration.value()) /
        thread_cpu_duration_seconds);
    }
  }
  last_wall_time_               = wall_time;
  last_thread_cpu_time_seconds_ = thread_cpu_time_seconds;
  return absl::make_unique<MatchingResult>(
    MatchingResult {time, pose_estimate, std::move(range_data_in_local),
                    std::move(insertion_result)});
}

5.InsertIntoSubmap

完成了扫描匹配之后，就可以调用函数InsertIntoSubmap将传感器数据插入到当前正在维护的子图中。

其中range_data_in_local是在优化之后的位姿估计下观测到的hit点和miss点云数据。而gravity_aligned_range_data中记录的则是扫描匹配之前执行了重力修正的扫描数据。pose_estimate则是优化之后的位姿估计，gravity_alignment描述了重力方向。

下面贴代码：

std::unique_ptr<LocalTrajectoryBuilder2D::InsertionResult>LocalTrajectoryBuilder2D::InsertIntoSubmap(
  const common::Time time, const sensor::RangeData& range_data_in_local, const sensor::PointCloud& filtered_gravity_aligned_point_cloud, const transform::Rigid3d& pose_estimate, const Eigen::Quaterniond& gravity_alignment)
{
// 在函数的一开始，先通过运动滤波器来判定但前是否发生了明显的运动，若未发生则直接返回退出。这样可以减少很多数据量。
  if (motion_filter_.IsSimilar(time, pose_estimate))
  {
    return nullptr;
  }
// 在通知对象active_submaps_接收新的数据更新地图之前，先把其维护的子图暂时保存在临时的容器insertion_submaps中。
  std::vector<std::shared_ptr<const Submap2D> > insertion_submaps =
    active_submaps_.InsertRangeData(range_data_in_local);
// 构建一个InsertionResult对象并返回
// 调用submap封装类， 执行插入新的激光数据， 即submap更新
// 返回的是更新和插入后的submap2d
  return absl::make_unique<InsertionResult>(InsertionResult {
        std::make_shared<const TrajectoryNode::Data>(TrajectoryNode::Data {
          time,
          gravity_alignment,
          filtered_gravity_aligned_point_cloud,
          {},  // 'high_resolution_point_cloud' is only used in 3D.
          {},  // 'low_resolution_point_cloud' is only used in 3D.
          {},  // 'rotational_scan_matcher_histogram' is only used in 3D.
          pose_estimate
        }),
        std::move(insertion_submaps)
      });
}

每一帧激光点云均会经过帧间匹配获取准确的机器人位置，则对应的点云可插入submap中，但是基于存储和建图的本身的需求，无需如此多的数据，会产生大量重复点云。因此机器人进行运动滤波，即只有当机器人运行了一定距离或一定角度时，则作为keypose进行存储，即进行后续的插入操作。

其中motion_filter_.IsSimilar(time, pose_estimate)即判断此次与上次之间的位置偏移和角度偏移是否达到一定阈值，否则无需进行插入处理。满足条件后直接调用ActiveSubmaps2D更新函数即可。

二、总结

LocalTrajectoryBuilder2D实际上就是前端处理的整体流程，通过对所有输入的传感器数据进行预处理。将laser的数据经过一定预处理形成单一的，稀疏的点云，进行帧间匹配获取真实位置，即完成SLAM中的定位。然后将laser点云分成return和miss两类，已知点云和位置进行更新submap。

换言之，LocalTrajectoryBuilder2D是一个没有闭环处理的完整的slam过程，可修改submap的帧的阈值（无限大），则完成了与开源hector slam算法一致的功能。