Add filtered point clouds to TrajectoryNode::Data for 3D. (#482)

cartographer/mapping/sparse_pose_graph/constraint_builder.cc

@@ -55,16 +55,6 @@ proto::ConstraintBuilderOptions CreateConstraintBuilderOptions(
           parameter_dictionary
               ->GetDictionary("fast_correlative_scan_matcher_3d")
               .get());
-  *options.mutable_high_resolution_adaptive_voxel_filter_options() =
-      sensor::CreateAdaptiveVoxelFilterOptions(
-          parameter_dictionary
-              ->GetDictionary("high_resolution_adaptive_voxel_filter")
-              .get());
-  *options.mutable_low_resolution_adaptive_voxel_filter_options() =
-      sensor::CreateAdaptiveVoxelFilterOptions(
-          parameter_dictionary
-              ->GetDictionary("low_resolution_adaptive_voxel_filter")
-              .get());
   *options.mutable_ceres_scan_matcher_options_3d() =
       mapping_3d::scan_matching::CreateCeresScanMatcherOptions(
           parameter_dictionary->GetDictionary("ceres_scan_matcher_3d").get());

cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto

@@ -61,14 +61,6 @@ message ConstraintBuilderOptions {
   optional mapping_3d.scan_matching.proto.FastCorrelativeScanMatcherOptions
       fast_correlative_scan_matcher_options_3d = 10;
 
-  // Voxel filter used for high resolution, only used for 3D loop closure.
-  optional sensor.proto.AdaptiveVoxelFilterOptions
-      high_resolution_adaptive_voxel_filter_options = 15;
-
-  // Voxel filter used for low resolution, 3D loop closure refinement.
-  optional sensor.proto.AdaptiveVoxelFilterOptions
-      low_resolution_adaptive_voxel_filter_options = 16;
-
   optional mapping_3d.scan_matching.proto.CeresScanMatcherOptions
       ceres_scan_matcher_options_3d = 12;
 }

cartographer/mapping/trajectory_node.h

@@ -35,6 +35,10 @@ struct TrajectoryNode {
     // Range data in 'pose' frame.
     sensor::CompressedRangeData range_data;
 
+    // Used for loop closure in 3D.
+    sensor::PointCloud high_resolution_point_cloud;
+    sensor::PointCloud low_resolution_point_cloud;
+
     // Transform from the 3D 'tracking' frame to the 'pose' frame of the range
     // data, which contains roll, pitch and height for 2D. In 3D this is always
     // identity.

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -106,7 +106,10 @@ void SparsePoseGraph::AddScan(
       trajectory_id,
       mapping::TrajectoryNode{
           std::make_shared<const mapping::TrajectoryNode::Data>(
-              mapping::TrajectoryNode::Data{time, Compress(range_data_in_pose),
+              mapping::TrajectoryNode::Data{time,
+                                            Compress(range_data_in_pose),
+                                            {},
+                                            {},
                                             tracking_to_pose}),
           optimized_pose});
   ++num_trajectory_nodes_;

cartographer/mapping_2d/sparse_pose_graph_test.cc

@@ -104,16 +104,6 @@ class SparsePoseGraphTest : public ::testing::Test {
                 linear_z_search_window = 4.,
                 angular_search_window = 0.1,
               },
-              high_resolution_adaptive_voxel_filter = {
-                max_length = 2.,
-                min_num_points = 150,
-                max_range = 15.,
-              },
-              low_resolution_adaptive_voxel_filter = {
-                max_length = 4.,
-                min_num_points = 200,
-                max_range = 60.,
-              },
               ceres_scan_matcher_3d = {
                 occupied_space_weight_0 = 20.,
                 translation_weight = 10.,

cartographer/mapping_3d/global_trajectory_builder.cc

@@ -43,6 +43,8 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
   }
   sparse_pose_graph_->AddScan(
       insertion_result->time, insertion_result->range_data_in_tracking,
+      insertion_result->high_resolution_point_cloud,
+      insertion_result->low_resolution_point_cloud,
       insertion_result->pose_observation, trajectory_id_,
       insertion_result->insertion_submaps);
 }

cartographer/mapping_3d/local_trajectory_builder.cc

@@ -162,7 +162,9 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
       sensor::TransformPointCloud(filtered_range_data.returns,
                                   pose_estimate.cast<float>())};
 
-  return InsertIntoSubmap(time, filtered_range_data, pose_estimate);
+  return InsertIntoSubmap(
+      time, filtered_range_data, filtered_point_cloud_in_tracking,
+      low_resolution_point_cloud_in_tracking, pose_estimate);
 }
 
 void LocalTrajectoryBuilder::AddOdometerData(
@@ -182,6 +184,8 @@ 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 sensor::PointCloud& high_resolution_point_cloud,
+    const sensor::PointCloud& low_resolution_point_cloud,
     const transform::Rigid3d& pose_observation) {
   if (motion_filter_.IsSimilar(time, pose_observation)) {
     return nullptr;
@@ -196,9 +200,10 @@ LocalTrajectoryBuilder::InsertIntoSubmap(
       sensor::TransformRangeData(range_data_in_tracking,
                                  pose_observation.cast<float>()),
       extrapolator_->gravity_orientation());
-  return std::unique_ptr<InsertionResult>(
+  return std::unique_ptr<InsertionResult>(new InsertionResult{
-      new InsertionResult{time, range_data_in_tracking, pose_observation,
+      time, range_data_in_tracking, high_resolution_point_cloud,
-                          std::move(insertion_submaps)});
+      low_resolution_point_cloud, pose_observation,
+      std::move(insertion_submaps)});
 }
 
 }  // namespace mapping_3d

cartographer/mapping_3d/local_trajectory_builder.h

@@ -41,6 +41,11 @@ class LocalTrajectoryBuilder {
   struct InsertionResult {
     common::Time time;
     sensor::RangeData range_data_in_tracking;
+
+    // Used for loop closure in 3D.
+    sensor::PointCloud high_resolution_point_cloud;
+    sensor::PointCloud low_resolution_point_cloud;
+
     transform::Rigid3d pose_observation;
     std::vector<std::shared_ptr<const Submap>> insertion_submaps;
   };
@@ -66,6 +71,8 @@ class LocalTrajectoryBuilder {
 
   std::unique_ptr<InsertionResult> InsertIntoSubmap(
       common::Time time, const sensor::RangeData& range_data_in_tracking,
+      const sensor::PointCloud& high_resolution_point_cloud,
+      const sensor::PointCloud& low_resolution_point_cloud,
       const transform::Rigid3d& pose_observation);
 
   const proto::LocalTrajectoryBuilderOptions options_;

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -94,18 +94,22 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
 
 void SparsePoseGraph::AddScan(
     common::Time time, const sensor::RangeData& range_data_in_tracking,
+    const sensor::PointCloud& high_resolution_point_cloud,
+    const sensor::PointCloud& low_resolution_point_cloud,
     const transform::Rigid3d& pose, const int trajectory_id,
     const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
   const transform::Rigid3d optimized_pose(
       GetLocalToGlobalTransform(trajectory_id) * pose);
   common::MutexLocker locker(&mutex_);
   trajectory_nodes_.Append(
-      trajectory_id, mapping::TrajectoryNode{
+      trajectory_id,
-                         std::make_shared<const mapping::TrajectoryNode::Data>(
+      mapping::TrajectoryNode{
-                             mapping::TrajectoryNode::Data{
+          std::make_shared<const mapping::TrajectoryNode::Data>(
-                                 time, sensor::Compress(range_data_in_tracking),
+              mapping::TrajectoryNode::Data{
-                                 transform::Rigid3d::Identity()}),
+                  time, sensor::Compress(range_data_in_tracking),
-                         optimized_pose});
+                  high_resolution_point_cloud, low_resolution_point_cloud,
+                  transform::Rigid3d::Identity()}),
+          optimized_pose});
   ++num_trajectory_nodes_;
   trajectory_connectivity_.Add(trajectory_id);
 
@@ -206,9 +210,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
     // FastCorrelativeScanMatcher, and the given yaw is essentially ignored.
     constraint_builder_.MaybeAddGlobalConstraint(
         submap_id, submap_data_.at(submap_id).submap.get(), node_id,
-        &trajectory_nodes_.at(node_id).constant_data->range_data.returns,
+        trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
-        submap_nodes, initial_relative_pose.rotation(),
+        initial_relative_pose.rotation(), &trajectory_connectivity_);
-        &trajectory_connectivity_);
   } else {
     const bool scan_and_submap_trajectories_connected =
         reverse_connected_components_.count(node_id.trajectory_id) > 0 &&
@@ -219,8 +222,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
         scan_and_submap_trajectories_connected) {
       constraint_builder_.MaybeAddConstraint(
           submap_id, submap_data_.at(submap_id).submap.get(), node_id,
-          &trajectory_nodes_.at(node_id).constant_data->range_data.returns,
+          trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
-          submap_nodes, initial_relative_pose);
+          initial_relative_pose);
     }
   }
 }

cartographer/mapping_3d/sparse_pose_graph.h

@@ -71,6 +71,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // this submap was inserted into for the last time.
   void AddScan(
       common::Time time, const sensor::RangeData& range_data_in_tracking,
+      const sensor::PointCloud& high_resolution_point_cloud,
+      const sensor::PointCloud& low_resolution_point_cloud,
       const transform::Rigid3d& pose, int trajectory_id,
       const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
       EXCLUDES(mutex_);

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc

@@ -45,10 +45,6 @@ ConstraintBuilder::ConstraintBuilder(
     : options_(options),
       thread_pool_(thread_pool),
       sampler_(options.sampling_ratio()),
-      high_resolution_adaptive_voxel_filter_(
-          options.high_resolution_adaptive_voxel_filter_options()),
-      low_resolution_adaptive_voxel_filter_(
-          options.low_resolution_adaptive_voxel_filter_options()),
       ceres_scan_matcher_(options.ceres_scan_matcher_options_3d()) {}
 
 ConstraintBuilder::~ConstraintBuilder() {
@@ -62,7 +58,7 @@ ConstraintBuilder::~ConstraintBuilder() {
 void ConstraintBuilder::MaybeAddConstraint(
     const mapping::SubmapId& submap_id, const Submap* const submap,
     const mapping::NodeId& node_id,
-    const sensor::CompressedPointCloud* const compressed_point_cloud,
+    const mapping::TrajectoryNode::Data* const constant_data,
     const std::vector<mapping::TrajectoryNode>& submap_nodes,
     const transform::Rigid3d& initial_pose) {
   if (initial_pose.translation().norm() > options_.max_constraint_distance()) {
@@ -78,7 +74,7 @@ void ConstraintBuilder::MaybeAddConstraint(
         submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) {
           ComputeConstraint(submap_id, node_id, false, /* match_full_submap */
                             nullptr, /* trajectory_connectivity */
-                            compressed_point_cloud, initial_pose, constraint);
+                            constant_data, initial_pose, constraint);
           FinishComputation(current_computation);
         });
   }
@@ -87,7 +83,7 @@ void ConstraintBuilder::MaybeAddConstraint(
 void ConstraintBuilder::MaybeAddGlobalConstraint(
     const mapping::SubmapId& submap_id, const Submap* const submap,
     const mapping::NodeId& node_id,
-    const sensor::CompressedPointCloud* const compressed_point_cloud,
+    const mapping::TrajectoryNode::Data* const constant_data,
     const std::vector<mapping::TrajectoryNode>& submap_nodes,
     const Eigen::Quaterniond& gravity_alignment,
     mapping::TrajectoryConnectivity* const trajectory_connectivity) {
@@ -99,7 +95,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
   ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
       submap_id, submap_nodes, submap, [=]() EXCLUDES(mutex_) {
         ComputeConstraint(submap_id, node_id, true, /* match_full_submap */
-                          trajectory_connectivity, compressed_point_cloud,
+                          trajectory_connectivity, constant_data,
                           transform::Rigid3d::Rotation(gravity_alignment),
                           constraint);
         FinishComputation(current_computation);
@@ -172,16 +168,13 @@ void ConstraintBuilder::ComputeConstraint(
     const mapping::SubmapId& submap_id, const mapping::NodeId& node_id,
     bool match_full_submap,
     mapping::TrajectoryConnectivity* trajectory_connectivity,
-    const sensor::CompressedPointCloud* const compressed_point_cloud,
+    const mapping::TrajectoryNode::Data* const constant_data,
     const transform::Rigid3d& initial_pose,
     std::unique_ptr<OptimizationProblem::Constraint>* constraint) {
   const SubmapScanMatcher* const submap_scan_matcher =
       GetSubmapScanMatcher(submap_id);
-  const sensor::PointCloud point_cloud = compressed_point_cloud->Decompress();
+  const sensor::PointCloud point_cloud =
-  const sensor::PointCloud high_resolution_point_cloud =
+      constant_data->range_data.returns.Decompress();
-      high_resolution_adaptive_voxel_filter_.Filter(point_cloud);
-  const sensor::PointCloud low_resolution_point_cloud =
-      low_resolution_adaptive_voxel_filter_.Filter(point_cloud);
 
   // The 'constraint_transform' (submap i <- scan j) is computed from:
   // - a 'high_resolution_point_cloud' in scan j and
@@ -193,7 +186,7 @@ void ConstraintBuilder::ComputeConstraint(
 
   const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher(
       submap_scan_matcher->low_resolution_hybrid_grid,
-      &low_resolution_point_cloud);
+      &constant_data->low_resolution_point_cloud);
 
   // Compute 'pose_estimate' in three stages:
   // 1. Fast estimate using the fast correlative scan matcher.
@@ -201,9 +194,10 @@ void ConstraintBuilder::ComputeConstraint(
   // 3. Refine.
   if (match_full_submap) {
     if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
-            initial_pose.rotation(), high_resolution_point_cloud, point_cloud,
+            initial_pose.rotation(), constant_data->high_resolution_point_cloud,
-            options_.global_localization_min_score(), low_resolution_matcher,
+            point_cloud, options_.global_localization_min_score(),
-            &score, &pose_estimate, &rotational_score, &low_resolution_score)) {
+            low_resolution_matcher, &score, &pose_estimate, &rotational_score,
+            &low_resolution_score)) {
       CHECK_GT(score, options_.global_localization_min_score());
       CHECK_GE(node_id.trajectory_id, 0);
       CHECK_GE(submap_id.trajectory_id, 0);
@@ -214,8 +208,8 @@ void ConstraintBuilder::ComputeConstraint(
     }
   } else {
     if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
-            initial_pose, high_resolution_point_cloud, point_cloud,
+            initial_pose, constant_data->high_resolution_point_cloud,
-            options_.min_score(), low_resolution_matcher, &score,
+            point_cloud, options_.min_score(), low_resolution_matcher, &score,
             &pose_estimate, &rotational_score, &low_resolution_score)) {
       // We've reported a successful local match.
       CHECK_GT(score, options_.min_score());
@@ -236,9 +230,9 @@ void ConstraintBuilder::ComputeConstraint(
   ceres::Solver::Summary unused_summary;
   transform::Rigid3d constraint_transform;
   ceres_scan_matcher_.Match(pose_estimate, pose_estimate,
-                            {{&high_resolution_point_cloud,
+                            {{&constant_data->high_resolution_point_cloud,
                               submap_scan_matcher->high_resolution_hybrid_grid},
-                             {&low_resolution_point_cloud,
+                             {&constant_data->low_resolution_point_cloud,
                               submap_scan_matcher->low_resolution_hybrid_grid}},
                             &constraint_transform, &unused_summary);
 
@@ -251,7 +245,8 @@ void ConstraintBuilder::ComputeConstraint(
 
   if (options_.log_matches()) {
     std::ostringstream info;
-    info << "Node " << node_id << " with " << high_resolution_point_cloud.size()
+    info << "Node " << node_id << " with "
+         << constant_data->high_resolution_point_cloud.size()
          << " points on submap " << submap_id << std::fixed;
     if (match_full_submap) {
       info << " matches";

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h

@@ -76,7 +76,7 @@ class ConstraintBuilder {
   void MaybeAddConstraint(
       const mapping::SubmapId& submap_id, const Submap* submap,
       const mapping::NodeId& node_id,
-      const sensor::CompressedPointCloud* compressed_point_cloud,
+      const mapping::TrajectoryNode::Data* const constant_data,
       const std::vector<mapping::TrajectoryNode>& submap_nodes,
       const transform::Rigid3d& initial_pose);
 
@@ -93,7 +93,7 @@ class ConstraintBuilder {
   void MaybeAddGlobalConstraint(
       const mapping::SubmapId& submap_id, const Submap* submap,
       const mapping::NodeId& node_id,
-      const sensor::CompressedPointCloud* compressed_point_cloud,
+      const mapping::TrajectoryNode::Data* const constant_data,
       const std::vector<mapping::TrajectoryNode>& submap_nodes,
       const Eigen::Quaterniond& gravity_alignment,
       mapping::TrajectoryConnectivity* trajectory_connectivity);
@@ -144,7 +144,7 @@ class ConstraintBuilder {
       const mapping::SubmapId& submap_id, const mapping::NodeId& node_id,
       bool match_full_submap,
       mapping::TrajectoryConnectivity* trajectory_connectivity,
-      const sensor::CompressedPointCloud* compressed_point_cloud,
+      const mapping::TrajectoryNode::Data* const constant_data,
       const transform::Rigid3d& initial_pose,
       std::unique_ptr<Constraint>* constraint) EXCLUDES(mutex_);
 
@@ -183,8 +183,6 @@ class ConstraintBuilder {
       submap_queued_work_items_ GUARDED_BY(mutex_);
 
   common::FixedRatioSampler sampler_;
-  const sensor::AdaptiveVoxelFilter high_resolution_adaptive_voxel_filter_;
-  const sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter_;
   scan_matching::CeresScanMatcher ceres_scan_matcher_;
 
   // Histograms of scan matcher scores.

configuration_files/sparse_pose_graph.lua

@@ -52,16 +52,6 @@ SPARSE_POSE_GRAPH = {
       linear_z_search_window = 1.,
       angular_search_window = math.rad(15.),
     },
-    high_resolution_adaptive_voxel_filter = {
-      max_length = 2.,
-      min_num_points = 150,
-      max_range = 15.,
-    },
-    low_resolution_adaptive_voxel_filter = {
-      max_length = 4.,
-      min_num_points = 200,
-      max_range = 60.,
-    },
     ceres_scan_matcher_3d = {
       occupied_space_weight_0 = 5.,
       occupied_space_weight_1 = 30.,