12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc
View File

@ -108,8 +108,18 @@ void LocalTrajectoryBuilder3D::AddImuData(const sensor::ImuData& imu_data) {
extrapolator_->AddImuData(imu_data);
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(
options_.pose_extrapolator_options(), {imu_data});
options_.pose_extrapolator_options(), initial_imu_data, initial_poses);
}
std::unique_ptr<LocalTrajectoryBuilder3D::MatchingResult>

16
cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc
View File

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

47
cartographer/mapping/pose_extrapolator_interface.cc
View File

@ -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>
PoseExtrapolatorInterface::CreateWithImuData(
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());
// 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

6
cartographer/mapping/pose_extrapolator_interface.h
View File

@ -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<PoseExtrapolatorInterface> CreateWithImuData(
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
// yet.

7
cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.proto
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/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;
@ -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;
}

16
configuration_files/trajectory_builder_2d.lua
View File

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

17
configuration_files/trajectory_builder_3d.lua
View File

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