From 6ebfa50291a53485e432902c11a23f22d0b87e84 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Tue, 8 Aug 2017 16:30:53 +0200 Subject: [PATCH] Use odometry to extrapolate linear motion. (#443) --- cartographer/mapping/pose_extrapolator.cc | 53 +++++++++++++++++++++-- cartographer/mapping/pose_extrapolator.h | 16 ++++--- 2 files changed, 59 insertions(+), 10 deletions(-) diff --git a/cartographer/mapping/pose_extrapolator.cc b/cartographer/mapping/pose_extrapolator.cc index 6d63d70..8f8b535 100644 --- a/cartographer/mapping/pose_extrapolator.cc +++ b/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 = - common::ToSeconds(time - newest_timed_pose.time); - return transform::Rigid3d::Translation(extrapolation_delta * - linear_velocity_from_poses_) * + return transform::Rigid3d::Translation(ExtrapolateTranslation(time)) * 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 diff --git a/cartographer/mapping/pose_extrapolator.h b/cartographer/mapping/pose_extrapolator.h index 263579b..1fc5eef 100644 --- a/cartographer/mapping/pose_extrapolator.h +++ b/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, -// and uses the velocities to extrapolate motion. Uses IMU data if available to -// improve the extrapolation of orientation. -// -// TODO(whess): Add odometry and provide improved extrapolation models making -// use of all available data. +// Keep poses for a certain duration to estimate linear and angular velocity. +// Uses the velocities to extrapolate motion. Uses IMU and/or odometry data if +// available to improve the extrapolation. 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 imu_data_; std::unique_ptr imu_tracker_; + + std::deque odometry_data_; + Eigen::Vector3d linear_velocity_from_odometry_ = Eigen::Vector3d::Zero(); }; } // namespace mapping