From 6c889490e245cc5d9da15023249c6fc7119def3f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20Schw=C3=B6rer?= Date: Thu, 30 Jul 2020 13:52:08 +0200 Subject: [PATCH] Integrate imu based extrapolator (#1732) This integrates the imu based extrapolator implementation, adds lua configuration and conversions from lua to proto. Note that the parameters of the imu based extrapolator are not tuned yet and the default is still the constant-velocity extrapolator. Signed-off-by: mschworer --- .../3d/local_trajectory_builder_3d.cc | 12 ++++- .../3d/local_trajectory_builder_3d_test.cc | 16 +++++++ .../mapping/pose_extrapolator_interface.cc | 47 +++++++++++++++++-- .../mapping/pose_extrapolator_interface.h | 6 ++- .../local_trajectory_builder_options_2d.proto | 12 ++--- .../local_trajectory_builder_options_3d.proto | 19 +++++--- configuration_files/trajectory_builder_2d.lua | 16 +++++++ configuration_files/trajectory_builder_3d.lua | 17 +++++++ 8 files changed, 125 insertions(+), 20 deletions(-) diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc index 3f34789..07c5aec 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc @@ -108,8 +108,18 @@ void LocalTrajectoryBuilder3D::AddImuData(const sensor::ImuData& imu_data) { extrapolator_->AddImuData(imu_data); return; } + std::vector initial_poses; + for (const auto& pose_proto : options_.initial_poses()) { + initial_poses.push_back(transform::FromProto(pose_proto)); + } + std::vector initial_imu_data; + for (const auto& imu : options_.initial_imu_data()) { + initial_imu_data.push_back(sensor::FromProto(imu)); + } + initial_imu_data.push_back(imu_data); + extrapolator_ = mapping::PoseExtrapolatorInterface::CreateWithImuData( - options_.pose_extrapolator_options(), {imu_data}); + options_.pose_extrapolator_options(), initial_imu_data, initial_poses); } std::unique_ptr 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 0558b0d..22ec9c9 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc @@ -98,10 +98,26 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { rotational_histogram_size = 120, pose_extrapolator = { + use_imu_based = false, constant_velocity = { imu_gravity_time_constant = 10., pose_queue_duration = 0.001, }, + imu_based = { + pose_queue_duration = 5., + gravity_constant = 9.806, + pose_translation_weight = 1., + pose_rotation_weight = 1., + imu_acceleration_weight = 1., + imu_rotation_weight = 1., + odometry_translation_weight = 1., + odometry_rotation_weight = 1., + solver_options = { + use_nonmonotonic_steps = false; + max_num_iterations = 10; + num_threads = 1; + }, + }, }, submaps = { diff --git a/cartographer/mapping/pose_extrapolator_interface.cc b/cartographer/mapping/pose_extrapolator_interface.cc index 32a5abd..413a8da 100644 --- a/cartographer/mapping/pose_extrapolator_interface.cc +++ b/cartographer/mapping/pose_extrapolator_interface.cc @@ -16,7 +16,9 @@ #include "cartographer/mapping/pose_extrapolator_interface.h" +#include "cartographer/common/ceres_solver_options.h" #include "cartographer/common/time.h" +#include "cartographer/mapping/imu_based_pose_extrapolator.h" #include "cartographer/mapping/pose_extrapolator.h" namespace cartographer { @@ -35,27 +37,64 @@ CreateConstantVelocityPoseExtrapolatorOptions( return options; } +proto::ImuBasedPoseExtrapolatorOptions CreateImuBasedPoseExtrapolatorOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::ImuBasedPoseExtrapolatorOptions options; + + options.set_pose_queue_duration( + parameter_dictionary->GetDouble("pose_queue_duration")); + options.set_gravity_constant( + parameter_dictionary->GetDouble("gravity_constant")); + options.set_pose_translation_weight( + parameter_dictionary->GetDouble("pose_translation_weight")); + options.set_pose_rotation_weight( + parameter_dictionary->GetDouble("pose_rotation_weight")); + options.set_imu_acceleration_weight( + parameter_dictionary->GetDouble("imu_acceleration_weight")); + options.set_imu_rotation_weight( + parameter_dictionary->GetDouble("imu_rotation_weight")); + options.set_odometry_rotation_weight( + parameter_dictionary->GetDouble("odometry_rotation_weight")); + options.set_odometry_translation_weight( + parameter_dictionary->GetDouble("odometry_translation_weight")); + *options.mutable_solver_options() = CreateCeresSolverOptionsProto( + parameter_dictionary->GetDictionary("solver_options").get()); + + return options; +} + } // namespace proto::PoseExtrapolatorOptions CreatePoseExtrapolatorOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::PoseExtrapolatorOptions options; + options.set_use_imu_based(parameter_dictionary->GetBool("use_imu_based")); *options.mutable_constant_velocity() = CreateConstantVelocityPoseExtrapolatorOptions( parameter_dictionary->GetDictionary("constant_velocity").get()); + *options.mutable_imu_based() = CreateImuBasedPoseExtrapolatorOptions( + parameter_dictionary->GetDictionary("imu_based").get()); + return options; } std::unique_ptr PoseExtrapolatorInterface::CreateWithImuData( const proto::PoseExtrapolatorOptions& options, - const std::vector& imu_data) { + const std::vector& imu_data, + const std::vector& initial_poses) { 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()); + if (options.use_imu_based()) { + return ImuBasedPoseExtrapolator::InitializeWithImu(options.imu_based(), + imu_data, initial_poses); + } else { + return PoseExtrapolator::InitializeWithImu( + common::FromSeconds(options.constant_velocity().pose_queue_duration()), + options.constant_velocity().imu_gravity_time_constant(), + imu_data.back()); + } } } // namespace mapping diff --git a/cartographer/mapping/pose_extrapolator_interface.h b/cartographer/mapping/pose_extrapolator_interface.h index 6f470db..bf258ce 100644 --- a/cartographer/mapping/pose_extrapolator_interface.h +++ b/cartographer/mapping/pose_extrapolator_interface.h @@ -45,13 +45,15 @@ class PoseExtrapolatorInterface { }; PoseExtrapolatorInterface(const PoseExtrapolatorInterface&) = delete; - PoseExtrapolatorInterface& operator=(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); + const std::vector& imu_data, + const std::vector& initial_poses); // Returns the time of the last added pose or Time::min() if no pose was added // yet. 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 0005c51..470dfe8 100644 --- a/cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.proto +++ b/cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.proto @@ -60,12 +60,12 @@ 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. - // TODO(schwoere,wohe): Remove this constant. This is only used for ROS and + // 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; 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 674189b..7039ddf 100644 --- a/cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.proto +++ b/cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.proto @@ -22,8 +22,10 @@ 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"; +import "cartographer/sensor/proto/sensor.proto"; +import "cartographer/transform/proto/timestamped_transform.proto"; -// NEXT ID: 19 +// NEXT ID: 21 message LocalTrajectoryBuilderOptions3D { // Rangefinder points outside these ranges will be dropped. float min_range = 1; @@ -53,12 +55,12 @@ 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. - // TODO(schwoere,wohe): Remove this constant. This is only used for ROS and + // 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; @@ -67,5 +69,8 @@ message LocalTrajectoryBuilderOptions3D { mapping.proto.PoseExtrapolatorOptions pose_extrapolator_options = 18; + repeated transform.proto.TimestampedTransform initial_poses = 19; + repeated sensor.proto.ImuData initial_imu_data = 20; + SubmapsOptions3D submaps_options = 8; } diff --git a/configuration_files/trajectory_builder_2d.lua b/configuration_files/trajectory_builder_2d.lua index 386e874..373b099 100644 --- a/configuration_files/trajectory_builder_2d.lua +++ b/configuration_files/trajectory_builder_2d.lua @@ -62,10 +62,26 @@ TRAJECTORY_BUILDER_2D = { -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS. imu_gravity_time_constant = 10., pose_extrapolator = { + use_imu_based = false, constant_velocity = { imu_gravity_time_constant = 10., pose_queue_duration = 0.001, }, + imu_based = { + pose_queue_duration = 5., + gravity_constant = 9.806, + pose_translation_weight = 1., + pose_rotation_weight = 1., + imu_acceleration_weight = 1., + imu_rotation_weight = 1., + odometry_translation_weight = 1., + odometry_rotation_weight = 1., + solver_options = { + use_nonmonotonic_steps = false; + max_num_iterations = 10; + num_threads = 1; + }, + }, }, submaps = { diff --git a/configuration_files/trajectory_builder_3d.lua b/configuration_files/trajectory_builder_3d.lua index 1195576..3bd3dd8 100644 --- a/configuration_files/trajectory_builder_3d.lua +++ b/configuration_files/trajectory_builder_3d.lua @@ -64,10 +64,27 @@ TRAJECTORY_BUILDER_3D = { -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS. imu_gravity_time_constant = 10., pose_extrapolator = { + use_imu_based = false, constant_velocity = { imu_gravity_time_constant = 10., pose_queue_duration = 0.001, }, + -- TODO(wohe): Tune these parameters on the example datasets. + imu_based = { + pose_queue_duration = 5., + gravity_constant = 9.806, + pose_translation_weight = 1., + pose_rotation_weight = 1., + imu_acceleration_weight = 1., + imu_rotation_weight = 1., + odometry_translation_weight = 1., + odometry_rotation_weight = 1., + solver_options = { + use_nonmonotonic_steps = false; + max_num_iterations = 10; + num_threads = 1; + }, + }, }, submaps = {