Cartographer源码阅读3D-前端数据处理-IMU&3D点云

Cartographer源码阅读3D-前端数据处理-IMU&3D点云

3D需要传入的数据包括：IMU（必须），点云数据(sensor_msgs::MultiEchoLaserScan或sensor_msgs::PointCloud2)，轮速计数据(可选，nav_msgs::Odometry，和2D相同，不再分析)

IMU数据的处理

通过接收sensor_msgs::Imu数据，转换为sensor::ImuData数据：

struct ImuData {
  // 时间
  common::Time time;
  // 线加速度
  Eigen::Vector3d linear_acceleration;
  // 角速度
  Eigen::Vector3d angular_velocity;
};

由于原始数据是IMU的线加速度和角速度，而实际上要转换到tracking坐标系下，因此，将线加速度和角速度都左乘一个sensor_to_tracking.rotation()。

IMU数据流向：

（1）通过数据分发器，分发到前端和后端；

（2）在Node.cc中，传入Node中的pose_extrapolators（位姿外推器）中，为位姿发布插值使用。

3D激光点云数据处理

接收sensor_msgs::MultiEchoLaserScan数据，转换为carto::sensor::TimedPointCloudData类型，传入前端。和2D激光数据的处理一致。数据流向前端。

点云数据处理

接收sensor_msgs::PointCloud2数据，转换为sensor::TimedPointCloudData数据，其中，每个点的时间戳赋值为0，即认为所有的点之间没有运动畸变。数据流向为前端。

关于IMU初始化位姿外推器

3D的前端必须要有IMU，从原理看，需要获取重力方向，以做切片等处理。从代码看，没有IMU数据，前端无法进行。接收到点云数据时，在LocalTrajectoryBuilder3D::AddRangeData入口函数里：

// 如果位姿外推器没有被初始化，则一直不往下进行
if (extrapolator_ == nullptr) {
    // Until we've initialized the extrapolator with our first IMU message, we
    // cannot compute the orientation of the rangefinder.
    LOG(INFO) << "IMU not yet initialized.";
    return nullptr;
  }

在接收到轮速计数据时，在LocalTrajectoryBuilder3D::AddOdometryData函数里，仍然不会进行位姿外推器的初始化：

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;
  }
  // 只有在位姿外推器被IMU初始化后，才能再加入轮速计数据
  extrapolator_->AddOdometryData(odometry_data);
}

在接收到IMU数据时，在LocalTrajectoryBuilder3D::AddImuData函数里，如果位姿外推器没有初始化，则使用IMU数据初始化。

void LocalTrajectoryBuilder3D::AddImuData(const sensor::ImuData& imu_data) {
  // 如果初始化过，则直接加入数据
  if (extrapolator_ != nullptr) {
    extrapolator_->AddImuData(imu_data);
    return;
  }
  // We derive velocities from poses which are at least 1 ms apart for numerical
  // stability. Usually poses known to the extrapolator will be further apart
  // in time and thus the last two are used.
  constexpr double kExtrapolationEstimationTimeSec = 0.001;
  // IMU初始化位姿外推器
  extrapolator_ = mapping::PoseExtrapolator::InitializeWithImu(
      ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
      options_.imu_gravity_time_constant(), imu_data);
}

3D前端点云处理

在LocalTrajectoryBuilder3D::AddRangeData函数里：

std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>
LocalTrajectoryBuilder3D::AddRangeData(
    const std::string& sensor_id,
    const sensor::TimedPointCloudData& unsynchronized_data) {
  // 基于时间戳滤波，剔除和上一帧数据重复的点
  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;
  // 必须在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) << "IMU not yet initialized.";
    return nullptr;
  }

  CHECK(!synchronized_data.ranges.empty());
  CHECK_LE(synchronized_data.ranges.back().point_time[3], 0.f);
  // 计算第一个点的时间戳
  const common::Time time_first_point =
      time +
      common::FromSeconds(synchronized_data.ranges.front().point_time[3]);
  // 需要保证第一个点的时间戳大于外推器最新位姿的时间戳
  if (time_first_point < extrapolator_->GetLastPoseTime()) {
    LOG(INFO) << "Extrapolator is still initializing.";
    return nullptr;
  }

  if (num_accumulated_ == 0) {
    accumulation_started_ = std::chrono::steady_clock::now();
  }
  // 对点进行体素滤波，以减小计算量
  std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> hits =
      sensor::VoxelFilter(0.5f * options_.voxel_filter_size())
          .Filter(synchronized_data.ranges);
  // 存放每个hit点的pose,做畸变校正
  std::vector<transform::Rigid3f> hits_poses;
  hits_poses.reserve(hits.size());
  bool warned = false;
  for (const auto& hit : hits) {
    // 计算hit点的时间戳
    common::Time time_point = time + common::FromSeconds(hit.point_time[3]);
    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;
      }
      // 如果该hit点的时间戳小于上一帧的时间戳，则把当前的时间戳修改，并在后面赋值时，该点的位姿赋值为位姿外推器里推算的最新位姿
      time_point = extrapolator_->GetLastExtrapolatedTime();
    }
    // 计算hit点时间戳的位姿
    hits_poses.push_back(
        extrapolator_->ExtrapolatePose(time_point).cast<float>());
  }

  if (num_accumulated_ == 0) {
    // 'accumulated_range_data_.origin' is not used.
    accumulated_range_data_ = sensor::RangeData{{}, {}, {}};
  }

  for (size_t i = 0; i < hits.size(); ++i) {
    // 计算Local SLAM坐标系下的hit点坐标
    const Eigen::Vector3f hit_in_local =
        hits_poses[i] * hits[i].point_time.head<3>();
    // 计算Local SLAM坐标系下的origion位置
    const Eigen::Vector3f origin_in_local =
        hits_poses[i] * synchronized_data.origins.at(hits[i].origin_index);
    // 计算转换后的hit点和origion位置点之间的距离
    const Eigen::Vector3f delta = hit_in_local - origin_in_local;
    const float range = delta.norm();
    if (range >= options_.min_range()) {
      if (range <= options_.max_range()) {
        // 如果距离值满足要求，则加入hit点，更新hit值
        accumulated_range_data_.returns.push_back(hit_in_local);
      } else {
        // We insert a ray cropped to 'max_range' as a miss for hits beyond the
        // maximum range. This way the free space up to the maximum range will
        // be updated.
        // 如果距离值不满足要求，则加入miss点，更新miss值
        accumulated_range_data_.misses.push_back(
            origin_in_local + options_.max_range() / range * delta);
      }
    }
  }
  ++num_accumulated_;

  if (num_accumulated_ >= options_.num_accumulated_range_data()) {
    num_accumulated_ = 0;
    // 计算最新点的位姿
    transform::Rigid3f current_pose =
        extrapolator_->ExtrapolatePose(time).cast<float>();
    // 再次进行hit点及miss点的体素滤波
    const sensor::RangeData filtered_range_data = {
        current_pose.translation(),
        sensor::VoxelFilter(options_.voxel_filter_size())
            .Filter(accumulated_range_data_.returns),
        sensor::VoxelFilter(options_.voxel_filter_size())
            .Filter(accumulated_range_data_.misses)};
    // 将滤波后的点乘以最新点的位姿的逆，转换到最新点的坐标系下，这些点是非重力对齐的，和2D有所差别
    return AddAccumulatedRangeData(
        time, sensor::TransformRangeData(filtered_range_data,
                                         current_pose.inverse()));
  }
  return nullptr;
}

std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>
LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
    const common::Time time,
    const sensor::RangeData& filtered_range_data_in_tracking) {
  // 如果hit点为空，跳过该帧
  if (filtered_range_data_in_tracking.returns.empty()) {
    LOG(WARNING) << "Dropped empty range data.";
    return nullptr;
  }
  // 推算当前位姿
  const transform::Rigid3d pose_prediction =
      extrapolator_->ExtrapolatePose(time);
  // 取出待匹配的submap，为active submap的第一个submap
  std::shared_ptr<const mapping::Submap3D> matching_submap =
      active_submaps_.submaps().front();
  // 传进去的初始位姿为matching_submap->local_pose().inverse()*pose_prediction，
  // CSM计算出来的结果为pose_estimate，需要再左乘matching_submap->local_pose().inverse()，得到该帧点云在Local SLAM坐标系下的位姿
  // 为什么不直接用pose_prediction？猜测，可能是因为，在后端matching_submap->local_pose()可能会被优化，为保证优化不带给匹配问题，保证点云匹配正确性。
  transform::Rigid3d initial_ceres_pose =
      matching_submap->local_pose().inverse() * pose_prediction;
  // 高分辨率点自适应滤波，生成高分辨率点云数据
  sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
      options_.high_resolution_adaptive_voxel_filter_options());
  const sensor::PointCloud high_resolution_point_cloud_in_tracking =
      adaptive_voxel_filter.Filter(filtered_range_data_in_tracking.returns);
  if (high_resolution_point_cloud_in_tracking.empty()) {
    LOG(WARNING) << "Dropped empty high resolution point cloud data.";
    return nullptr;
  }
  if (options_.use_online_correlative_scan_matching()) {
    // We take a copy since we use 'initial_ceres_pose' as an output argument.
    // 基于搜索框，更新初始位姿
    const transform::Rigid3d initial_pose = initial_ceres_pose;
    double score = real_time_correlative_scan_matcher_->Match(
        initial_pose, high_resolution_point_cloud_in_tracking,
        matching_submap->high_resolution_hybrid_grid(), &initial_ceres_pose);
    kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);
  }

  transform::Rigid3d pose_observation_in_submap;
  ceres::Solver::Summary summary;
  // 低分辨率点云自适应滤波，生成低分辨率点云数据
  sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter(
      options_.low_resolution_adaptive_voxel_filter_options());
  const sensor::PointCloud low_resolution_point_cloud_in_tracking =
      low_resolution_adaptive_voxel_filter.Filter(
          filtered_range_data_in_tracking.returns);
  if (low_resolution_point_cloud_in_tracking.empty()) {
    LOG(WARNING) << "Dropped empty low resolution point cloud data.";
    return nullptr;
  }
  // 高低分辨率点云都进行CSM匹配，计算得到位姿
  ceres_scan_matcher_->Match(
      (matching_submap->local_pose().inverse() * pose_prediction).translation(),
      initial_ceres_pose,
      {{&high_resolution_point_cloud_in_tracking,
        &matching_submap->high_resolution_hybrid_grid()},
       {&low_resolution_point_cloud_in_tracking,
        &matching_submap->low_resolution_hybrid_grid()}},
      &pose_observation_in_submap, &summary);
  kCeresScanMatcherCostMetric->Observe(summary.final_cost);
  double residual_distance = (pose_observation_in_submap.translation() -
                              initial_ceres_pose.translation())
                                 .norm();
  kScanMatcherResidualDistanceMetric->Observe(residual_distance);
  double residual_angle = pose_observation_in_submap.rotation().angularDistance(
      initial_ceres_pose.rotation());
  kScanMatcherResidualAngleMetric->Observe(residual_angle);
  // 计算Local SLAM坐标系下的位姿
  const transform::Rigid3d pose_estimate =
      matching_submap->local_pose() * pose_observation_in_submap;
  // 更新位姿外推器
  extrapolator_->AddPose(time, pose_estimate);
  const Eigen::Quaterniond gravity_alignment =
      extrapolator_->EstimateGravityOrientation(time);
  // 将点云数据转换到Local SLAM坐标系下
  sensor::RangeData filtered_range_data_in_local = sensor::TransformRangeData(
      filtered_range_data_in_tracking, pose_estimate.cast<float>());
  // 将在Local SLAM坐标系下的点插入submap,并计算直方图
  std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
      time, filtered_range_data_in_local, filtered_range_data_in_tracking,
      high_resolution_point_cloud_in_tracking,
      low_resolution_point_cloud_in_tracking, pose_estimate, gravity_alignment);
  auto duration = std::chrono::steady_clock::now() - accumulation_started_;
  kLocalSlamLatencyMetric->Set(
      std::chrono::duration_cast<std::chrono::seconds>(duration).count());
  return common::make_unique<MatchingResult>(MatchingResult{
      time, pose_estimate, std::move(filtered_range_data_in_local),
      std::move(insertion_result)});
}