Adds accumulation of range data to 2D. (#408)

This mostly follows the implementation as done for 3D.

Towards googlecartographer/cartographer_ros#212.

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<float>();
+    accumulated_range_data_ =
+        sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}};
+  }
+
+  const transform::Rigid3f tracking_delta =
+      first_pose_estimate_.inverse() * pose_estimate_.cast<float>();
+  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::InsertionResult>
+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_;
 }

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<std::shared_ptr<const Submap>> 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<InsertionResult> 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<InsertionResult> 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<mapping::ImuTracker> 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

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(

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;

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_;
 

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;

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