From 340a9ac21a5a7f77febcbec3d3ffa312b5c3063e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20Schw=C3=B6rer?= Date: Tue, 28 Jul 2020 22:53:12 +0200 Subject: [PATCH] add pose extrapolator interface and extrapolator options (#1726) This adds an interface around the extrapolator without further integration into any call sides. Currently there is only a constant-velocity extrapolator implementation. The next step is to add an extrapolator implementation which integrates imu measurements for pose extrapolation. Signed-off-by: mschworer --- cartographer/mapping/pose_extrapolator.cc | 16 ++++ cartographer/mapping/pose_extrapolator.h | 20 +++-- .../mapping/pose_extrapolator_interface.cc | 61 ++++++++++++++ .../mapping/pose_extrapolator_interface.h | 80 +++++++++++++++++++ .../proto/pose_extrapolator_options.proto | 32 ++++++++ 5 files changed, 201 insertions(+), 8 deletions(-) create mode 100644 cartographer/mapping/pose_extrapolator_interface.cc create mode 100644 cartographer/mapping/pose_extrapolator_interface.h create mode 100644 cartographer/mapping/proto/pose_extrapolator_options.proto diff --git a/cartographer/mapping/pose_extrapolator.cc b/cartographer/mapping/pose_extrapolator.cc index d02fd5b..24e3800 100644 --- a/cartographer/mapping/pose_extrapolator.cc +++ b/cartographer/mapping/pose_extrapolator.cc @@ -242,5 +242,21 @@ Eigen::Vector3d PoseExtrapolator::ExtrapolateTranslation(common::Time time) { return extrapolation_delta * linear_velocity_from_odometry_; } +PoseExtrapolator::ExtrapolationResult +PoseExtrapolator::ExtrapolatePosesWithGravity( + const std::vector& times) { + std::vector poses; + for (auto it = times.begin(); it != std::prev(times.end()); ++it) { + poses.push_back(ExtrapolatePose(*it).cast()); + } + + const Eigen::Vector3d current_velocity = odometry_data_.size() < 2 + ? linear_velocity_from_poses_ + : linear_velocity_from_odometry_; + return ExtrapolationResult{poses, ExtrapolatePose(times.back()), + current_velocity, + EstimateGravityOrientation(times.back())}; +} + } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/pose_extrapolator.h b/cartographer/mapping/pose_extrapolator.h index 0b14c12..1c6885d 100644 --- a/cartographer/mapping/pose_extrapolator.h +++ b/cartographer/mapping/pose_extrapolator.h @@ -22,6 +22,7 @@ #include "cartographer/common/time.h" #include "cartographer/mapping/imu_tracker.h" +#include "cartographer/mapping/pose_extrapolator_interface.h" #include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/odometry_data.h" #include "cartographer/transform/rigid_transform.h" @@ -32,7 +33,7 @@ namespace mapping { // 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 { +class PoseExtrapolator : public PoseExtrapolatorInterface { public: explicit PoseExtrapolator(common::Duration pose_queue_duration, double imu_gravity_time_constant); @@ -46,17 +47,20 @@ class PoseExtrapolator { // Returns the time of the last added pose or Time::min() if no pose was added // yet. - common::Time GetLastPoseTime() const; - common::Time GetLastExtrapolatedTime() const; + common::Time GetLastPoseTime() const override; + common::Time GetLastExtrapolatedTime() const override; - 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); + void AddPose(common::Time time, const transform::Rigid3d& pose) override; + void AddImuData(const sensor::ImuData& imu_data) override; + void AddOdometryData(const sensor::OdometryData& odometry_data) override; + transform::Rigid3d ExtrapolatePose(common::Time time) override; + + ExtrapolationResult ExtrapolatePosesWithGravity( + const std::vector& times) override; // Returns the current gravity alignment estimate as a rotation from // the tracking frame into a gravity aligned frame. - Eigen::Quaterniond EstimateGravityOrientation(common::Time time); + Eigen::Quaterniond EstimateGravityOrientation(common::Time time) override; private: void UpdateVelocitiesFromPoses(); diff --git a/cartographer/mapping/pose_extrapolator_interface.cc b/cartographer/mapping/pose_extrapolator_interface.cc new file mode 100644 index 0000000..a563e7b --- /dev/null +++ b/cartographer/mapping/pose_extrapolator_interface.cc @@ -0,0 +1,61 @@ +/* + * Copyright 2018 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_interface.h" + +#include "cartographer/common/time.h" +#include "cartographer/mapping/pose_extrapolator.h" + +namespace cartographer { +namespace mapping { + +namespace { + +proto::ConstantVelocityPoseExtrapolatorOptions +CreateConstantVelocityPoseExtrapolatorOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::ConstantVelocityPoseExtrapolatorOptions options; + options.set_pose_queue_duration( + parameter_dictionary->GetDouble("pose_queue_duration")); + options.set_imu_gravity_time_constant( + parameter_dictionary->GetDouble("imu_gravity_time_constant")); + return options; +} + +} + +proto::PoseExtrapolatorOptions CreatePoseExtrapolatorOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::PoseExtrapolatorOptions options; + *options.mutable_constant_velocity() = + CreateConstantVelocityPoseExtrapolatorOptions( + parameter_dictionary->GetDictionary("constant_velocity").get()); + return options; +} + +std::unique_ptr +PoseExtrapolatorInterface::CreateWithImuData( + const proto::PoseExtrapolatorOptions& options, + const std::vector& imu_data) { + CHECK(!imu_data.empty()); + return PoseExtrapolator::InitializeWithImu( + common::FromSeconds(options.constant_velocity().pose_queue_duration()), + options.constant_velocity().imu_gravity_time_constant(), + imu_data.back()); +} + +} +} diff --git a/cartographer/mapping/pose_extrapolator_interface.h b/cartographer/mapping/pose_extrapolator_interface.h new file mode 100644 index 0000000..6f470db --- /dev/null +++ b/cartographer/mapping/pose_extrapolator_interface.h @@ -0,0 +1,80 @@ +/* + * Copyright 2018 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_INTERFACE_H_ +#define CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_INTERFACE_H_ + +#include +#include + +#include "cartographer/common/time.h" +#include "cartographer/mapping/proto/pose_extrapolator_options.pb.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/odometry_data.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/timestamped_transform.h" + +namespace cartographer { +namespace mapping { + +proto::PoseExtrapolatorOptions CreatePoseExtrapolatorOptions( + common::LuaParameterDictionary* const parameter_dictionary); + +class PoseExtrapolatorInterface { + public: + struct ExtrapolationResult { + // The poses for the requested times at index 0 to N-1. + std::vector previous_poses; + // The pose for the requested time at index N. + transform::Rigid3d current_pose; + Eigen::Vector3d current_velocity; + Eigen::Quaterniond gravity_from_tracking; + }; + + PoseExtrapolatorInterface(const PoseExtrapolatorInterface&) = delete; + PoseExtrapolatorInterface& operator=(const PoseExtrapolatorInterface&) = delete; + virtual ~PoseExtrapolatorInterface() {} + + // TODO: Remove dependency cycle. + static std::unique_ptr CreateWithImuData( + const proto::PoseExtrapolatorOptions& options, + const std::vector& imu_data); + + // Returns the time of the last added pose or Time::min() if no pose was added + // yet. + virtual common::Time GetLastPoseTime() const = 0; + virtual common::Time GetLastExtrapolatedTime() const = 0; + + virtual void AddPose(common::Time time, const transform::Rigid3d& pose) = 0; + virtual void AddImuData(const sensor::ImuData& imu_data) = 0; + virtual void AddOdometryData(const sensor::OdometryData& odometry_data) = 0; + virtual transform::Rigid3d ExtrapolatePose(common::Time time) = 0; + + virtual ExtrapolationResult ExtrapolatePosesWithGravity( + const std::vector& times) = 0; + + // Returns the current gravity alignment estimate as a rotation from + // the tracking frame into a gravity aligned frame. + virtual Eigen::Quaterniond EstimateGravityOrientation(common::Time time) = 0; + + protected: + PoseExtrapolatorInterface() {} +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_INTERFACE_H_ diff --git a/cartographer/mapping/proto/pose_extrapolator_options.proto b/cartographer/mapping/proto/pose_extrapolator_options.proto new file mode 100644 index 0000000..ede5fd8 --- /dev/null +++ b/cartographer/mapping/proto/pose_extrapolator_options.proto @@ -0,0 +1,32 @@ +// Copyright 2018 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. + +syntax = "proto3"; + +package cartographer.mapping.proto; + +message ConstantVelocityPoseExtrapolatorOptions { + // Time constant in seconds for the orientation moving average based on + // observed gravity via the IMU. It should be chosen so that the error + // 1. from acceleration measurements not due to gravity (which gets worse when + // the constant is reduced) and + // 2. from integration of angular velocities (which gets worse when the + // constant is increased) is balanced. + double imu_gravity_time_constant = 1; + double pose_queue_duration = 2; +} + +message PoseExtrapolatorOptions { + ConstantVelocityPoseExtrapolatorOptions constant_velocity = 2; +}