From 1bce14b7b57b14259fee65a9f32ce9bab3758b4c Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Tue, 18 Jul 2017 12:10:40 +0200 Subject: [PATCH] Adds accumulation of range data to 2D. (#408) This mostly follows the implementation as done for 3D. Towards googlecartographer/cartographer_ros#212. --- .../mapping_2d/local_trajectory_builder.cc | 58 +++++++++++++------ .../mapping_2d/local_trajectory_builder.h | 14 ++++- .../local_trajectory_builder_options.cc | 2 + .../local_trajectory_builder_options.proto | 4 ++ .../mapping_3d/local_trajectory_builder.h | 2 +- .../local_trajectory_builder_options.proto | 4 +- configuration_files/trajectory_builder_2d.lua | 1 + 7 files changed, 62 insertions(+), 23 deletions(-) diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index bdf1fd3..c421d1a 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -39,23 +39,8 @@ LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {} sensor::RangeData LocalTrajectoryBuilder::TransformAndFilterRangeData( const transform::Rigid3f& tracking_to_tracking_2d, const sensor::RangeData& range_data) const { - // Drop any returns below the minimum range and convert returns beyond the - // maximum range into misses. - sensor::RangeData returns_and_misses{range_data.origin, {}, {}}; - for (const Eigen::Vector3f& hit : range_data.returns) { - const float range = (hit - range_data.origin).norm(); - if (range >= options_.min_range()) { - if (range <= options_.max_range()) { - returns_and_misses.returns.push_back(hit); - } else { - returns_and_misses.misses.push_back( - range_data.origin + options_.missing_data_ray_length() * - (hit - range_data.origin).normalized()); - } - } - } const sensor::RangeData cropped = sensor::CropRangeData( - sensor::TransformRangeData(returns_and_misses, tracking_to_tracking_2d), + sensor::TransformRangeData(range_data, tracking_to_tracking_2d), options_.min_z(), options_.max_z()); return sensor::RangeData{ cropped.origin, @@ -112,6 +97,45 @@ LocalTrajectoryBuilder::AddHorizontalRangeData( } Predict(time); + if (num_accumulated_ == 0) { + first_pose_estimate_ = pose_estimate_.cast(); + accumulated_range_data_ = + sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}; + } + + const transform::Rigid3f tracking_delta = + first_pose_estimate_.inverse() * pose_estimate_.cast(); + const sensor::RangeData range_data_in_first_tracking = + sensor::TransformRangeData(range_data, tracking_delta); + // Drop any returns below the minimum range and convert returns beyond the + // maximum range into misses. + for (const Eigen::Vector3f& hit : range_data_in_first_tracking.returns) { + const Eigen::Vector3f delta = hit - range_data_in_first_tracking.origin; + const float range = delta.norm(); + if (range >= options_.min_range()) { + if (range <= options_.max_range()) { + accumulated_range_data_.returns.push_back(hit); + } else { + accumulated_range_data_.misses.push_back( + range_data_in_first_tracking.origin + + options_.missing_data_ray_length() / range * delta); + } + } + } + ++num_accumulated_; + + if (num_accumulated_ >= options_.scans_per_accumulation()) { + num_accumulated_ = 0; + return AddAccumulatedRangeData( + time, sensor::TransformRangeData(accumulated_range_data_, + tracking_delta.inverse())); + } + return nullptr; +} + +std::unique_ptr +LocalTrajectoryBuilder::AddAccumulatedRangeData( + const common::Time time, const sensor::RangeData& range_data) { const transform::Rigid3d odometry_prediction = pose_estimate_ * odometry_correction_; const transform::Rigid3d model_prediction = pose_estimate_; @@ -192,7 +216,7 @@ LocalTrajectoryBuilder::AddHorizontalRangeData( range_data_in_tracking_2d, pose_estimate_2d}); } -const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& +const LocalTrajectoryBuilder::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const { return last_pose_estimate_; } diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index 26ec09d..cb2b385 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/cartographer/mapping_2d/local_trajectory_builder.h @@ -38,6 +38,8 @@ namespace mapping_2d { // closure. class LocalTrajectoryBuilder { public: + using PoseEstimate = mapping::GlobalTrajectoryBuilderInterface::PoseEstimate; + struct InsertionResult { common::Time time; std::vector> insertion_submaps; @@ -53,8 +55,7 @@ class LocalTrajectoryBuilder { LocalTrajectoryBuilder(const LocalTrajectoryBuilder&) = delete; LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete; - const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate() - const; + const PoseEstimate& pose_estimate() const; std::unique_ptr AddHorizontalRangeData( common::Time, const sensor::RangeData& range_data); void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, @@ -62,6 +63,8 @@ class LocalTrajectoryBuilder { void AddOdometerData(common::Time time, const transform::Rigid3d& pose); private: + std::unique_ptr AddAccumulatedRangeData( + common::Time time, const sensor::RangeData& range_data); sensor::RangeData TransformAndFilterRangeData( const transform::Rigid3f& tracking_to_tracking_2d, const sensor::RangeData& range_data) const; @@ -81,7 +84,8 @@ class LocalTrajectoryBuilder { const proto::LocalTrajectoryBuilderOptions options_; ActiveSubmaps active_submaps_; - mapping::GlobalTrajectoryBuilderInterface::PoseEstimate last_pose_estimate_; + + PoseEstimate last_pose_estimate_; // Current 'pose_estimate_' and 'velocity_estimate_' at 'time_'. common::Time time_ = common::Time::min(); @@ -100,6 +104,10 @@ class LocalTrajectoryBuilder { std::unique_ptr imu_tracker_; mapping::OdometryStateTracker odometry_state_tracker_; + + int num_accumulated_ = 0; + transform::Rigid3f first_pose_estimate_ = transform::Rigid3f::Identity(); + sensor::RangeData accumulated_range_data_; }; } // namespace mapping_2d diff --git a/cartographer/mapping_2d/local_trajectory_builder_options.cc b/cartographer/mapping_2d/local_trajectory_builder_options.cc index 906c206..29e53cb 100644 --- a/cartographer/mapping_2d/local_trajectory_builder_options.cc +++ b/cartographer/mapping_2d/local_trajectory_builder_options.cc @@ -34,6 +34,8 @@ proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions( options.set_max_z(parameter_dictionary->GetDouble("max_z")); options.set_missing_data_ray_length( parameter_dictionary->GetDouble("missing_data_ray_length")); + options.set_scans_per_accumulation( + parameter_dictionary->GetInt("scans_per_accumulation")); options.set_voxel_filter_size( parameter_dictionary->GetDouble("voxel_filter_size")); options.set_use_online_correlative_scan_matching( diff --git a/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto b/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto index 6ca1c0c..44cbaaf 100644 --- a/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto +++ b/cartographer/mapping_2d/proto/local_trajectory_builder_options.proto @@ -32,6 +32,10 @@ message LocalTrajectoryBuilderOptions { // Points beyond 'max_range' will be inserted with this length as empty space. optional float missing_data_ray_length = 16; + // Number of scans to accumulate into one unwarped, combined scan to use for + // scan matching. + optional int32 scans_per_accumulation = 19; + // Voxel filter that gets applied to the range data immediately after // cropping. optional float voxel_filter_size = 3; diff --git a/cartographer/mapping_3d/local_trajectory_builder.h b/cartographer/mapping_3d/local_trajectory_builder.h index 818fc29..960e5cd 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.h +++ b/cartographer/mapping_3d/local_trajectory_builder.h @@ -75,7 +75,7 @@ class LocalTrajectoryBuilder { const transform::Rigid3d& pose_observation); const proto::LocalTrajectoryBuilderOptions options_; - mapping_3d::ActiveSubmaps active_submaps_; + ActiveSubmaps active_submaps_; PoseEstimate last_pose_estimate_; diff --git a/cartographer/mapping_3d/proto/local_trajectory_builder_options.proto b/cartographer/mapping_3d/proto/local_trajectory_builder_options.proto index 36b9745..14fc207 100644 --- a/cartographer/mapping_3d/proto/local_trajectory_builder_options.proto +++ b/cartographer/mapping_3d/proto/local_trajectory_builder_options.proto @@ -14,14 +14,14 @@ syntax = "proto2"; +package cartographer.mapping_3d.proto; + import "cartographer/mapping_3d/proto/motion_filter_options.proto"; import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto"; import "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto"; import "cartographer/mapping_3d/proto/submaps_options.proto"; import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto"; -package cartographer.mapping_3d.proto; - message LocalTrajectoryBuilderOptions { // Rangefinder points outside these ranges will be dropped. optional float min_range = 1; diff --git a/configuration_files/trajectory_builder_2d.lua b/configuration_files/trajectory_builder_2d.lua index a61835c..6b9836b 100644 --- a/configuration_files/trajectory_builder_2d.lua +++ b/configuration_files/trajectory_builder_2d.lua @@ -19,6 +19,7 @@ TRAJECTORY_BUILDER_2D = { min_z = -0.8, max_z = 2., missing_data_ray_length = 5., + scans_per_accumulation = 1, voxel_filter_size = 0.025, adaptive_voxel_filter = {