Use odometry to extrapolate linear motion. (#443)

cartographer/mapping/pose_extrapolator.cc

@@ -55,6 +55,7 @@ void PoseExtrapolator::AddPose(const common::Time time,
   UpdateVelocitiesFromPoses();
   AdvanceImuTracker(time, imu_tracker_.get());
   TrimImuData();
+  TrimOdometryData();
 }
 
 void PoseExtrapolator::AddImuData(const sensor::ImuData& imu_data) {
@@ -64,14 +65,41 @@ void PoseExtrapolator::AddImuData(const sensor::ImuData& imu_data) {
   TrimImuData();
 }
 
+void PoseExtrapolator::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();
+  if (odometry_data_.size() < 2 || timed_pose_queue_.empty()) {
+    return;
+  }
+  // TODO(whess): Improve by using more than just the last two odometry poses.
+  // TODO(whess): Use odometry to predict orientation if there is no IMU.
+  // Compute extrapolation in the tracking frame.
+  const sensor::OdometryData& odometry_data_older =
+      odometry_data_[odometry_data_.size() - 2];
+  const sensor::OdometryData& odometry_data_newer =
+      odometry_data_[odometry_data_.size() - 1];
+  const Eigen::Vector3d
+      linear_velocity_in_tracking_frame_at_newer_odometry_time =
+          (odometry_data_newer.pose.inverse() * odometry_data_older.pose)
+              .translation() /
+          common::ToSeconds(odometry_data_older.time -
+                            odometry_data_newer.time);
+  const Eigen::Quaterniond orientation_at_newer_odometry_time =
+      timed_pose_queue_.back().pose.rotation() *
+      ExtrapolateRotation(odometry_data_newer.time);
+  linear_velocity_from_odometry_ =
+      orientation_at_newer_odometry_time *
+      linear_velocity_in_tracking_frame_at_newer_odometry_time;
+}
+
 transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) {
   // TODO(whess): Keep the last extrapolated pose.
   const TimedPose& newest_timed_pose = timed_pose_queue_.back();
   CHECK_GE(time, newest_timed_pose.time);
-  const double extrapolation_delta =
+  return transform::Rigid3d::Translation(ExtrapolateTranslation(time)) *
-      common::ToSeconds(time - newest_timed_pose.time);
-  return transform::Rigid3d::Translation(extrapolation_delta *
-                                         linear_velocity_from_poses_) *
          newest_timed_pose.pose *
          transform::Rigid3d::Rotation(ExtrapolateRotation(time));
 }
@@ -109,6 +137,13 @@ void PoseExtrapolator::TrimImuData() {
   }
 }
 
+void PoseExtrapolator::TrimOdometryData() {
+  while (odometry_data_.size() > 2 && !timed_pose_queue_.empty() &&
+         odometry_data_[1].time <= timed_pose_queue_.back().time) {
+    odometry_data_.pop_front();
+  }
+}
+
 void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
                                          ImuTracker* const imu_tracker) {
   CHECK_GE(time, imu_tracker->time());
@@ -146,5 +181,15 @@ Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation(
   return last_orientation.inverse() * imu_tracker.orientation();
 }
 
+Eigen::Vector3d PoseExtrapolator::ExtrapolateTranslation(common::Time time) {
+  const TimedPose& newest_timed_pose = timed_pose_queue_.back();
+  const double extrapolation_delta =
+      common::ToSeconds(time - newest_timed_pose.time);
+  if (odometry_data_.size() < 2) {
+    return extrapolation_delta * linear_velocity_from_poses_;
+  }
+  return extrapolation_delta * linear_velocity_from_odometry_;
+}
+
 }  // namespace mapping
 }  // namespace cartographer

cartographer/mapping/pose_extrapolator.h

@@ -23,17 +23,15 @@
 #include "cartographer/common/time.h"
 #include "cartographer/mapping/imu_tracker.h"
 #include "cartographer/sensor/imu_data.h"
+#include "cartographer/sensor/odometry_data.h"
 #include "cartographer/transform/rigid_transform.h"
 
 namespace cartographer {
 namespace mapping {
 
-// Keeps poses for a certain duration to estimate linear and angular velocity,
+// Keep poses for a certain duration to estimate linear and angular velocity.
-// and uses the velocities to extrapolate motion. Uses IMU data if available to
+// Uses the velocities to extrapolate motion. Uses IMU and/or odometry data if
-// improve the extrapolation of orientation.
+// available to improve the extrapolation.
-//
-// TODO(whess): Add odometry and provide improved extrapolation models making
-// use of all available data.
 class PoseExtrapolator {
  public:
   explicit PoseExtrapolator(common::Duration pose_queue_duration,
@@ -48,13 +46,16 @@ class PoseExtrapolator {
 
   void AddPose(common::Time time, const transform::Rigid3d& pose);
   void AddImuData(const sensor::ImuData& imu_data);
+  void AddOdometryData(const sensor::OdometryData& odometry_data);
   transform::Rigid3d ExtrapolatePose(common::Time time);
 
  private:
   void UpdateVelocitiesFromPoses();
   void TrimImuData();
+  void TrimOdometryData();
   void AdvanceImuTracker(common::Time time, ImuTracker* imu_tracker);
   Eigen::Quaterniond ExtrapolateRotation(common::Time time);
+  Eigen::Vector3d ExtrapolateTranslation(common::Time time);
 
   const common::Duration pose_queue_duration_;
   struct TimedPose {
@@ -68,6 +69,9 @@ class PoseExtrapolator {
   const double gravity_time_constant_;
   std::deque<sensor::ImuData> imu_data_;
   std::unique_ptr<ImuTracker> imu_tracker_;
+
+  std::deque<sensor::OdometryData> odometry_data_;
+  Eigen::Vector3d linear_velocity_from_odometry_ = Eigen::Vector3d::Zero();
 };
 
 }  // namespace mapping