cartographer/mapping/internal/imu_based_pose_extrapolator.h

Overview

ImuBasedPoseExtrapolator is a PoseExtrapolatorInterface implementation that estimates pose using the IMU’s linear acceleration and angular velocity, instead of assuming constant velocity. It solves a small Ceres optimization problem over the recent pose queue, integrating IMU measurements between poses, to predict the tracking frame pose at arbitrary times and to estimate gravity alignment. It is used by 3D local SLAM when use_imu_based is set.

Class: cartographer::mapping::ImuBasedPoseExtrapolator

Interface

Implements PoseExtrapolatorInterface:

common::Time GetLastPoseTime() const override;
common::Time GetLastExtrapolatedTime() const override;

void AddPose(common::Time time, const transform::Rigid3d& pose) override;
void AddImuData(const sensor::ImuData& imu_data) override;
void AddOdometryData(const sensor::OdometryData& odometry_data) override;

transform::Rigid3d ExtrapolatePose(common::Time time) override;

ExtrapolationResult ExtrapolatePosesWithGravity(
    const std::vector<common::Time>& times) override;

Eigen::Quaterniond EstimateGravityOrientation(common::Time time) override;

Factory

static std::unique_ptr<PoseExtrapolatorInterface> InitializeWithImu(
    const proto::ImuBasedPoseExtrapolatorOptions& options,
    const std::vector<sensor::ImuData>& imu_data,
    const std::vector<transform::TimestampedTransform>& initial_poses);

Seeds the extrapolator with buffered IMU data and optional initial poses. If no initial poses are given, the first pose is built by aligning the last linear acceleration measurement with the world z-axis (gravity) via FromTwoVectors.

ExtrapolationResult

Defined in PoseExtrapolatorInterface:

struct ExtrapolationResult {
  std::vector<transform::Rigid3f> previous_poses;  // times[0..N-1]
  transform::Rigid3d current_pose;                 // times[N]
  Eigen::Vector3d current_velocity;
  Eigen::Quaterniond gravity_from_tracking;
};

Extrapolation with Gravity

ExtrapolatePosesWithGravity(times):

  1. Requires at least 3 poses in the queue and a positive time span; otherwise returns the last known pose for all requested times.

  2. Builds a Ceres problem with a gravity_from_local pose (the slowly varying gravity alignment), one node per queued pose, plus an extra node for the requested extrapolation time seeded by ExtrapolatePoseWithImu.

  3. Adds residual blocks:

    • SpaCostFunction3D per queued pose (weighted by pose_translation_weight / pose_rotation_weight) keeping nodes near their scan-match poses.

    • AccelerationCostFunction3D between consecutive triples of nodes, constraining acceleration computed from node translations against the IMU-integrated delta_velocity (weight imu_acceleration_weight).

    • RotationCostFunction3D between consecutive nodes, constraining relative rotation against the IMU delta_rotation (weight imu_rotation_weight).

    • SpaCostFunction3D between consecutive nodes when odometry is available, constraining relative motion to the interpolated odometry delta (weighted by odometry_translation_weight / odometry_rotation_weight).

  4. The gravity_constant (default 9.8) is optimized unless a positive gravity_constant option fixes it; the IMU calibration quaternion is added as a constant parameter block.

  5. After ceres::Solve, the extrapolated pose is expressed relative to the last known scan-match pose, poses are interpolated to the requested times, the current velocity is computed, and gravity_from_local plus the solution are cached for reuse.

Estimation of Gravity

EstimateGravityOrientation(time) returns ExtrapolatePosesWithGravity({time}).gravity_from_tracking, the rotation from the tracking frame into the gravity-aligned frame at the requested time.

Data Management

  • AddPose pushes into timed_pose_queue_, trimming entries older than pose_queue_duration and dropping them from previous_solution_.

  • AddImuData / AddOdometryData append to their deques (timestamps must not go backwards) and TrimImuData / TrimOdometryData drop data older than the front of the pose queue.

  • InterpolateOdometry linearly interpolates odometry poses within the buffered odometry range; CalculateOdometryBetweenNodes computes the relative odometry between two node poses.

Options

proto::ImuBasedPoseExtrapolatorOptions:

Field

Type

Meaning

pose_queue_duration

double

Keep poses within this time horizon (s)

gravity_constant

double

Fix gravity (if > 0); otherwise estimated

pose_translation_weight

double

Weight keeping nodes near scan-match poses

pose_rotation_weight

double

Rotation weight for scan-match poses

imu_acceleration_weight

double

Weight of the acceleration residual

imu_rotation_weight

double

Weight of the rotation residual

odometry_translation_weight

double

Translation weight for odometry deltas

odometry_rotation_weight

double

Rotation weight for odometry deltas

solver_options

CeresSolverOptions

Ceres solver configuration

Configuration

From trajectory_builder_3d.lua:

pose_extrapolator = {
  use_imu_based = true,
  imu_based = {
    pose_queue_duration = 0.25,
    gravity_constant = 9.8,
    pose_translation_weight = 1.,
    pose_rotation_weight = 1.,
    imu_acceleration_weight = 1.,
    imu_rotation_weight = 1.,
    odometry_translation_weight = 1.,
    odometry_rotation_weight = 1.,
    solver_options = { ... },
  },
}

Role in the SLAM Pipeline

LocalTrajectoryBuilder3D creates this extrapolator on the first IMU message (via InitializeWithImu) and uses ExtrapolatePosesWithGravity to obtain a pose for every point of an accumulated scan, plus the current pose and gravity alignment used to seed scan matching and submap insertion.

See Also