From 6035f6386064b30b513040458a48c8be5dc7581f Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 26 Jul 2017 15:21:05 +0200 Subject: [PATCH] Add a extrapolator for poses. (#430) This will be used in Cartographer ROS to extrapolate poses for tf. PAIR=damonkohler --- cartographer/mapping/pose_extrapolator.cc | 79 +++++++++++++++++++ cartographer/mapping/pose_extrapolator.h | 59 ++++++++++++++ .../mapping/pose_graph_trimmer_test.cc | 4 +- 3 files changed, 141 insertions(+), 1 deletion(-) create mode 100644 cartographer/mapping/pose_extrapolator.cc create mode 100644 cartographer/mapping/pose_extrapolator.h diff --git a/cartographer/mapping/pose_extrapolator.cc b/cartographer/mapping/pose_extrapolator.cc new file mode 100644 index 0000000..b803ed7 --- /dev/null +++ b/cartographer/mapping/pose_extrapolator.cc @@ -0,0 +1,79 @@ +/* + * Copyright 2017 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/pose_extrapolator.h" + +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +PoseExtrapolator::PoseExtrapolator(const common::Duration pose_queue_duration) + : pose_queue_duration_(pose_queue_duration) {} + +common::Time PoseExtrapolator::GetLastPoseTime() const { + if (timed_pose_queue_.empty()) { + return common::Time::min(); + } + return timed_pose_queue_.back().time; +} + +void PoseExtrapolator::AddPose(const common::Time time, + const transform::Rigid3d& pose) { + timed_pose_queue_.push_back(TimedPose{time, pose}); + while (timed_pose_queue_.size() > 2 && + timed_pose_queue_[1].time <= time - pose_queue_duration_) { + timed_pose_queue_.pop_front(); + } +} + +transform::Rigid3d PoseExtrapolator::ExtrapolatePose(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; + const transform::Rigid3d& newest_pose = newest_timed_pose.pose; + CHECK(time >= newest_time); + if (timed_pose_queue_.size() == 1) { + return newest_pose; + } + const TimedPose& oldest_timed_pose = timed_pose_queue_.front(); + const auto oldest_time = oldest_timed_pose.time; + const transform::Rigid3d& oldest_pose = oldest_timed_pose.pose; + const double queue_delta = common::ToSeconds(newest_time - oldest_time); + if (queue_delta < 0.001) { // 1 ms + LOG(WARNING) << "Queue too short for extrapolation, returning most recent " + "pose. Queue duration: " + << queue_delta << " ms"; + return newest_pose; + } + const Eigen::Vector3d linear_velocity = + (newest_pose.translation() - oldest_pose.translation()) / queue_delta; + const Eigen::Vector3d angular_velocity = + transform::RotationQuaternionToAngleAxisVector( + oldest_pose.rotation().inverse() * newest_pose.rotation()) / + queue_delta; + const double extrapolation_delta = common::ToSeconds(time - newest_time); + return transform::Rigid3d::Translation(extrapolation_delta * + linear_velocity) * + newest_pose * + transform::Rigid3d::Rotation( + transform::AngleAxisVectorToRotationQuaternion( + Eigen::Vector3d(extrapolation_delta * angular_velocity))); +} + +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/pose_extrapolator.h b/cartographer/mapping/pose_extrapolator.h new file mode 100644 index 0000000..850f88d --- /dev/null +++ b/cartographer/mapping/pose_extrapolator.h @@ -0,0 +1,59 @@ +/* + * Copyright 2017 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_POSE_EXTRAPOLATOR_H_ +#define CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_H_ + +#include + +#include "cartographer/common/time.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. +// +// TODO(whess): Add IMU data and odometry and provide improved extrapolation +// models making use of all available data. +class PoseExtrapolator { + public: + explicit PoseExtrapolator(common::Duration pose_queue_duration); + + PoseExtrapolator(const PoseExtrapolator&) = delete; + PoseExtrapolator& operator=(const PoseExtrapolator&) = delete; + + // Returns the time of the last added pose or Time::min() if no pose was added + // yet. + common::Time GetLastPoseTime() const; + + void AddPose(common::Time time, const transform::Rigid3d& pose); + transform::Rigid3d ExtrapolatePose(common::Time time); + + private: + const common::Duration pose_queue_duration_; + struct TimedPose { + common::Time time; + transform::Rigid3d pose; + }; + std::deque timed_pose_queue_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_H_ diff --git a/cartographer/mapping/pose_graph_trimmer_test.cc b/cartographer/mapping/pose_graph_trimmer_test.cc index a3044a2..8c63bfb 100644 --- a/cartographer/mapping/pose_graph_trimmer_test.cc +++ b/cartographer/mapping/pose_graph_trimmer_test.cc @@ -29,7 +29,9 @@ class FakePoseGraph : public Trimmable { public: ~FakePoseGraph() override {} - int num_submaps(int trajectory_id) const override { return 17 - trimmed_submaps_.size(); } + int num_submaps(int trajectory_id) const override { + return 17 - trimmed_submaps_.size(); + } void MarkSubmapAsTrimmed(const SubmapId& submap_id) override { trimmed_submaps_.push_back(submap_id);