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 = {