cartographer源码解读——2D前端（LocalTrajectoryBuilder2D）

cartographer源码解读——2D前端（LocalTrajectoryBuilder2D）

前端的主要功能

1. 点云数据处理：滤波、畸变校正
2. 位姿外推器推算位姿
3. 暴力匹配、CSM匹配
4. 生成submap
5. 生成Node传入后端

sensors数据通过在cartographer_ros中获取并传入cartographer中，由GlobalTrajectoryBuilder.cc中的AddSensorData（）函数，将sensor数据传入前后端，首先考虑前端

本节将讲述LocalTrajectoryBuilder2D中主要的函数功能和接口。

Local_Trajectory_Builder类

class LocalTrajectoryBuilder2D {
 public:
  struct InsertionResult {
    // Node数据
    std::shared_ptr<const TrajectoryNode::Data> constant_data;
    // 该Node数据插入的两个submap
    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;
  };
  struct MatchingResult {
    common::Time time;
    // tracking 坐标系下的位姿(Local SLAM坐标系下的位姿), 
    // 需要乘以tracking frame in map的转换(Local SLAM坐标系到Global SLAM坐标系的转换)才能转到全局坐标系
    transform::Rigid3d local_pose;
    // tracking 坐标系下的点云（即Local SLAM坐标系下的位姿）
    sensor::RangeData range_data_in_local;
    // 'nullptr' if dropped by the motion filter.
    std::unique_ptr<const InsertionResult> insertion_result;
  };

  explicit LocalTrajectoryBuilder2D(
      const proto::LocalTrajectoryBuilderOptions2D& options,
      const std::vector<std::string>& expected_range_sensor_ids);
  ~LocalTrajectoryBuilder2D();

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

  // Returns 'MatchingResult' when range data accumulation completed,
  // otherwise 'nullptr'. Range data must be approximately horizontal
  // for 2D SLAM. `TimedPointCloudData::time` is when the last point in
  // `range_data` was acquired, `TimedPointCloudData::ranges` contains the
  // relative time of point with respect to `TimedPointCloudData::time`.
  // 添加sensor数据 
  std::unique_ptr<MatchingResult> AddRangeData(
      const std::string& sensor_id,
      const sensor::TimedPointCloudData& range_data);
  void AddImuData(const sensor::ImuData& imu_data);
  void AddOdometryData(const sensor::OdometryData& odometry_data);

  static void RegisterMetrics(metrics::FamilyFactory* family_factory);

 private:
  std::unique_ptr<MatchingResult> AddAccumulatedRangeData(
      common::Time time, const sensor::RangeData& gravity_aligned_range_data,
      const transform::Rigid3d& gravity_alignment,
      const absl::optional<common::Duration>& sensor_duration);
  sensor::RangeData TransformToGravityAlignedFrameAndFilter(
      const transform::Rigid3f& transform_to_gravity_aligned_frame,
      const sensor::RangeData& range_data) const;
  std::unique_ptr<InsertionResult> InsertIntoSubmap(
      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);

  // Scan matches 'filtered_gravity_aligned_point_cloud' and returns the
  // observed pose, or nullptr on failure.
  std::unique_ptr<transform::Rigid2d> ScanMatch(
      common::Time time, const transform::Rigid2d& pose_prediction,
      const sensor::PointCloud& filtered_gravity_aligned_point_cloud);

  // Lazily constructs a PoseExtrapolator.
  void InitializeExtrapolator(common::Time time);

  const proto::LocalTrajectoryBuilderOptions2D options_;
  // 存放2个最新的submap，执行插入点云，生成submap，和匹配的类
  ActiveSubmaps2D active_submaps_;
  // 稀疏滤波，防止重复的激光帧插入active_submaps_
  MotionFilter motion_filter_;
  // online scan matcher接口（暴力匹配）
  scan_matching::RealTimeCorrelativeScanMatcher2D
      real_time_correlative_scan_matcher_;
  // CSM接口
  scan_matching::CeresScanMatcher2D ceres_scan_matcher_;
  // 位姿外推器
  std::unique_ptr<PoseExtrapolator> extrapolator_;
  // 几帧数据合成一帧，一般设置为1，因为只用一个激光雷达
  int num_accumulated_ = 0;
  // 临时变量，几帧激光合成的数据
  sensor::RangeData accumulated_range_data_;

  absl::optional<std::chrono::steady_clock::time_point> last_wall_time_;
  absl::optional<double> last_thread_cpu_time_seconds_;
  absl::optional<common::Time> last_sensor_time_;
  // 滤波的类
  RangeDataCollator range_data_collator_;
};

轮速计处理函数

void LocalTrajectoryBuilder3D::AddOdometryData(
    const sensor::OdometryData& odometry_data) {
  if (extrapolator_ == nullptr) {
    // Until we've initialized the extrapolator we cannot add odometry data.
    LOG(INFO) << "Extrapolator not yet initialized.";
    return;
  }
  // 位姿外推器接受轮速计数据
  extrapolator_->AddOdometryData(odometry_data);
}

IMU数据处理

void LocalTrajectoryBuilder2D::AddImuData(const sensor::ImuData& imu_data) {
  CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added.";
  // 检查是否有位姿外推器生成？若有，则添加IMU数据
  InitializeExtrapolator(imu_data.time);
  // 位姿外推器接受IMU数据
  extrapolator_->AddImuData(imu_data);
}

激光点云数据处理

根据时间滤波重复数据

使用位姿外推器进行畸变校正

点云数据重力对齐

点云体素滤波

// 添加激光数据
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(
    const std::string& sensor_id,
    const sensor::TimedPointCloudData& unsynchronized_data) {
  // 数据同步，什么情况下才会同步？第一：两个激光雷达的话题名字重了（找出时间上重复的，保留不重复的地方）；第二：多个雷达建图，scan1，scan2，变成scan_merged
  // 使用range_data_collator_来同步，查看其类型为RangeDataCollator，
  // 用此来AddRangeData,查看进入range_data_collator.cc
  // 同步数据的检查，查看range_data_collator_.AddRangeData的返回值的格式（同步后的激光数据格式），进入range_data_collator.cc函数
  // 而其接受的数据是未同步的激光数据格式是sensor::TimedPointCloudData，查看其定义，进入timed_point_cloud_data.h
  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.如果不使用IMU的话，初始化位姿外推器
  // 为什么不适用IMU的时候才尝试创建位姿外推器？因为添加IMU数据的时候，最先生成了外推器
  if (!options_.use_imu_data()) {
    // 进入InitializeExtrapolator（），查看，主要做了：如果位姿外推器不是空的，就返回
    // 尝试用激光数据建立外推器
    InitializeExtrapolator(time);
  }
  // 如果位姿外推器还是空的，说明没有创建成功，没有初始化成功，则不进行位姿推算
  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);
  // 第一个点的绝对时间戳就等于time(激光帧的时间戳)加上第一个点的相对时间戳
  const common::Time time_first_point =
      time +
      common::FromSeconds(synchronized_data.ranges.front().point_time.time);
  // 如果第一个点的时间比外推器最新的位姿还要早，说明将要预估的位姿已经存在了，所以不对这些数据做处理
  // 查看该函数GetLastPoseTime（）定义
  if (time_first_point < extrapolator_->GetLastPoseTime()) {
    LOG(INFO) << "Extrapolator is still initializing.";
    return nullptr;
  }

  // range_data_poses激光雷达发射原点所处的pose
  std::vector<transform::Rigid3f> range_data_poses;
  // 预留空间
  range_data_poses.reserve(synchronized_data.ranges.size());
  bool warned = false;
  // for循环遍历每个点激光雷达所处的pose
  for (const auto& range : synchronized_data.ranges) {
    // 计算每个点的绝对时间戳=激光帧的绝对时间戳+每个点的相对时间戳
    common::Time time_point = time + common::FromSeconds(range.point_time.time);
    // 当前点的时间比上一次外推器记录的最后一个点的时间还要早，状态警告，让time_point直接复制上一次外推器记录的最后一个点的时间
    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数据（外推器预估一个激光雷达的pose，使用当前点的绝对时间戳）
    // 点击查看外推器ExtrapolatePose的定义，进入pose_extrapolator.cc中
    range_data_poses.push_back(
        extrapolator_->ExtrapolatePose(time_point).cast<float>());
  }

  // 累积的激光帧数（cartographer支持好几帧数据累积到一块儿和地图匹配）
  if (num_accumulated_ == 0) {
    // 'accumulated_range_data_.origin' is uninitialized until the last
    // accumulation.
    // 如果进去是0的话，就初始化一下成sensor::RangeData此类型
    accumulated_range_data_ = sensor::RangeData{{}, {}, {}};
  }

  // Drop any returns below the minimum range and convert returns beyond the
  // maximum range into misses.
  // 此for循环是做激光畸变矫正的核心
  for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
    // hit是数据同步后的格式（查看point_time的类型，发现其包含点的坐标（sensor坐标系下的）和时间戳）
    const sensor::TimedRangefinderPoint& hit =
        synchronized_data.ranges[i].point_time;
    // 激光雷达（sensor）原点在Local下的pose = range_data_poses[i]（每个点预估的pose） * 每个激光雷达的origin_index（点击查看）
    const Eigen::Vector3f origin_in_local =
        range_data_poses[i] *
        synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
    // range_data_poses[i](给每个点的pose上外推估计了一个pose) * ToRangefinderPoint函数的作用是拿出hit的坐标 
    // 就是将sensor坐标系下的数据转换到Local坐标系下 
    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()) {
        // 经过畸变校正后，超过测距范围的点，认为是miss点
        accumulated_range_data_.returns.push_back(hit_in_local);
      } else {
        hit_in_local.position =
            origin_in_local +
            options_.missing_data_ray_length() / range * delta;
        accumulated_range_data_.misses.push_back(hit_in_local);
      }
    }
  }
  ++num_accumulated_;

  // 激光数据帧收集够了
  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 = 当前传感器的绝对时间 - 上一次记录的传感器的时间
      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'.
    // 多个激光雷达要合并，要给出一个origin = 最后一个点的发射点的pose
    accumulated_range_data_.origin = range_data_poses.back().translation();
    // 将Local坐标系下的点云转换到sensor下（一帧数据的最后一个点），并乘以重力方向，做重力对齐，然后CSM，
    // 计算最新点的位姿，机器人此时在LocalSLAM的坐标
    return AddAccumulatedRangeData(
        time,
        // 重力对齐，并进行体素滤波
        TransformToGravityAlignedFrameAndFilter(
            gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
            accumulated_range_data_),
        gravity_alignment, sensor_duration);
  }
  return nullptr;
}

体素滤波接口

// 点云体素滤波接口
sensor::RangeData
LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter(
    const transform::Rigid3f& transform_to_gravity_aligned_frame,
    const sensor::RangeData& range_data) const {
  // 按照Z方向阈值，剔除点
  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())};
}

没有IMU的位姿外推器初始化

void LocalTrajectoryBuilder2D::AddImuData(const sensor::ImuData& imu_data) {
  CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added.";
  // 检查是否有位姿外推器生成？若有，则添加IMU数据
  InitializeExtrapolator(imu_data.time);
  // 位姿外推器接受IMU数据
  extrapolator_->AddImuData(imu_data);
}

// 没有IMU的位姿外推器初始化
void LocalTrajectoryBuilder2D::InitializeExtrapolator(const common::Time time) {
  // 如果extrapolator_位姿外推器不是空的，就返回
  if (extrapolator_ != nullptr) {
    return;
  }
  // 使用IMU数据生成位姿外推器extrapolator_
  CHECK(!options_.pose_extrapolator_options().use_imu_based());
  // TODO(gaschler): Consider using InitializeWithImu as 3D does.
  extrapolator_ = absl::make_unique<PoseExtrapolator>(
      ::cartographer::common::FromSeconds(options_.pose_extrapolator_options()
                                              .constant_velocity()
                                              .pose_queue_duration()),
      options_.pose_extrapolator_options()
          .constant_velocity()
          .imu_gravity_time_constant());
  extrapolator_->AddPose(time, transform::Rigid3d::Identity());
}

位姿外推器推算当前时刻的机器人位姿；
点云自适应滤波，并进行暴力匹配和CSM，计算点云在LocalSLAM坐标系下的位姿；
更新位姿外推器；
将LocalSLAM坐标系下的点云插入active_submap；
生成Node传入后端。

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.
  // 位姿外推器计算当前时刻Local_slam 的位姿
  const transform::Rigid3d non_gravity_aligned_pose_prediction =
      extrapolator_->ExtrapolatePose(time);
  // 因为在CSM内，需要做T*p和submap做点云匹配，而p（点云数据）是经过重力对齐的，所有T（位姿）需要减去重力方向。
  const transform::Rigid2d pose_prediction = transform::Project2D(
      non_gravity_aligned_pose_prediction * gravity_alignment.inverse());

  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　　CSM计算LocalSLAM位姿
  std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
      ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);
  if (pose_estimate_2d == nullptr) {
    LOG(WARNING) << "Scan matching failed.";
    return nullptr;
  }
  // 计算的位姿需要再乘以重力方向才是LocalSLAM的位姿
  const transform::Rigid3d pose_estimate =
      transform::Embed3D(*pose_estimate_2d) * gravity_alignment;
  // 将当前CSM估计的LocalSLAM的位姿放入位姿外推器中
  extrapolator_->AddPose(time, pose_estimate);
　// 计算在LocalSLAM坐标系下，转换后的点云
  sensor::RangeData range_data_in_local =
      TransformRangeData(gravity_aligned_range_data,
                         transform::Embed3D(pose_estimate_2d->cast<float>()));
  // 将LocalSLAM坐标系下的点插入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)});
}

Scan match接口

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);
  }
  //　获取active_submap的第一个submap作为匹配的submap
  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;

  // 根据参数设置，是否使用real_time_correlative_scan_matcher做暴力匹配，得到更好的初始位姿
  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);
  }

  // 做CSM，计算点云位姿
  auto pose_observation = absl::make_unique<transform::Rigid2d>();
  ceres::Solver::Summary summary;
  ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
                            filtered_gravity_aligned_point_cloud,
                            *matching_submap->grid(), pose_observation.get(),
                            &summary);
  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;
}

Submap接口

稀疏滤波；
插入数据；
生成InsertionResult数据。

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;
  }
  // 插入数据
  std::vector<std::shared_ptr<const Submap2D>> insertion_submaps =
      active_submaps_.InsertRangeData(range_data_in_local);
  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)});
}

参考链接: https://blog.csdn.net/yeluohanchan/article/details/108674497.