cartographer之MotionFilter

        MotionFilter是cartographer中用来判断是否对子图进行更新的一个类，具体应用在local_trajectory_build_2d和local_trajectory_build_2d中。首先这里对MotionFilter的使用场景进行推导，例如，在local_trajectory_build_2d中，AddRangeData函数调用AddAccumulatedRangeData函数来扫描匹配并插入扫描数据，AddAccumulatedRangeData函数在扫描匹配完成后，会调用InsertIntoSubmap函数将扫描数据插入子图，而InsertIntoSubmap函数第一步就会调用motion_filter_.IsSimilar(time, pose_estimate)来判断当前扫描是否能插入子图中。

    在motion_filter.h中完成了对MotionFilter类的声明，MotionFilter的结构如下：

// Takes poses as input and filters them to get fewer poses.
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;
  int num_different_ = 0;
  common::Time last_time_;
  transform::Rigid3d last_pose_;
};

其中MotionFilter构造函数完成对参数的配置，IsSimilar来判断当前位姿与上一时刻更新时的位姿是否有足够的空间和时间距离，options_是MotionFilter相关的参数配置，num_total_是当前轨迹中进入判断的所有扫描数，num_different_ 是参与更新的扫描数，last_time_是上一次更新的时间，last_pose_是上一次更新的位姿。

    在motion_filter.cc中完成了对MotionFilter类中函数的定义，主要是IsSimilar函数：

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_time_ = time;
  last_pose_ = pose;
  ++num_different_;
  return false;
}

    函数首先对num_total_进行了累加，表明这是一个记录所有的进入更新函数的扫描数的总数的变量。然后对当前位姿是否可更新进行了判断，首先判断num_total_是否大于1，因为num_total_等于1时表明地图还没有更新过，那么当前扫描当然是要对地图进行更新了;其次是判断当前时间与上一次更新时间之间的时间差，cartographer默认是每隔一段时间就要对子图进行强制更新的，这与hectorSLAM不同，如果你在使用cartographer中觉得这个功能不好，可以自行隐藏这一段代码；最后就是对当前位姿与上一更新时刻的位姿进行比较，当位姿差满足要求是，也是要进行更新的。参数的设置要根据机器人本体运行的速度和雷达的性能来的，大部分情况下，默认参数都能很好的工作。