# 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`:

```cpp
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

```cpp
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`:

```cpp
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`:

```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

- `PoseExtrapolatorInterface` — Abstract interface and `ExtrapolationResult`
- [LocalTrajectoryBuilder3D](3d/local_trajectory_builder_3d.md) — Primary consumer
- [Sensor ImuData](../../sensor/imu_data.md) — IMU measurement type
- [Sensor OdometryData](../../sensor/odometry_data.md) — Odometry input type