From a6d94c07cfbfb7e57b7bdd43fb36f25f45b01072 Mon Sep 17 00:00:00 2001
From: Wolfgang Hess <whess@google.com>
Date: Fri, 28 Jul 2017 17:12:12 +0200
Subject: [PATCH] Use IMU data when extrapolating poses if available. (#434)

---
 cartographer/mapping/pose_extrapolator.cc | 67 +++++++++++++++++++++--
 cartographer/mapping/pose_extrapolator.h  |  7 +++
 2 files changed, 70 insertions(+), 4 deletions(-)

diff --git a/cartographer/mapping/pose_extrapolator.cc b/cartographer/mapping/pose_extrapolator.cc
index b803ed7..381e75a 100644
--- a/cartographer/mapping/pose_extrapolator.cc
+++ b/cartographer/mapping/pose_extrapolator.cc
@@ -22,6 +22,16 @@
 namespace cartographer {
 namespace mapping {
 
+namespace {
+
+Eigen::Quaterniond RotationFromAngularVelocity(
+    const common::Duration duration, const Eigen::Vector3d& angular_velocity) {
+  return transform::AngleAxisVectorToRotationQuaternion(
+      Eigen::Vector3d(common::ToSeconds(duration) * angular_velocity));
+}
+
+}  // namespace
+
 PoseExtrapolator::PoseExtrapolator(const common::Duration pose_queue_duration)
     : pose_queue_duration_(pose_queue_duration) {}
 
@@ -39,9 +49,15 @@ void PoseExtrapolator::AddPose(const common::Time time,
          timed_pose_queue_[1].time <= time - pose_queue_duration_) {
     timed_pose_queue_.pop_front();
   }
+  TrimImuData();
 }
 
-transform::Rigid3d PoseExtrapolator::ExtrapolatePose(common::Time time) {
+void PoseExtrapolator::AddImuData(const sensor::ImuData& imu_data) {
+  imu_data_.push_back(imu_data);
+  TrimImuData();
+}
+
+transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) {
   CHECK(!timed_pose_queue_.empty());
   const TimedPose& newest_timed_pose = timed_pose_queue_.back();
   const auto newest_time = newest_timed_pose.time;
@@ -62,7 +78,7 @@ transform::Rigid3d PoseExtrapolator::ExtrapolatePose(common::Time time) {
   }
   const Eigen::Vector3d linear_velocity =
       (newest_pose.translation() - oldest_pose.translation()) / queue_delta;
-  const Eigen::Vector3d angular_velocity =
+  const Eigen::Vector3d angular_velocity_from_pose =
       transform::RotationQuaternionToAngleAxisVector(
           oldest_pose.rotation().inverse() * newest_pose.rotation()) /
       queue_delta;
@@ -71,8 +87,51 @@ transform::Rigid3d PoseExtrapolator::ExtrapolatePose(common::Time time) {
                                          linear_velocity) *
          newest_pose *
          transform::Rigid3d::Rotation(
-             transform::AngleAxisVectorToRotationQuaternion(
-                 Eigen::Vector3d(extrapolation_delta * angular_velocity)));
+             ExtrapolateRotation(time, angular_velocity_from_pose));
+}
+
+void PoseExtrapolator::TrimImuData() {
+  while (imu_data_.size() > 2 && !timed_pose_queue_.empty() &&
+         imu_data_[1].time <= timed_pose_queue_.back().time) {
+    imu_data_.pop_front();
+  }
+}
+
+Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation(
+    const common::Time time,
+    const Eigen::Vector3d& angular_velocity_from_pose) {
+  common::Time current = timed_pose_queue_.back().time;
+  if (imu_data_.empty() || imu_data_.front().time >= time) {
+    return RotationFromAngularVelocity(time - current,
+                                       angular_velocity_from_pose);
+  }
+  // TODO(whess): Use the ImuTracker here?
+  // TODO(whess): Keep the last extrapolated pose.
+  Eigen::Quaterniond current_rotation;
+  auto imu_it = imu_data_.begin();
+  if (imu_it->time > current) {
+    current_rotation = RotationFromAngularVelocity(imu_it->time - current,
+                                                   angular_velocity_from_pose);
+    current = imu_it->time;
+  } else {
+    current_rotation = Eigen::Quaterniond::Identity();
+  }
+  CHECK(imu_it != imu_data_.end());
+  CHECK(imu_it->time <= current);
+  CHECK(current < time);
+  while (current < time) {
+    common::Time next = time;
+    CHECK(imu_it != imu_data_.end());
+    auto next_it = imu_it + 1;
+    if (next_it != imu_data_.end() && next > next_it->time) {
+      next = next_it->time;
+    }
+    current_rotation *=
+        RotationFromAngularVelocity(next - current, imu_it->angular_velocity);
+    current = next;
+    imu_it = next_it;
+  }
+  return current_rotation;
 }
 
 }  // namespace mapping
diff --git a/cartographer/mapping/pose_extrapolator.h b/cartographer/mapping/pose_extrapolator.h
index 850f88d..7abeab6 100644
--- a/cartographer/mapping/pose_extrapolator.h
+++ b/cartographer/mapping/pose_extrapolator.h
@@ -20,6 +20,7 @@
 #include <deque>
 
 #include "cartographer/common/time.h"
+#include "cartographer/sensor/imu_data.h"
 #include "cartographer/transform/rigid_transform.h"
 
 namespace cartographer {
@@ -42,15 +43,21 @@ class PoseExtrapolator {
   common::Time GetLastPoseTime() const;
 
   void AddPose(common::Time time, const transform::Rigid3d& pose);
+  void AddImuData(const sensor::ImuData& imu_data);
   transform::Rigid3d ExtrapolatePose(common::Time time);
 
  private:
+  void TrimImuData();
+  Eigen::Quaterniond ExtrapolateRotation(
+      common::Time time, const Eigen::Vector3d& angular_velocity_from_pose);
+
   const common::Duration pose_queue_duration_;
   struct TimedPose {
     common::Time time;
     transform::Rigid3d pose;
   };
   std::deque<TimedPose> timed_pose_queue_;
+  std::deque<sensor::ImuData> imu_data_;
 };
 
 }  // namespace mapping