slam建图与定位_cartographer代码阅读(5)_前端运动滤波

1.代码走向
1.1扫描匹配对先验位姿优化后将点云按照这个local坐标系下的位姿去校正点云，然后将点云插入submap，这个函数里执行了运动滤波
1.2local_trajectory_builder_2d.cc

/**
 * @brief 将处理后的雷达数据写入submap
 * 
 * @param[in] time 点云的时间
 * @param[in] range_data_in_local 校正后的点云
 * @param[in] filtered_gravity_aligned_point_cloud 自适应体素滤波后的点云
 * @param[in] pose_estimate 扫描匹配后的三维位姿
 * @param[in] gravity_alignment 
 * @return std::unique_ptr<LocalTrajectoryBuilder2D::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;
  }
  // 将点云数据写入到submap中
  std::vector<std::shared_ptr<const Submap2D>> insertion_submaps =
      active_submaps_.InsertRangeData(range_data_in_local);

  // 生成InsertionResult格式的数据进行返回
  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)});
}

1.3motion_filter.cc

/**
 * @brief 将当前时间与当前位姿 与 上一次保存的时间与位姿进行比对, 
 * 时间,移动距离,角度 变换量大于阈值 时返回true
 * 
 * @param[in] time 当前的时间
 * @param[in] pose 当前的位姿
 * @return true 两个位姿的时间与距离很接近
 * @return false 两个位姿的时间或距离不接近
 */
bool MotionFilter::IsSimilar(const common::Time time,
                             const transform::Rigid3d& pose) {
  LOG_IF_EVERY_N(INFO, num_total_ >= 500, 500)
      << "Motion filter reduced the number of nodes to "
      << 100. * num_different_ / num_total_ << "%.";
  ++num_total_;
  if (num_total_ > 1 &&
      time - last_time_ <= common::FromSeconds(options_.max_time_seconds()) &&
      (pose.translation() - last_pose_.translation()).norm() <=
          options_.max_distance_meters() &&
      transform::GetAngle(pose.inverse() * last_pose_) <=
          options_.max_angle_radians()) {
    return true;
  }
  // 只有时间,移动距离,角度 变换量大于阈值 才进行 last_pose_ 的更新
  last_time_ = time;
  last_pose_ = pose;
  ++num_different_;
  return false;
}

2.输入是当前的时间和当前的位姿输出是（是否时间差角度增量位置增量大于参数中设置的数值的一个判断的返回值）
目的过滤调不满足运动条件增量的scan插入submap 如果旋转时定位或地图有飘移可以试试这个修改这个角度增量参数

slam建图与定位_cartographer代码阅读(5)_前端运动滤波

猜你喜欢