From efc64934d640f3f7561693b09feb3c6b5b0e0fe7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20Schw=C3=B6rer?= Date: Wed, 29 Jul 2020 19:15:15 +0200 Subject: [PATCH] use extrapolator interface (#1730) This integrates the extrapolator interface into the 3D trajectory builder. The construction is now done within the interface and local trajectory builder 3D just makes use of the interface. No functional changes. Signed-off-by: mschworer --- .../internal/2d/local_trajectory_builder_2d.cc | 13 +++++++------ .../2d/local_trajectory_builder_options_2d.cc | 3 +++ .../internal/3d/local_trajectory_builder_3d.cc | 9 ++------- .../internal/3d/local_trajectory_builder_3d.h | 4 ++-- .../3d/local_trajectory_builder_3d_test.cc | 9 ++++++++- .../3d/local_trajectory_builder_options_3d.cc | 3 +++ .../mapping/pose_extrapolator_interface.cc | 13 +++++++------ .../local_trajectory_builder_options_2d.proto | 15 ++++++++++----- .../local_trajectory_builder_options_3d.proto | 17 +++++++++++------ configuration_files/trajectory_builder_2d.lua | 7 +++++++ configuration_files/trajectory_builder_3d.lua | 10 +++++++++- 11 files changed, 69 insertions(+), 34 deletions(-) diff --git a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc index 08823e0..55a1e4f 100644 --- a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc +++ b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc @@ -319,14 +319,15 @@ void LocalTrajectoryBuilder2D::InitializeExtrapolator(const common::Time time) { if (extrapolator_ != nullptr) { return; } - // We derive velocities from poses which are at least 1 ms apart for numerical - // stability. Usually poses known to the extrapolator will be further apart - // in time and thus the last two are used. - constexpr double kExtrapolationEstimationTimeSec = 0.001; + CHECK(!options_.pose_extrapolator_options().use_imu_based()); // TODO(gaschler): Consider using InitializeWithImu as 3D does. extrapolator_ = absl::make_unique( - ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec), - options_.imu_gravity_time_constant()); + ::cartographer::common::FromSeconds(options_.pose_extrapolator_options() + .constant_velocity() + .pose_queue_duration()), + options_.pose_extrapolator_options() + .constant_velocity() + .imu_gravity_time_constant()); extrapolator_->AddPose(time, transform::Rigid3d::Identity()); } diff --git a/cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.cc b/cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.cc index 552e0d5..434da3d 100644 --- a/cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.cc +++ b/cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.cc @@ -20,6 +20,7 @@ #include "cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h" #include "cartographer/mapping/internal/motion_filter.h" #include "cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h" +#include "cartographer/mapping/pose_extrapolator_interface.h" #include "cartographer/sensor/internal/voxel_filter.h" namespace cartographer { @@ -58,6 +59,8 @@ proto::LocalTrajectoryBuilderOptions2D CreateLocalTrajectoryBuilderOptions2D( parameter_dictionary->GetDictionary("ceres_scan_matcher").get()); *options.mutable_motion_filter_options() = mapping::CreateMotionFilterOptions( parameter_dictionary->GetDictionary("motion_filter").get()); + *options.mutable_pose_extrapolator_options() = CreatePoseExtrapolatorOptions( + parameter_dictionary->GetDictionary("pose_extrapolator").get()); options.set_imu_gravity_time_constant( parameter_dictionary->GetDouble("imu_gravity_time_constant")); *options.mutable_submaps_options() = CreateSubmapsOptions2D( diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc index 5cd26ab..1f670e2 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc @@ -108,13 +108,8 @@ void LocalTrajectoryBuilder3D::AddImuData(const sensor::ImuData& imu_data) { extrapolator_->AddImuData(imu_data); return; } - // We derive velocities from poses which are at least 1 ms apart for numerical - // stability. Usually poses known to the extrapolator will be further apart - // in time and thus the last two are used. - constexpr double kExtrapolationEstimationTimeSec = 0.001; - extrapolator_ = mapping::PoseExtrapolator::InitializeWithImu( - ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec), - options_.imu_gravity_time_constant(), imu_data); + extrapolator_ = mapping::PoseExtrapolatorInterface::CreateWithImuData( + options_.pose_extrapolator_options(), {imu_data}); } std::unique_ptr diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h index 2abeee8..b0ee05b 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h @@ -26,7 +26,7 @@ #include "cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d.h" #include "cartographer/mapping/internal/motion_filter.h" #include "cartographer/mapping/internal/range_data_collator.h" -#include "cartographer/mapping/pose_extrapolator.h" +#include "cartographer/mapping/pose_extrapolator_interface.h" #include "cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.pb.h" #include "cartographer/metrics/family_factory.h" #include "cartographer/sensor/imu_data.h" @@ -103,7 +103,7 @@ class LocalTrajectoryBuilder3D { real_time_correlative_scan_matcher_; std::unique_ptr ceres_scan_matcher_; - std::unique_ptr extrapolator_; + std::unique_ptr extrapolator_; int num_accumulated_ = 0; sensor::RangeData accumulated_range_data_; diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc index 423ee91..0558b0d 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc @@ -96,7 +96,14 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { imu_gravity_time_constant = 1., rotational_histogram_size = 120, - + + pose_extrapolator = { + constant_velocity = { + imu_gravity_time_constant = 10., + pose_queue_duration = 0.001, + }, + }, + submaps = { high_resolution = 0.2, high_resolution_max_range = 50., diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.cc index 92c9ed1..6dd63ab 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.cc @@ -20,6 +20,7 @@ #include "cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h" #include "cartographer/mapping/internal/motion_filter.h" #include "cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h" +#include "cartographer/mapping/pose_extrapolator_interface.h" #include "cartographer/sensor/internal/voxel_filter.h" #include "glog/logging.h" @@ -57,6 +58,8 @@ proto::LocalTrajectoryBuilderOptions3D CreateLocalTrajectoryBuilderOptions3D( parameter_dictionary->GetDictionary("ceres_scan_matcher").get()); *options.mutable_motion_filter_options() = CreateMotionFilterOptions( parameter_dictionary->GetDictionary("motion_filter").get()); + *options.mutable_pose_extrapolator_options() = CreatePoseExtrapolatorOptions( + parameter_dictionary->GetDictionary("pose_extrapolator").get()); options.set_imu_gravity_time_constant( parameter_dictionary->GetDouble("imu_gravity_time_constant")); options.set_rotational_histogram_size( diff --git a/cartographer/mapping/pose_extrapolator_interface.cc b/cartographer/mapping/pose_extrapolator_interface.cc index a563e7b..32a5abd 100644 --- a/cartographer/mapping/pose_extrapolator_interface.cc +++ b/cartographer/mapping/pose_extrapolator_interface.cc @@ -35,7 +35,7 @@ CreateConstantVelocityPoseExtrapolatorOptions( return options; } -} +} // namespace proto::PoseExtrapolatorOptions CreatePoseExtrapolatorOptions( common::LuaParameterDictionary* const parameter_dictionary) { @@ -51,11 +51,12 @@ PoseExtrapolatorInterface::CreateWithImuData( const proto::PoseExtrapolatorOptions& options, const std::vector& imu_data) { CHECK(!imu_data.empty()); + // TODO(schwoere): Implement/integrate imu based extrapolator. + CHECK(!options.use_imu_based()) << "Not implemented!"; return PoseExtrapolator::InitializeWithImu( - common::FromSeconds(options.constant_velocity().pose_queue_duration()), - options.constant_velocity().imu_gravity_time_constant(), - imu_data.back()); + common::FromSeconds(options.constant_velocity().pose_queue_duration()), + options.constant_velocity().imu_gravity_time_constant(), imu_data.back()); } -} -} +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.proto b/cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.proto index 2189091..0005c51 100644 --- a/cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.proto +++ b/cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.proto @@ -17,11 +17,13 @@ syntax = "proto3"; package cartographer.mapping.proto; import "cartographer/mapping/proto/motion_filter_options.proto"; +import "cartographer/mapping/proto/pose_extrapolator_options.proto"; import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto"; import "cartographer/mapping/proto/2d/submaps_options_2d.proto"; import "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_2d.proto"; import "cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.proto"; +// NEXT ID: 22 message LocalTrajectoryBuilderOptions2D { // Rangefinder points outside these ranges will be dropped. float min_range = 14; @@ -58,12 +60,15 @@ message LocalTrajectoryBuilderOptions2D { MotionFilterOptions motion_filter_options = 13; // 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. + // 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. + // TODO(schwoere,wohe): Remove this constant. This is only used for ROS and + // was replaced by pose_extrapolator_options. double imu_gravity_time_constant = 17; + mapping.proto.PoseExtrapolatorOptions pose_extrapolator_options = 21; SubmapsOptions2D submaps_options = 11; diff --git a/cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.proto b/cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.proto index 5bdd4f2..674189b 100644 --- a/cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.proto +++ b/cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.proto @@ -18,11 +18,12 @@ package cartographer.mapping.proto; import "cartographer/mapping/proto/3d/submaps_options_3d.proto"; import "cartographer/mapping/proto/motion_filter_options.proto"; +import "cartographer/mapping/proto/pose_extrapolator_options.proto"; import "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.proto"; import "cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.proto"; import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto"; -// NEXT ID: 18 +// NEXT ID: 19 message LocalTrajectoryBuilderOptions3D { // Rangefinder points outside these ranges will be dropped. float min_range = 1; @@ -52,15 +53,19 @@ message LocalTrajectoryBuilderOptions3D { mapping.proto.MotionFilterOptions motion_filter_options = 7; // 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. + // 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. + // TODO(schwoere,wohe): Remove this constant. This is only used for ROS and + // was replaced by pose_extrapolator_options. double imu_gravity_time_constant = 15; // Number of histogram buckets for the rotational scan matcher. int32 rotational_histogram_size = 17; + mapping.proto.PoseExtrapolatorOptions pose_extrapolator_options = 18; + SubmapsOptions3D submaps_options = 8; } diff --git a/configuration_files/trajectory_builder_2d.lua b/configuration_files/trajectory_builder_2d.lua index 483f47f..386e874 100644 --- a/configuration_files/trajectory_builder_2d.lua +++ b/configuration_files/trajectory_builder_2d.lua @@ -59,7 +59,14 @@ TRAJECTORY_BUILDER_2D = { max_angle_radians = math.rad(1.), }, + -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS. imu_gravity_time_constant = 10., + pose_extrapolator = { + constant_velocity = { + imu_gravity_time_constant = 10., + pose_queue_duration = 0.001, + }, + }, submaps = { num_range_data = 90, diff --git a/configuration_files/trajectory_builder_3d.lua b/configuration_files/trajectory_builder_3d.lua index 49567f4..1195576 100644 --- a/configuration_files/trajectory_builder_3d.lua +++ b/configuration_files/trajectory_builder_3d.lua @@ -59,9 +59,17 @@ TRAJECTORY_BUILDER_3D = { max_angle_radians = 0.004, }, - imu_gravity_time_constant = 10., rotational_histogram_size = 120, + -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS. + imu_gravity_time_constant = 10., + pose_extrapolator = { + constant_velocity = { + imu_gravity_time_constant = 10., + pose_queue_duration = 0.001, + }, + }, + submaps = { high_resolution = 0.10, high_resolution_max_range = 20.,