Cartographer源码阅读2D-前端Node稀疏化-MotionFilter

Cartographer源码阅读-2D前端Node稀疏化-MotionFilter

在trajectory_builder_2d.cc的LocalTrajectoryBuilder2D::AddAccumulatedRangeData函数中，执行稀疏化任务，即，在AddAccumulatedRangeData函数中调用InsertIntoSubmap函数，在InsertIntoSubmap函数内调用(motion_filter_.IsSimilar(time, pose_estimate)函数，进行稀疏化，如果被判断和上一帧的时间及位姿变化较小，则认为该帧不是Node，不放入后端。

看一下MotionFilter类：

class MotionFilter {
 public:
   // 构造函数，传入阈值
  explicit MotionFilter(const proto::MotionFilterOptions& options);

  // If the accumulated motion (linear, rotational, or time) is above the
  // threshold, returns false. Otherwise the relative motion is accumulated and
  // true is returned.
  bool IsSimilar(common::Time time, const transform::Rigid3d& pose);

 private:
  const proto::MotionFilterOptions options_;
  // 总共的激光帧数
  int num_total_ = 0;
  // Node数量
  int num_different_ = 0;
  common::Time last_time_;
  transform::Rigid3d last_pose_;
};
bool MotionFilter::IsSimilar(const common::Time time,
                             const transform::Rigid3d& pose) {
  // 每隔500帧激光数据输出一次Node所占的比重
  LOG_IF_EVERY_N(INFO, num_total_ >= 500, 500)
      << "Motion filter reduced the number of nodes to "
      << 100. * num_different_ / num_total_ << "%.";
  ++num_total_;
  // 进行滤波的条件：激光帧数量大于20，当前激光帧的时间和上一个Node的时间的时间差小于时间阈值，当前激光帧的位姿和上一个Node之间的距离和角度都小于阈值，则认为是相同帧
  if (num_total_ > 20 &&
       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_time_ = time;
  last_pose_ = pose;
  ++num_different_;
  return false;
}