cartographer/mapping/internal/motion_filter.h

Overview

MotionFilter takes poses as input and filters them to produce fewer poses. It decides whether a new scan-matched pose is similar enough to the last accepted pose that inserting it would be redundant. This keeps the number of nodes and submap updates manageable and reduces computation.

Class: cartographer::mapping::MotionFilter

Construction

explicit MotionFilter(const proto::MotionFilterOptions& options);

IsSimilar

// 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);

The logic:

++num_total_;
if (num_total_ > 1 &&
    time - last_time_ <= FromSeconds(options_.max_time_seconds()) &&
    (pose.translation() - last_pose_.translation()).norm() <=
        options_.max_distance_meters() &&
    GetAngle(pose.inverse() * last_pose_) <= options_.max_angle_radians()) {
  return true;   // Too similar to the last accepted pose: drop this one.
}
last_time_ = time;
last_pose_ = pose;
++num_different_;
return false;    // Different enough: accept and remember this pose.

A pose is considered similar (and therefore dropped) only when all three thresholds are satisfied simultaneously: the time elapsed since the last accepted pose is within max_time_seconds, the translational distance is within max_distance_meters, and the relative rotation angle is within max_angle_radians. As soon as one criterion is exceeded the pose is accepted and becomes the new reference.

Every 500 total poses the filter logs the reduction ratio (num_different / num_total).

Options

proto::MotionFilterOptions:

Field

Type

Meaning

max_time_seconds

double

Maximum elapsed time below which a pose can be considered similar

max_distance_meters

double

Maximum translation below which a pose can be considered similar

max_angle_radians

double

Maximum rotation below which a pose can be considered similar

Configuration

From trajectory_builder_2d.lua / trajectory_builder_3d.lua:

motion_filter = {
  max_time_seconds = 0.5,
  max_distance_meters = 0.1,
  max_angle_radians = 0.1,
}

CreateMotionFilterOptions() reads the three keys.

Role in the SLAM Pipeline

Both LocalTrajectoryBuilder2D and LocalTrajectoryBuilder3D call motion_filter_.IsSimilar(time, pose_estimate) before inserting a scan into the submaps. When it returns true, the insertion_result in the returned MatchingResult is nullptr and no new node data or submap update is produced, even though the local pose estimate is still reported.

See Also