Refactoring of LocalTrajectoryBuilder::AddAccumulatedRangeData (#618)

- introduce InsertIntoSubmap for 2D
- clarify some variable names in 3D
- move rotational_scan_matcher_histogram calculation
  to InsertIntoSubmap for 3D
- refactor last version of range data before insertion into
  range_data_in_local (filtered_range_data_in_local for 3D)

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -130,8 +130,8 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
 std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
 LocalTrajectoryBuilder::AddAccumulatedRangeData(
     const common::Time time, const sensor::RangeData& range_data) {
-  // Transforms 'range_data' into a frame where gravity direction is
+  // Transforms 'range_data' from the tracking frame into a frame where gravity
-  // approximately +z.
+  // direction is approximately +z.
   const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
       extrapolator_->EstimateGravityOrientation(time));
   const sensor::RangeData gravity_aligned_range_data =
@@ -147,19 +147,27 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
   const transform::Rigid2d pose_prediction = transform::Project2D(
       non_gravity_aligned_pose_prediction * gravity_alignment.inverse());
 
-  transform::Rigid2d pose_estimate_2d;
+  transform::Rigid2d pose_estimate_2d;  // local frame <- gravity-aligned frame
   ScanMatch(time, pose_prediction, gravity_aligned_range_data,
             &pose_estimate_2d);
   const transform::Rigid3d pose_estimate =
       transform::Embed3D(pose_estimate_2d) * gravity_alignment;
   extrapolator_->AddPose(time, pose_estimate);
 
-  last_pose_estimate_ = {
+  sensor::RangeData range_data_in_local =
-      time, pose_estimate,
+      TransformRangeData(gravity_aligned_range_data,
-      sensor::TransformPointCloud(
+                         transform::Embed3D(pose_estimate_2d.cast<float>()));
-          gravity_aligned_range_data.returns,
+  last_pose_estimate_ = {time, pose_estimate, range_data_in_local.returns};
-          transform::Embed3D(pose_estimate_2d.cast<float>()))};
+  return InsertIntoSubmap(time, range_data_in_local, gravity_aligned_range_data,
+                          pose_estimate, gravity_alignment.rotation());
+}
 
+std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
+LocalTrajectoryBuilder::InsertIntoSubmap(
+    const common::Time time, const sensor::RangeData& range_data_in_local,
+    const sensor::RangeData& gravity_aligned_range_data,
+    const transform::Rigid3d& pose_estimate,
+    const Eigen::Quaterniond& gravity_alignment) {
   if (motion_filter_.IsSimilar(time, pose_estimate)) {
     return nullptr;
   }
@@ -170,9 +178,7 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
   for (const std::shared_ptr<Submap>& submap : active_submaps_.submaps()) {
     insertion_submaps.push_back(submap);
   }
-  active_submaps_.InsertRangeData(
+  active_submaps_.InsertRangeData(range_data_in_local);
-      TransformRangeData(gravity_aligned_range_data,
-                         transform::Embed3D(pose_estimate_2d.cast<float>())));
 
   sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
       options_.loop_closure_adaptive_voxel_filter_options());
@@ -183,7 +189,7 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
       std::make_shared<const mapping::TrajectoryNode::Data>(
           mapping::TrajectoryNode::Data{
               time,
-              gravity_alignment.rotation(),
+              gravity_alignment,
               filtered_gravity_aligned_point_cloud,
               {},  // 'high_resolution_point_cloud' is only used in 3D.
               {},  // 'low_resolution_point_cloud' is only used in 3D.

cartographer/mapping_2d/local_trajectory_builder.h

@@ -68,6 +68,12 @@ class LocalTrajectoryBuilder {
       const transform::Rigid3f& gravity_alignment,
       const sensor::RangeData& range_data) const;
 
+  std::unique_ptr<InsertionResult> InsertIntoSubmap(
+      common::Time time, const sensor::RangeData& range_data_in_local,
+      const sensor::RangeData& gravity_aligned_range_data,
+      const transform::Rigid3d& pose_estimate,
+      const Eigen::Quaterniond& gravity_alignment);
+
   // Scan matches 'gravity_aligned_range_data' and fill in the
   // 'pose_observation' with the result.
   void ScanMatch(common::Time time, const transform::Rigid2d& pose_prediction,

cartographer/mapping_3d/local_trajectory_builder.cc

@@ -110,14 +110,14 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
 std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
 LocalTrajectoryBuilder::AddAccumulatedRangeData(
     const common::Time time, const sensor::RangeData& range_data_in_tracking) {
-  const sensor::RangeData filtered_range_data = {
+  const sensor::RangeData filtered_range_data_in_tracking = {
       range_data_in_tracking.origin,
       sensor::VoxelFiltered(range_data_in_tracking.returns,
                             options_.voxel_filter_size()),
       sensor::VoxelFiltered(range_data_in_tracking.misses,
                             options_.voxel_filter_size())};
 
-  if (filtered_range_data.returns.empty()) {
+  if (filtered_range_data_in_tracking.returns.empty()) {
     LOG(WARNING) << "Dropped empty range data.";
     return nullptr;
   }
@@ -131,13 +131,13 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
       matching_submap->local_pose().inverse() * pose_prediction;
   sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
       options_.high_resolution_adaptive_voxel_filter_options());
-  const sensor::PointCloud filtered_point_cloud_in_tracking =
+  const sensor::PointCloud high_resolution_point_cloud_in_tracking =
-      adaptive_voxel_filter.Filter(filtered_range_data.returns);
+      adaptive_voxel_filter.Filter(filtered_range_data_in_tracking.returns);
   if (options_.use_online_correlative_scan_matching()) {
     // We take a copy since we use 'initial_ceres_pose' as an output argument.
     const transform::Rigid3d initial_pose = initial_ceres_pose;
     real_time_correlative_scan_matcher_->Match(
-        initial_pose, filtered_point_cloud_in_tracking,
+        initial_pose, high_resolution_point_cloud_in_tracking,
         matching_submap->high_resolution_hybrid_grid(), &initial_ceres_pose);
   }
 
@@ -147,11 +147,12 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
   sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter(
       options_.low_resolution_adaptive_voxel_filter_options());
   const sensor::PointCloud low_resolution_point_cloud_in_tracking =
-      low_resolution_adaptive_voxel_filter.Filter(filtered_range_data.returns);
+      low_resolution_adaptive_voxel_filter.Filter(
+          filtered_range_data_in_tracking.returns);
   ceres_scan_matcher_->Match(
       matching_submap->local_pose().inverse() * pose_prediction,
       initial_ceres_pose,
-      {{&filtered_point_cloud_in_tracking,
+      {{&high_resolution_point_cloud_in_tracking,
         &matching_submap->high_resolution_hybrid_grid()},
        {&low_resolution_point_cloud_in_tracking,
         &matching_submap->low_resolution_hybrid_grid()}},
@@ -161,22 +162,15 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
   extrapolator_->AddPose(time, pose_estimate);
   const Eigen::Quaterniond gravity_alignment =
       extrapolator_->EstimateGravityOrientation(time);
-  const auto rotational_scan_matcher_histogram =
-      scan_matching::RotationalScanMatcher::ComputeHistogram(
-          sensor::TransformPointCloud(
-              filtered_range_data.returns,
-              transform::Rigid3f::Rotation(gravity_alignment.cast<float>())),
-          options_.rotational_histogram_size());
 
-  last_pose_estimate_ = {
+  sensor::RangeData filtered_range_data_in_local = sensor::TransformRangeData(
-      time, pose_estimate,
+      filtered_range_data_in_tracking, pose_estimate.cast<float>());
-      sensor::TransformPointCloud(filtered_range_data.returns,
+  last_pose_estimate_ = {time, pose_estimate,
-                                  pose_estimate.cast<float>())};
+                         filtered_range_data_in_local.returns};
-
+  return InsertIntoSubmap(
-  return InsertIntoSubmap(time, filtered_range_data, gravity_alignment,
+      time, filtered_range_data_in_local, filtered_range_data_in_tracking,
-                          filtered_point_cloud_in_tracking,
+      high_resolution_point_cloud_in_tracking,
-                          low_resolution_point_cloud_in_tracking,
+      low_resolution_point_cloud_in_tracking, pose_estimate, gravity_alignment);
-                          rotational_scan_matcher_histogram, pose_estimate);
 }
 
 void LocalTrajectoryBuilder::AddOdometerData(
@@ -195,13 +189,14 @@ const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const {
 
 std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
 LocalTrajectoryBuilder::InsertIntoSubmap(
-    const common::Time time, const sensor::RangeData& range_data_in_tracking,
+    const common::Time time,
-    const Eigen::Quaterniond& gravity_alignment,
+    const sensor::RangeData& filtered_range_data_in_local,
-    const sensor::PointCloud& high_resolution_point_cloud,
+    const sensor::RangeData& filtered_range_data_in_tracking,
-    const sensor::PointCloud& low_resolution_point_cloud,
+    const sensor::PointCloud& high_resolution_point_cloud_in_tracking,
-    const Eigen::VectorXf& rotational_scan_matcher_histogram,
+    const sensor::PointCloud& low_resolution_point_cloud_in_tracking,
-    const transform::Rigid3d& pose_observation) {
+    const transform::Rigid3d& pose_estimate,
-  if (motion_filter_.IsSimilar(time, pose_observation)) {
+    const Eigen::Quaterniond& gravity_alignment) {
+  if (motion_filter_.IsSimilar(time, pose_estimate)) {
     return nullptr;
   }
   // Querying the active submaps must be done here before calling
@@ -210,21 +205,24 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
   for (const std::shared_ptr<Submap>& submap : active_submaps_.submaps()) {
     insertion_submaps.push_back(submap);
   }
-  active_submaps_.InsertRangeData(
+  active_submaps_.InsertRangeData(filtered_range_data_in_local,
-      sensor::TransformRangeData(range_data_in_tracking,
+                                  gravity_alignment);
-                                 pose_observation.cast<float>()),
+  const auto rotational_scan_matcher_histogram =
-      gravity_alignment);
+      scan_matching::RotationalScanMatcher::ComputeHistogram(
+          sensor::TransformPointCloud(
+              filtered_range_data_in_tracking.returns,
+              transform::Rigid3f::Rotation(gravity_alignment.cast<float>())),
+          options_.rotational_histogram_size());
   return std::unique_ptr<InsertionResult>(new InsertionResult{
-
       std::make_shared<const mapping::TrajectoryNode::Data>(
           mapping::TrajectoryNode::Data{
               time,
               gravity_alignment,
               {},  // 'filtered_point_cloud' is only used in 2D.
-              high_resolution_point_cloud,
+              high_resolution_point_cloud_in_tracking,
-              low_resolution_point_cloud,
+              low_resolution_point_cloud_in_tracking,
               rotational_scan_matcher_histogram,
-              pose_observation}),
+              pose_estimate}),
       std::move(insertion_submaps)});
 }
 

cartographer/mapping_3d/local_trajectory_builder.h

@@ -63,12 +63,12 @@ class LocalTrajectoryBuilder {
       common::Time time, const sensor::RangeData& range_data_in_tracking);
 
   std::unique_ptr<InsertionResult> InsertIntoSubmap(
-      common::Time time, const sensor::RangeData& range_data_in_tracking,
+      common::Time time, const sensor::RangeData& filtered_range_data_in_local,
-      const Eigen::Quaterniond& gravity_alignment,
+      const sensor::RangeData& filtered_range_data_in_tracking,
-      const sensor::PointCloud& high_resolution_point_cloud,
+      const sensor::PointCloud& high_resolution_point_cloud_in_tracking,
-      const sensor::PointCloud& low_resolution_point_cloud,
+      const sensor::PointCloud& low_resolution_point_cloud_in_tracking,
-      const Eigen::VectorXf& rotational_scan_matcher_histogram,
+      const transform::Rigid3d& pose_estimate,
-      const transform::Rigid3d& pose_observation);
+      const Eigen::Quaterniond& gravity_alignment);
 
   const proto::LocalTrajectoryBuilderOptions options_;
   ActiveSubmaps active_submaps_;