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 <mschworer@lyft.com>
master
Martin Schwörer 2020-07-29 19:15:15 +02:00 committed by GitHub
parent 23351be571
commit efc64934d6
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
11 changed files with 69 additions and 34 deletions

View File

@ -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<PoseExtrapolator>(
::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());
}

View File

@ -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(

View File

@ -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<LocalTrajectoryBuilder3D::MatchingResult>

View File

@ -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<scan_matching::CeresScanMatcher3D> ceres_scan_matcher_;
std::unique_ptr<mapping::PoseExtrapolator> extrapolator_;
std::unique_ptr<mapping::PoseExtrapolatorInterface> extrapolator_;
int num_accumulated_ = 0;
sensor::RangeData accumulated_range_data_;

View File

@ -97,6 +97,13 @@ 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.,

View File

@ -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(

View File

@ -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<sensor::ImuData>& 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());
options.constant_velocity().imu_gravity_time_constant(), imu_data.back());
}
}
}
} // namespace mapping
} // namespace cartographer

View File

@ -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;
@ -63,7 +65,10 @@ message LocalTrajectoryBuilderOptions2D {
// 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;

View File

@ -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;
@ -57,10 +58,14 @@ message LocalTrajectoryBuilderOptions3D {
// 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;
}

View File

@ -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,

View File

@ -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.,