Estimate angular velocities from odometry. (cartographer-project#458)

If no IMU is available we will now use odometry to estimate angular velocities if available instead of the last poses. Fixes cartographer-project#453.
doronhi · Aug 16, 2017 · 9498cc9 · 9498cc9
1 parent 0c6d697
commit 9498cc9
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 7 deletions.
diff --git a/cartographer/mapping/pose_extrapolator.cc b/cartographer/mapping/pose_extrapolator.cc
@@ -90,22 +90,29 @@ void PoseExtrapolator::AddOdometryData(
         odometry_data.time >= timed_pose_queue_.back().time);
   odometry_data_.push_back(odometry_data);
   TrimOdometryData();
-  if (odometry_data_.size() < 2 || timed_pose_queue_.empty()) {
+  if (odometry_data_.size() < 2) {
     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 double odometry_time_delta =
+      common::ToSeconds(odometry_data_older.time - odometry_data_newer.time);
+  const transform::Rigid3d odometry_pose_delta =
+      odometry_data_newer.pose.inverse() * odometry_data_older.pose;
+  angular_velocity_from_odometry_ =
+      transform::RotationQuaternionToAngleAxisVector(
+          odometry_pose_delta.rotation()) /
+      odometry_time_delta;
+  if (timed_pose_queue_.empty()) {
+    return;
+  }
   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);
+          odometry_pose_delta.translation() / odometry_time_delta;
   const Eigen::Quaterniond orientation_at_newer_odometry_time =
       timed_pose_queue_.back().pose.rotation() *
       ExtrapolateRotation(odometry_data_newer.time);
@@ -171,7 +178,9 @@ void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
     // the angular velocities from poses and fake gravity to help 2D stability.
     imu_tracker->Advance(time);
     imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());
-    imu_tracker->AddImuAngularVelocityObservation(angular_velocity_from_poses_);
+    imu_tracker->AddImuAngularVelocityObservation(
+        odometry_data_.size() < 2 ? angular_velocity_from_poses_
+                                  : angular_velocity_from_odometry_);
     return;
   }
   if (imu_tracker->time() < imu_data_.front().time) {

diff --git a/cartographer/mapping/pose_extrapolator.h b/cartographer/mapping/pose_extrapolator.h
@@ -81,6 +81,7 @@ class PoseExtrapolator {
 
   std::deque<sensor::OdometryData> odometry_data_;
   Eigen::Vector3d linear_velocity_from_odometry_ = Eigen::Vector3d::Zero();
+  Eigen::Vector3d angular_velocity_from_odometry_ = Eigen::Vector3d::Zero();
 };
 
 }  // namespace mapping