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 <mschworer@lyft.com>
master
Martin Schwörer 2020-07-30 13:52:08 +02:00 committed by GitHub
parent d3794a420a
commit 6c889490e2
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
8 changed files with 125 additions and 20 deletions

View File

@ -108,8 +108,18 @@ void LocalTrajectoryBuilder3D::AddImuData(const sensor::ImuData& imu_data) {
extrapolator_->AddImuData(imu_data); extrapolator_->AddImuData(imu_data);
return; return;
} }
std::vector<transform::TimestampedTransform> initial_poses;
for (const auto& pose_proto : options_.initial_poses()) {
initial_poses.push_back(transform::FromProto(pose_proto));
}
std::vector<sensor::ImuData> 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( extrapolator_ = mapping::PoseExtrapolatorInterface::CreateWithImuData(
options_.pose_extrapolator_options(), {imu_data}); options_.pose_extrapolator_options(), initial_imu_data, initial_poses);
} }
std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult> std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>

View File

@ -98,10 +98,26 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
rotational_histogram_size = 120, rotational_histogram_size = 120,
pose_extrapolator = { pose_extrapolator = {
use_imu_based = false,
constant_velocity = { constant_velocity = {
imu_gravity_time_constant = 10., imu_gravity_time_constant = 10.,
pose_queue_duration = 0.001, 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 = { submaps = {

View File

@ -16,7 +16,9 @@
#include "cartographer/mapping/pose_extrapolator_interface.h" #include "cartographer/mapping/pose_extrapolator_interface.h"
#include "cartographer/common/ceres_solver_options.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/imu_based_pose_extrapolator.h"
#include "cartographer/mapping/pose_extrapolator.h" #include "cartographer/mapping/pose_extrapolator.h"
namespace cartographer { namespace cartographer {
@ -35,27 +37,64 @@ CreateConstantVelocityPoseExtrapolatorOptions(
return options; 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 } // namespace
proto::PoseExtrapolatorOptions CreatePoseExtrapolatorOptions( proto::PoseExtrapolatorOptions CreatePoseExtrapolatorOptions(
common::LuaParameterDictionary* const parameter_dictionary) { common::LuaParameterDictionary* const parameter_dictionary) {
proto::PoseExtrapolatorOptions options; proto::PoseExtrapolatorOptions options;
options.set_use_imu_based(parameter_dictionary->GetBool("use_imu_based"));
*options.mutable_constant_velocity() = *options.mutable_constant_velocity() =
CreateConstantVelocityPoseExtrapolatorOptions( CreateConstantVelocityPoseExtrapolatorOptions(
parameter_dictionary->GetDictionary("constant_velocity").get()); parameter_dictionary->GetDictionary("constant_velocity").get());
*options.mutable_imu_based() = CreateImuBasedPoseExtrapolatorOptions(
parameter_dictionary->GetDictionary("imu_based").get());
return options; return options;
} }
std::unique_ptr<PoseExtrapolatorInterface> std::unique_ptr<PoseExtrapolatorInterface>
PoseExtrapolatorInterface::CreateWithImuData( PoseExtrapolatorInterface::CreateWithImuData(
const proto::PoseExtrapolatorOptions& options, const proto::PoseExtrapolatorOptions& options,
const std::vector<sensor::ImuData>& imu_data) { const std::vector<sensor::ImuData>& imu_data,
const std::vector<transform::TimestampedTransform>& initial_poses) {
CHECK(!imu_data.empty()); CHECK(!imu_data.empty());
// TODO(schwoere): Implement/integrate imu based extrapolator. // TODO(schwoere): Implement/integrate imu based extrapolator.
CHECK(!options.use_imu_based()) << "Not implemented!"; CHECK(!options.use_imu_based()) << "Not implemented!";
return PoseExtrapolator::InitializeWithImu( if (options.use_imu_based()) {
common::FromSeconds(options.constant_velocity().pose_queue_duration()), return ImuBasedPoseExtrapolator::InitializeWithImu(options.imu_based(),
options.constant_velocity().imu_gravity_time_constant(), imu_data.back()); 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 } // namespace mapping

View File

@ -45,13 +45,15 @@ class PoseExtrapolatorInterface {
}; };
PoseExtrapolatorInterface(const PoseExtrapolatorInterface&) = delete; PoseExtrapolatorInterface(const PoseExtrapolatorInterface&) = delete;
PoseExtrapolatorInterface& operator=(const PoseExtrapolatorInterface&) = delete; PoseExtrapolatorInterface& operator=(const PoseExtrapolatorInterface&) =
delete;
virtual ~PoseExtrapolatorInterface() {} virtual ~PoseExtrapolatorInterface() {}
// TODO: Remove dependency cycle. // TODO: Remove dependency cycle.
static std::unique_ptr<PoseExtrapolatorInterface> CreateWithImuData( static std::unique_ptr<PoseExtrapolatorInterface> CreateWithImuData(
const proto::PoseExtrapolatorOptions& options, const proto::PoseExtrapolatorOptions& options,
const std::vector<sensor::ImuData>& imu_data); const std::vector<sensor::ImuData>& imu_data,
const std::vector<transform::TimestampedTransform>& initial_poses);
// Returns the time of the last added pose or Time::min() if no pose was added // Returns the time of the last added pose or Time::min() if no pose was added
// yet. // yet.

View File

@ -60,12 +60,12 @@ message LocalTrajectoryBuilderOptions2D {
MotionFilterOptions motion_filter_options = 13; MotionFilterOptions motion_filter_options = 13;
// Time constant in seconds for the orientation moving average based on // Time constant in seconds for the orientation moving average based on
// observed gravity via the IMU. It should be chosen so that the error // 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 // 1. from acceleration measurements not due to gravity (which gets worse when
// the constant is reduced) and // the constant is reduced) and
// 2. from integration of angular velocities (which gets worse when the // 2. from integration of angular velocities (which gets worse when the
// constant is increased) is balanced. // constant is increased) is balanced.
// TODO(schwoere,wohe): Remove this constant. This is only used for ROS and // TODO(schwoere,wohe): Remove this constant. This is only used for ROS and
// was replaced by pose_extrapolator_options. // was replaced by pose_extrapolator_options.
double imu_gravity_time_constant = 17; double imu_gravity_time_constant = 17;
mapping.proto.PoseExtrapolatorOptions pose_extrapolator_options = 21; mapping.proto.PoseExtrapolatorOptions pose_extrapolator_options = 21;

View File

@ -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/ceres_scan_matcher_options_3d.proto";
import "cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.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/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 { message LocalTrajectoryBuilderOptions3D {
// Rangefinder points outside these ranges will be dropped. // Rangefinder points outside these ranges will be dropped.
float min_range = 1; float min_range = 1;
@ -53,12 +55,12 @@ message LocalTrajectoryBuilderOptions3D {
mapping.proto.MotionFilterOptions motion_filter_options = 7; mapping.proto.MotionFilterOptions motion_filter_options = 7;
// Time constant in seconds for the orientation moving average based on // Time constant in seconds for the orientation moving average based on
// observed gravity via the IMU. It should be chosen so that the error // 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 // 1. from acceleration measurements not due to gravity (which gets worse when
// the constant is reduced) and // the constant is reduced) and
// 2. from integration of angular velocities (which gets worse when the // 2. from integration of angular velocities (which gets worse when the
// constant is increased) is balanced. // constant is increased) is balanced.
// TODO(schwoere,wohe): Remove this constant. This is only used for ROS and // TODO(schwoere,wohe): Remove this constant. This is only used for ROS and
// was replaced by pose_extrapolator_options. // was replaced by pose_extrapolator_options.
double imu_gravity_time_constant = 15; double imu_gravity_time_constant = 15;
@ -67,5 +69,8 @@ message LocalTrajectoryBuilderOptions3D {
mapping.proto.PoseExtrapolatorOptions pose_extrapolator_options = 18; 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; SubmapsOptions3D submaps_options = 8;
} }

View File

@ -62,10 +62,26 @@ TRAJECTORY_BUILDER_2D = {
-- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS. -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS.
imu_gravity_time_constant = 10., imu_gravity_time_constant = 10.,
pose_extrapolator = { pose_extrapolator = {
use_imu_based = false,
constant_velocity = { constant_velocity = {
imu_gravity_time_constant = 10., imu_gravity_time_constant = 10.,
pose_queue_duration = 0.001, 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 = { submaps = {

View File

@ -64,10 +64,27 @@ TRAJECTORY_BUILDER_3D = {
-- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS. -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS.
imu_gravity_time_constant = 10., imu_gravity_time_constant = 10.,
pose_extrapolator = { pose_extrapolator = {
use_imu_based = false,
constant_velocity = { constant_velocity = {
imu_gravity_time_constant = 10., imu_gravity_time_constant = 10.,
pose_queue_duration = 0.001, 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 = { submaps = {