added imu based extrapolator impl (#1728)

This adds an alternative implementation for pose extrapolation which uses imu integration as well as past local SLAM results (and odometry data) to predict a pose. This allows to perform motion compensation and initialize scan matching afterwards.

Using imu data for pose extrapolation and motion compensation is more accurate, because it does not make a constant velocity assumption like the PoseExtrapolator implementation. Next steps are the integration into local trajectory builder as well as tuning the parameters for existing datasets.

This PR is quite large but was already reviewed in smaller chunks internally and combines contributions from @danielsievers, @wohe and @schwoere.

Signed-off-by: mschworer <mschworer@lyft.com>

cartographer/mapping/imu_based_pose_extrapolator.cc

@@ -0,0 +1,439 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer/mapping/imu_based_pose_extrapolator.h"
+
+#include <algorithm>
+
+#include "absl/memory/memory.h"
+#include "cartographer/mapping/eigen_quaterniond_from_two_vectors.h"
+#include "cartographer/mapping/internal/3d/imu_integration.h"
+#include "cartographer/mapping/internal/3d/rotation_parameterization.h"
+#include "cartographer/mapping/internal/optimization/ceres_pose.h"
+#include "cartographer/mapping/internal/optimization/cost_functions/acceleration_cost_function_3d.h"
+#include "cartographer/mapping/internal/optimization/cost_functions/rotation_cost_function_3d.h"
+#include "cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_3d.h"
+#include "cartographer/mapping/pose_graph_interface.h"
+#include "cartographer/transform/transform.h"
+#include "glog/logging.h"
+
+namespace cartographer {
+namespace mapping {
+
+using ::cartographer::transform::TimestampedTransform;
+
+ImuBasedPoseExtrapolator::ImuBasedPoseExtrapolator(
+    const proto::ImuBasedPoseExtrapolatorOptions& options)
+    : options_(options),
+      solver_options_(
+          common::CreateCeresSolverOptions(options_.solver_options())) {}
+
+ImuBasedPoseExtrapolator::~ImuBasedPoseExtrapolator() {
+  LOG(INFO) << "Number of iterations for pose extrapolation:";
+  LOG(INFO) << num_iterations_hist_.ToString(10);
+}
+
+std::unique_ptr<PoseExtrapolatorInterface>
+ImuBasedPoseExtrapolator::InitializeWithImu(
+    const proto::ImuBasedPoseExtrapolatorOptions& options,
+    const std::vector<sensor::ImuData>& imu_data,
+    const std::vector<transform::TimestampedTransform>& initial_poses) {
+  CHECK(!imu_data.empty());
+  LOG(INFO) << options.DebugString();
+  auto extrapolator = absl::make_unique<ImuBasedPoseExtrapolator>(options);
+  std::copy(imu_data.begin(), imu_data.end(),
+            std::back_inserter(extrapolator->imu_data_));
+  if (!initial_poses.empty()) {
+    for (const auto& pose : initial_poses) {
+      if (pose.time > imu_data.front().time) {
+        extrapolator->AddPose(pose.time, pose.transform);
+      }
+    }
+  } else {
+    extrapolator->AddPose(
+        imu_data.back().time,
+        transform::Rigid3d::Rotation(FromTwoVectors(
+            imu_data.back().linear_acceleration, Eigen::Vector3d::UnitZ())));
+  }
+  return extrapolator;
+}
+
+common::Time ImuBasedPoseExtrapolator::GetLastPoseTime() const {
+  if (timed_pose_queue_.empty()) {
+    return common::Time::min();
+  }
+  return timed_pose_queue_.back().time;
+}
+
+common::Time ImuBasedPoseExtrapolator::GetLastExtrapolatedTime() const {
+  return last_extrapolated_time_;
+}
+
+void ImuBasedPoseExtrapolator::AddPose(const common::Time time,
+                                       const transform::Rigid3d& pose) {
+  timed_pose_queue_.push_back(TimestampedTransform{time, pose});
+  while (timed_pose_queue_.size() > 3 &&
+         timed_pose_queue_[1].time <=
+             time - common::FromSeconds(options_.pose_queue_duration())) {
+    if (!previous_solution_.empty()) {
+      CHECK_EQ(timed_pose_queue_.front().time, previous_solution_.front().time);
+      previous_solution_.pop_front();
+    }
+    timed_pose_queue_.pop_front();
+  }
+  TrimImuData();
+}
+
+void ImuBasedPoseExtrapolator::AddImuData(const sensor::ImuData& imu_data) {
+  CHECK(timed_pose_queue_.empty() ||
+        imu_data.time >= timed_pose_queue_.back().time);
+  imu_data_.push_back(imu_data);
+  TrimImuData();
+}
+
+void ImuBasedPoseExtrapolator::AddOdometryData(
+    const sensor::OdometryData& odometry_data) {
+  CHECK(timed_pose_queue_.empty() ||
+        odometry_data.time >= timed_pose_queue_.back().time);
+  odometry_data_.push_back(odometry_data);
+  TrimOdometryData();
+}
+
+ImuBasedPoseExtrapolator::ExtrapolationResult
+ImuBasedPoseExtrapolator::ExtrapolatePosesWithGravity(
+    const std::vector<common::Time>& times) {
+  const auto& time = times.back();
+  const auto& newest_timed_pose = timed_pose_queue_.back();
+  CHECK_GE(time, newest_timed_pose.time);
+  CHECK_GE(times.size(), 1);
+  last_extrapolated_time_ = time;
+
+  if (timed_pose_queue_.size() < 3 ||
+      common::ToSeconds(time - newest_timed_pose.time) < 1e-6) {
+    return ExtrapolationResult{
+        std::vector<transform::Rigid3f>(
+            times.size() - 1, timed_pose_queue_.back().transform.cast<float>()),
+        timed_pose_queue_.back().transform, Eigen::Vector3d::Zero(),
+        timed_pose_queue_.back().transform.rotation()};
+  }
+
+  ceres::Problem::Options problem_options;
+  ceres::Problem problem(problem_options);
+
+  // Track gravity alignment over time and use this as a frame here so that
+  // we can estimate the gravity alignment of the current pose.
+  optimization::CeresPose gravity_from_local(
+      gravity_from_local_, nullptr,
+      absl::make_unique<ceres::QuaternionParameterization>(), &problem);
+  // Use deque so addresses stay constant during problem formulation.
+  std::deque<optimization::CeresPose> nodes;
+  std::vector<common::Time> node_times;
+
+  for (size_t i = 0; i < timed_pose_queue_.size(); i++) {
+    const bool is_last = (i == (timed_pose_queue_.size() - 1));
+    const auto& timed_pose = timed_pose_queue_[i];
+    node_times.push_back(timed_pose.time);
+
+    transform::Rigid3d gravity_from_node;
+    // Use the last scan match result (timed_pose_queue_.back()) for
+    // initialization here instead of the last result from the optimization.
+    // This keeps poses from slowly drifting apart due to lack of feedback
+    // from the scan matching here.
+    if (!previous_solution_.empty() && !is_last) {
+      CHECK_GT(previous_solution_.size(), i);
+      CHECK_EQ(timed_pose.time, previous_solution_[i].time);
+      gravity_from_node = previous_solution_[i].transform;
+    } else {
+      gravity_from_node = gravity_from_local_ * timed_pose.transform;
+    }
+
+    if (is_last) {
+      nodes.emplace_back(gravity_from_node, nullptr,
+                         absl::make_unique<ceres::AutoDiffLocalParameterization<
+                             ConstantYawQuaternionPlus, 4, 2>>(),
+                         &problem);
+      problem.SetParameterBlockConstant(nodes.back().translation());
+    } else {
+      nodes.emplace_back(gravity_from_node, nullptr,
+                         absl::make_unique<ceres::QuaternionParameterization>(),
+                         &problem);
+    }
+  }
+
+  double gravity_constant = 9.8;
+  bool fix_gravity = false;
+  if (options_.gravity_constant() > 0) {
+    fix_gravity = true;
+    gravity_constant = options_.gravity_constant();
+  }
+
+  auto imu_it_prev_prev = imu_data_.begin();
+  while (imu_it_prev_prev != std::prev(imu_data_.end()) &&
+         std::next(imu_it_prev_prev)->time <= timed_pose_queue_.back().time) {
+    ++imu_it_prev_prev;
+  }
+
+  const TimestampedTransform prev_gravity_from_tracking =
+      TimestampedTransform{node_times.back(), nodes.back().ToRigid()};
+  const TimestampedTransform prev_prev_gravity_from_tracking =
+      TimestampedTransform{node_times.at(node_times.size() - 2),
+                           nodes.at(nodes.size() - 2).ToRigid()};
+  const transform::Rigid3d initial_estimate =
+      ExtrapolatePoseWithImu<double>(
+          prev_gravity_from_tracking.transform, prev_gravity_from_tracking.time,
+          prev_prev_gravity_from_tracking.transform,
+          prev_prev_gravity_from_tracking.time,
+          gravity_constant * Eigen::Vector3d::UnitZ(), time, imu_data_,
+          &imu_it_prev_prev)
+          .pose;
+  nodes.emplace_back(initial_estimate, nullptr,
+                     absl::make_unique<ceres::QuaternionParameterization>(),
+                     &problem);
+  node_times.push_back(time);
+
+  // Add cost functions for node constraints.
+  for (size_t i = 0; i < timed_pose_queue_.size(); i++) {
+    const auto& timed_pose = timed_pose_queue_[i];
+    problem.AddResidualBlock(
+        optimization::SpaCostFunction3D::CreateAutoDiffCostFunction(
+            PoseGraphInterface::Constraint::Pose{
+                timed_pose.transform, options_.pose_translation_weight(),
+                options_.pose_rotation_weight()}),
+        nullptr /* loss function */, gravity_from_local.rotation(),
+        gravity_from_local.translation(), nodes.at(i).rotation(),
+        nodes.at(i).translation());
+  }
+
+  CHECK(!imu_data_.empty());
+  CHECK_LE(imu_data_.front().time, timed_pose_queue_.front().time);
+
+  std::array<double, 4> imu_calibration{{1., 0., 0., 0.}};
+
+  problem.AddParameterBlock(imu_calibration.data(), 4,
+                            new ceres::QuaternionParameterization());
+  problem.SetParameterBlockConstant(imu_calibration.data());
+
+  auto imu_it = imu_data_.begin();
+  CHECK(imu_data_.size() == 1 ||
+        std::next(imu_it)->time > timed_pose_queue_.front().time);
+
+  transform::Rigid3d last_node_odometry;
+  common::Time last_node_odometry_time;
+
+  for (size_t i = 1; i < nodes.size(); i++) {
+    const common::Time first_time = node_times[i - 1];
+    const common::Time second_time = node_times[i];
+
+    auto imu_it2 = imu_it;
+    const IntegrateImuResult<double> result =
+        IntegrateImu(imu_data_, first_time, second_time, &imu_it);
+    if ((i + 1) < nodes.size()) {
+      const common::Time third_time = node_times[i + 1];
+      const common::Duration first_duration = second_time - first_time;
+      const common::Duration second_duration = third_time - second_time;
+      const common::Time first_center = first_time + first_duration / 2;
+      const common::Time second_center = second_time + second_duration / 2;
+      const IntegrateImuResult<double> result_to_first_center =
+          IntegrateImu(imu_data_, first_time, first_center, &imu_it2);
+      const IntegrateImuResult<double> result_center_to_center =
+          IntegrateImu(imu_data_, first_center, second_center, &imu_it2);
+      // 'delta_velocity' is the change in velocity from the point in time
+      // halfway between the first and second poses to halfway between
+      // second and third pose. It is computed from IMU data and still
+      // contains a delta due to gravity. The orientation of this vector is
+      // in the IMU frame at the second pose.
+      const Eigen::Vector3d delta_velocity =
+          (result.delta_rotation.inverse() *
+           result_to_first_center.delta_rotation) *
+          result_center_to_center.delta_velocity;
+      problem.AddResidualBlock(
+          AccelerationCostFunction3D::CreateAutoDiffCostFunction(
+              options_.imu_acceleration_weight(), delta_velocity,
+              common::ToSeconds(first_duration),
+              common::ToSeconds(second_duration)),
+          nullptr /* loss function */, nodes.at(i).rotation(),
+          nodes.at(i - 1).translation(), nodes.at(i).translation(),
+          nodes.at(i + 1).translation(), &gravity_constant,
+          imu_calibration.data());
+      // TODO(danielsievers): Fix gravity in CostFunction.
+      if (fix_gravity) {
+        problem.SetParameterBlockConstant(&gravity_constant);
+      } else {
+        // Force gravity constant to be positive.
+        problem.SetParameterLowerBound(&gravity_constant, 0, 0.0);
+      }
+    }
+    problem.AddResidualBlock(
+        RotationCostFunction3D::CreateAutoDiffCostFunction(
+            options_.imu_rotation_weight(), result.delta_rotation),
+        nullptr /* loss function */, nodes.at(i - 1).rotation(),
+        nodes.at(i).rotation(), imu_calibration.data());
+
+    // Add a relative pose constraint based on the odometry (if available).
+    if (HasOdometryDataForTime(first_time) &&
+        HasOdometryDataForTime(second_time)) {
+      // Here keep track of last node odometry to avoid double computation.
+      // Do this if first loop, or if there were some missing odometry nodes
+      // then recalculate.
+      if (i == 1 || last_node_odometry_time != first_time) {
+        last_node_odometry = InterpolateOdometry(first_time);
+        last_node_odometry_time = first_time;
+      }
+      const transform::Rigid3d current_node_odometry =
+          InterpolateOdometry(second_time);
+      const transform::Rigid3d relative_odometry =
+          CalculateOdometryBetweenNodes(last_node_odometry,
+                                        current_node_odometry);
+
+      problem.AddResidualBlock(
+          optimization::SpaCostFunction3D::CreateAutoDiffCostFunction(
+              PoseGraphInterface::Constraint::Pose{
+                  relative_odometry, options_.odometry_translation_weight(),
+                  options_.odometry_rotation_weight()}),
+          nullptr /* loss function */, nodes.at(i - 1).rotation(),
+          nodes.at(i - 1).translation(), nodes.at(i).rotation(),
+          nodes.at(i).translation());
+      // Use the current node odometry in the next iteration
+      last_node_odometry = current_node_odometry;
+      last_node_odometry_time = second_time;
+    }
+  }
+
+  // Solve.
+  ceres::Solver::Summary summary;
+  ceres::Solve(solver_options_, &problem, &summary);
+  LOG_IF_EVERY_N(INFO, !fix_gravity, 20) << "Gravity was: " << gravity_constant;
+
+  const auto gravity_estimate = nodes.back().ToRigid().rotation();
+
+  const auto& last_pose = timed_pose_queue_.back();
+  const auto extrapolated_pose = TimestampedTransform{
+      time, last_pose.transform *
+                nodes.at(nodes.size() - 2).ToRigid().inverse() *
+                nodes.back().ToRigid()};
+
+  num_iterations_hist_.Add(summary.iterations.size());
+
+  gravity_from_local_ = gravity_from_local.ToRigid();
+
+  previous_solution_.clear();
+  for (size_t i = 0; i < nodes.size(); ++i) {
+    previous_solution_.push_back(
+        TimestampedTransform{node_times.at(i), nodes.at(i).ToRigid()});
+  }
+
+  const Eigen::Vector3d current_velocity =
+      (extrapolated_pose.transform.translation() -
+       last_pose.transform.translation()) /
+      common::ToSeconds(time - last_pose.time);
+
+  return ExtrapolationResult{
+      InterpolatePoses(last_pose, extrapolated_pose, times.begin(),
+                       std::prev(times.end())),
+      extrapolated_pose.transform, current_velocity, gravity_estimate};
+}
+
+std::vector<transform::Rigid3f> ImuBasedPoseExtrapolator::InterpolatePoses(
+    const TimestampedTransform& start, const TimestampedTransform& end,
+    const std::vector<common::Time>::const_iterator times_begin,
+    const std::vector<common::Time>::const_iterator times_end) {
+  std::vector<transform::Rigid3f> poses;
+  poses.reserve(std::distance(times_begin, times_end));
+  const float duration_scale = 1. / common::ToSeconds(end.time - start.time);
+
+  const Eigen::Quaternionf start_rotation =
+      Eigen::Quaternionf(start.transform.rotation());
+  const Eigen::Quaternionf end_rotation =
+      Eigen::Quaternionf(end.transform.rotation());
+  const Eigen::Vector3f start_translation =
+      start.transform.translation().cast<float>();
+  const Eigen::Vector3f end_translation =
+      end.transform.translation().cast<float>();
+
+  for (auto it = times_begin; it != times_end; ++it) {
+    const float factor = common::ToSeconds(*it - start.time) * duration_scale;
+    const Eigen::Vector3f origin =
+        start_translation + (end_translation - start_translation) * factor;
+    const Eigen::Quaternionf rotation =
+        start_rotation.slerp(factor, end_rotation);
+    poses.emplace_back(origin, rotation);
+  }
+  return poses;
+}
+
+transform::Rigid3d ImuBasedPoseExtrapolator::ExtrapolatePose(
+    const common::Time time) {
+  return ExtrapolatePosesWithGravity(std::vector<common::Time>{time})
+      .current_pose;
+}
+
+Eigen::Quaterniond ImuBasedPoseExtrapolator::EstimateGravityOrientation(
+    const common::Time time) {
+  return ExtrapolatePosesWithGravity(std::vector<common::Time>{time})
+      .gravity_from_tracking;
+}
+
+template <typename T>
+void ImuBasedPoseExtrapolator::TrimDequeData(std::deque<T>* data) {
+  while (data->size() > 1 && !timed_pose_queue_.empty() &&
+         data->at(1).time <= timed_pose_queue_.front().time) {
+    data->pop_front();
+  }
+}
+
+void ImuBasedPoseExtrapolator::TrimImuData() {
+  TrimDequeData<sensor::ImuData>(&imu_data_);
+}
+
+void ImuBasedPoseExtrapolator::TrimOdometryData() {
+  TrimDequeData<sensor::OdometryData>(&odometry_data_);
+}
+
+// Odometry methods
+bool ImuBasedPoseExtrapolator::HasOdometryData() const {
+  return odometry_data_.size() >= 2;
+}
+
+bool ImuBasedPoseExtrapolator::HasOdometryDataForTime(
+    const common::Time& time) const {
+  return HasOdometryData() && odometry_data_.front().time < time &&
+         time < odometry_data_.back().time;
+}
+
+transform::Rigid3d ImuBasedPoseExtrapolator::InterpolateOdometry(
+    const common::Time& time) const {
+  // Only interpolate if time is within odometry data range.
+  CHECK(HasOdometryDataForTime(time))
+      << "Odometry data range does not include time " << time;
+  std::deque<sensor::OdometryData>::const_iterator data = std::upper_bound(
+      odometry_data_.begin(), odometry_data_.end(), time,
+      [](const common::Time& time, const sensor::OdometryData& odometry_data) {
+        return time < odometry_data.time;
+      });
+  const TimestampedTransform first{std::prev(data)->time,
+                                   std::prev(data)->pose};
+  const TimestampedTransform second{data->time, data->pose};
+  return Interpolate(first, second, time).transform;
+}
+
+transform::Rigid3d ImuBasedPoseExtrapolator::CalculateOdometryBetweenNodes(
+    const transform::Rigid3d& first_node_odometry,
+    const transform::Rigid3d& second_node_odometry) const {
+  return first_node_odometry.inverse() * second_node_odometry;
+}
+
+}  // namespace mapping
+}  // namespace cartographer

cartographer/mapping/imu_based_pose_extrapolator.h

@@ -0,0 +1,102 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_MAPPING_IMU_BASED_POSE_EXTRAPOLATOR_H_
+#define CARTOGRAPHER_MAPPING_IMU_BASED_POSE_EXTRAPOLATOR_H_
+
+#include <deque>
+#include <memory>
+#include <vector>
+
+#include "cartographer/common/ceres_solver_options.h"
+#include "cartographer/common/histogram.h"
+#include "cartographer/mapping/pose_extrapolator_interface.h"
+#include "cartographer/sensor/imu_data.h"
+#include "cartographer/transform/timestamped_transform.h"
+
+namespace cartographer {
+namespace mapping {
+
+// Uses the linear acceleration and rotational velocities to estimate a pose.
+class ImuBasedPoseExtrapolator : public PoseExtrapolatorInterface {
+ public:
+  explicit ImuBasedPoseExtrapolator(
+      const proto::ImuBasedPoseExtrapolatorOptions& options);
+  ~ImuBasedPoseExtrapolator() override;
+
+  static std::unique_ptr<PoseExtrapolatorInterface> InitializeWithImu(
+      const proto::ImuBasedPoseExtrapolatorOptions& options,
+      const std::vector<sensor::ImuData>& imu_data,
+      const std::vector<transform::TimestampedTransform>& initial_poses);
+
+  // Returns the time of the last added pose or Time::min() if no pose was added
+  // yet.
+  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;
+
+  // Gravity alignment estimate.
+  Eigen::Quaterniond EstimateGravityOrientation(common::Time time) override;
+
+ private:
+  template <typename T>
+  void TrimDequeData(std::deque<T>* data);
+
+  void TrimImuData();
+  void TrimOdometryData();
+
+  // Odometry methods.
+  bool HasOdometryData() const;
+  bool HasOdometryDataForTime(const common::Time& first_time) const;
+  transform::Rigid3d InterpolateOdometry(const common::Time& first_time) const;
+  transform::Rigid3d CalculateOdometryBetweenNodes(
+      const transform::Rigid3d& first_node_odometry,
+      const transform::Rigid3d& second_node_odometry) const;
+
+  std::vector<transform::Rigid3f> InterpolatePoses(
+      const ::cartographer::transform::TimestampedTransform& start,
+      const ::cartographer::transform::TimestampedTransform& end,
+      const std::vector<common::Time>::const_iterator times_begin,
+      const std::vector<common::Time>::const_iterator times_end);
+
+  std::deque<::cartographer::transform::TimestampedTransform> timed_pose_queue_;
+  std::deque<::cartographer::transform::TimestampedTransform>
+      previous_solution_;
+
+  std::deque<sensor::ImuData> imu_data_;
+  std::deque<sensor::OdometryData> odometry_data_;
+  common::Time last_extrapolated_time_ = common::Time::min();
+
+  transform::Rigid3d gravity_from_local_ = transform::Rigid3d::Identity();
+
+  const proto::ImuBasedPoseExtrapolatorOptions options_;
+  const ceres::Solver::Options solver_options_;
+
+  common::Histogram num_iterations_hist_;
+};
+
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_IMU_BASED_POSE_EXTRAPOLATOR_H_

cartographer/mapping/internal/3d/imu_integration.h

@@ -24,6 +24,7 @@
 #include "Eigen/Geometry"
 #include "cartographer/common/time.h"
 #include "cartographer/sensor/imu_data.h"
+#include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
 #include "glog/logging.h"
 
@@ -33,6 +34,7 @@ namespace mapping {
 template <typename T>
 struct IntegrateImuResult {
   Eigen::Matrix<T, 3, 1> delta_velocity;
+  Eigen::Matrix<T, 3, 1> delta_translation;
   Eigen::Quaternion<T> delta_rotation;
 };
 
@@ -54,6 +56,7 @@ IntegrateImuResult<T> IntegrateImu(
   common::Time current_time = start_time;
 
   IntegrateImuResult<T> result = {Eigen::Matrix<T, 3, 1>::Zero(),
+                                  Eigen::Matrix<T, 3, 1>::Zero(),
                                   Eigen::Quaterniond::Identity().cast<T>()};
   while (current_time < end_time) {
     common::Time next_imu_data = common::Time::max();
@@ -73,6 +76,7 @@ IntegrateImuResult<T> IntegrateImu(
                              ((linear_acceleration_calibration *
                                (*it)->linear_acceleration.template cast<T>()) *
                               delta_t);
+    result.delta_translation += delta_t * result.delta_velocity;
     current_time = next_time;
     if (current_time == next_imu_data) {
       ++(*it);
@@ -92,6 +96,67 @@ IntegrateImuResult<double> IntegrateImu(const RangeType& imu_data,
       start_time, end_time, it);
 }
 
+template <typename T>
+struct ExtrapolatePoseResult {
+  transform::Rigid3<T> pose;
+  Eigen::Matrix<T, 3, 1> velocity;
+};
+
+// Returns pose and linear velocity at 'time' which is equal to
+// 'prev_from_tracking' extrapolated using IMU data.
+template <typename T, typename RangeType, typename IteratorType>
+ExtrapolatePoseResult<T> ExtrapolatePoseWithImu(
+    const transform::Rigid3<T>& prev_from_tracking,
+    const Eigen::Matrix<T, 3, 1>& prev_velocity_in_tracking,
+    const common::Time prev_time, const Eigen::Matrix<T, 3, 1>& gravity,
+    const common::Time time, const RangeType& imu_data,
+    IteratorType* const imu_it) {
+  const IntegrateImuResult<T> result =
+      IntegrateImu(imu_data, Eigen::Transform<T, 3, Eigen::Affine>::Identity(),
+                   Eigen::Transform<T, 3, Eigen::Affine>::Identity(), prev_time,
+                   time, imu_it);
+
+  const T delta_t = static_cast<T>(common::ToSeconds(time - prev_time));
+  const Eigen::Matrix<T, 3, 1> translation =
+      prev_from_tracking.translation() +
+      prev_from_tracking.rotation() *
+          (delta_t * prev_velocity_in_tracking + result.delta_translation) -
+      static_cast<T>(.5) * delta_t * delta_t * gravity;
+  const Eigen::Quaternion<T> rotation =
+      prev_from_tracking.rotation() * result.delta_rotation;
+
+  const Eigen::Matrix<T, 3, 1> velocity =
+      prev_from_tracking.rotation() *
+          (prev_velocity_in_tracking + result.delta_velocity) -
+      delta_t * gravity;
+  return ExtrapolatePoseResult<T>{transform::Rigid3<T>(translation, rotation),
+                                  velocity};
+}
+
+// Same as above but given the last two poses.
+template <typename T, typename RangeType, typename IteratorType>
+ExtrapolatePoseResult<T> ExtrapolatePoseWithImu(
+    const transform::Rigid3<T>& prev_from_tracking,
+    const common::Time prev_time,
+    const transform::Rigid3<T>& prev_prev_from_tracking,
+    const common::Time prev_prev_time, const Eigen::Matrix<T, 3, 1>& gravity,
+    const common::Time time, const RangeType& imu_data,
+    IteratorType* const imu_it) {
+  // TODO(danielsievers): Really we should integrate velocities starting from
+  // the midpoint in between two poses, since this is how we fit them to poses
+  // in the optimization.
+  const T prev_delta_t =
+      static_cast<T>(common::ToSeconds(prev_time - prev_prev_time));
+  const Eigen::Matrix<T, 3, 1> prev_velocity_in_tracking =
+      prev_from_tracking.inverse().rotation() *
+      (prev_from_tracking.translation() -
+       prev_prev_from_tracking.translation()) /
+      prev_delta_t;
+
+  return ExtrapolatePoseWithImu(prev_from_tracking, prev_velocity_in_tracking,
+                                prev_time, gravity, time, imu_data, imu_it);
+}
+
 }  // namespace mapping
 }  // namespace cartographer
 

cartographer/mapping/proto/pose_extrapolator_options.proto

@@ -14,6 +14,8 @@
 
 syntax = "proto3";
 
+import "cartographer/common/proto/ceres_solver_options.proto";
+
 package cartographer.mapping.proto;
 
 message ConstantVelocityPoseExtrapolatorOptions {
@@ -27,6 +29,20 @@ message ConstantVelocityPoseExtrapolatorOptions {
   double pose_queue_duration = 2;
 }
 
-message PoseExtrapolatorOptions {
+message ImuBasedPoseExtrapolatorOptions {
-  ConstantVelocityPoseExtrapolatorOptions constant_velocity = 2;
+  double pose_queue_duration = 1;
+  double gravity_constant = 2;
+  double pose_translation_weight = 3;
+  double pose_rotation_weight = 4;
+  double imu_acceleration_weight = 5;
+  double imu_rotation_weight = 6;
+  cartographer.common.proto.CeresSolverOptions solver_options = 7;
+  double odometry_translation_weight = 8;
+  double odometry_rotation_weight = 9;
+}
+
+message PoseExtrapolatorOptions {
+  bool use_imu_based = 1;
+  ConstantVelocityPoseExtrapolatorOptions constant_velocity = 2;
+  ImuBasedPoseExtrapolatorOptions imu_based = 3;
 }