Add filtered point cloud to TrajectoryNode::Data for 2D. (#483)

cartographer/mapping/sparse_pose_graph/constraint_builder.cc

@@ -32,9 +32,6 @@ proto::ConstraintBuilderOptions CreateConstraintBuilderOptions(
   options.set_sampling_ratio(parameter_dictionary->GetDouble("sampling_ratio"));
   options.set_max_constraint_distance(
       parameter_dictionary->GetDouble("max_constraint_distance"));
-  *options.mutable_adaptive_voxel_filter_options() =
-      sensor::CreateAdaptiveVoxelFilterOptions(
-          parameter_dictionary->GetDictionary("adaptive_voxel_filter").get());
   options.set_min_score(parameter_dictionary->GetDouble("min_score"));
   options.set_global_localization_min_score(
       parameter_dictionary->GetDouble("global_localization_min_score"));

cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto

@@ -16,7 +16,6 @@ syntax = "proto2";
 
 package cartographer.mapping.sparse_pose_graph.proto;
 
-import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto";
 import "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto";
 import "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.proto";
 import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto";
@@ -30,11 +29,6 @@ message ConstraintBuilderOptions {
   // Threshold for poses to be considered near a submap.
   optional double max_constraint_distance = 2;
 
-  // This is only used for 2D.
-  // Voxel filter used to compute a sparser point cloud for matching.
-  optional sensor.proto.AdaptiveVoxelFilterOptions
-      adaptive_voxel_filter_options = 3;
-
   // Threshold for the scan match score below which a match is not considered.
   // Low scores indicate that the scan and map do not look similar.
   optional double min_score = 4;

cartographer/mapping/trajectory_node.h

@@ -35,6 +35,9 @@ struct TrajectoryNode {
     // Range data in 'pose' frame.
     sensor::CompressedRangeData range_data;
 
+    // Used for loop closure in 2D: voxel filtered returns in 'pose' frame.
+    sensor::PointCloud filtered_point_cloud;
+
     // Used for loop closure in 3D.
     sensor::PointCloud high_resolution_point_cloud;
     sensor::PointCloud low_resolution_point_cloud;

cartographer/mapping_2d/global_trajectory_builder.cc

@@ -40,6 +40,7 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
   sparse_pose_graph_->AddScan(
       insertion_result->time, insertion_result->tracking_to_tracking_2d,
       insertion_result->range_data_in_tracking_2d,
+      insertion_result->filtered_point_cloud_in_tracking_2d,
       insertion_result->pose_estimate_2d, trajectory_id_,
       insertion_result->insertion_submaps);
 }

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -181,9 +181,15 @@ LocalTrajectoryBuilder::AddAccumulatedRangeData(
       TransformRangeData(range_data_in_tracking_2d,
                          transform::Embed3D(pose_estimate_2d.cast<float>())));
 
-  return common::make_unique<InsertionResult>(InsertionResult{
+  sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
-      time, std::move(insertion_submaps), tracking_to_tracking_2d,
+      options_.loop_closure_adaptive_voxel_filter_options());
-      range_data_in_tracking_2d, pose_estimate_2d});
+  const sensor::PointCloud filtered_point_cloud_in_tracking_2d =
+      adaptive_voxel_filter.Filter(range_data_in_tracking_2d.returns);
+
+  return common::make_unique<InsertionResult>(
+      InsertionResult{time, std::move(insertion_submaps),
+                      tracking_to_tracking_2d, range_data_in_tracking_2d,
+                      pose_estimate_2d, filtered_point_cloud_in_tracking_2d});
 }
 
 const mapping::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const {

cartographer/mapping_2d/local_trajectory_builder.h

@@ -43,6 +43,7 @@ class LocalTrajectoryBuilder {
     transform::Rigid3d tracking_to_tracking_2d;
     sensor::RangeData range_data_in_tracking_2d;
     transform::Rigid2d pose_estimate_2d;
+    sensor::PointCloud filtered_point_cloud_in_tracking_2d;
   };
 
   explicit LocalTrajectoryBuilder(

cartographer/mapping_2d/local_trajectory_builder_options.cc

@@ -43,6 +43,11 @@ proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
   *options.mutable_adaptive_voxel_filter_options() =
       sensor::CreateAdaptiveVoxelFilterOptions(
           parameter_dictionary->GetDictionary("adaptive_voxel_filter").get());
+  *options.mutable_loop_closure_adaptive_voxel_filter_options() =
+      sensor::CreateAdaptiveVoxelFilterOptions(
+          parameter_dictionary
+              ->GetDictionary("loop_closure_adaptive_voxel_filter")
+              .get());
   *options.mutable_real_time_correlative_scan_matcher_options() =
       scan_matching::CreateRealTimeCorrelativeScanMatcherOptions(
           parameter_dictionary

cartographer/mapping_2d/proto/local_trajectory_builder_options.proto

@@ -44,6 +44,11 @@ message LocalTrajectoryBuilderOptions {
   optional sensor.proto.AdaptiveVoxelFilterOptions
       adaptive_voxel_filter_options = 6;
 
+  // Voxel filter used to compute a sparser point cloud for finding loop
+  // closures.
+  optional sensor.proto.AdaptiveVoxelFilterOptions
+      loop_closure_adaptive_voxel_filter_options = 20;
+
   // Whether to solve the online scan matching first using the correlative scan
   // matcher to generate a good starting point for Ceres.
   optional bool use_online_correlative_scan_matching = 5;

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -95,8 +95,9 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
 
 void SparsePoseGraph::AddScan(
     common::Time time, const transform::Rigid3d& tracking_to_pose,
-    const sensor::RangeData& range_data_in_pose, const transform::Rigid2d& pose,
+    const sensor::RangeData& range_data_in_pose,
-    const int trajectory_id,
+    const sensor::PointCloud& filtered_point_cloud,
+    const transform::Rigid2d& pose, const int trajectory_id,
     const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
   const transform::Rigid3d optimized_pose(
       GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose));
@@ -108,6 +109,7 @@ void SparsePoseGraph::AddScan(
           std::make_shared<const mapping::TrajectoryNode::Data>(
               mapping::TrajectoryNode::Data{time,
                                             Compress(range_data_in_pose),
+                                            filtered_point_cloud,
                                             {},
                                             {},
                                             tracking_to_pose}),
@@ -184,7 +186,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
       global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
     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(),
         &trajectory_connectivity_);
   } else {
     const bool scan_and_submap_trajectories_connected =
@@ -206,7 +208,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
               .point_cloud_pose;
       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(),
           initial_relative_pose);
     }
   }

cartographer/mapping_2d/sparse_pose_graph.h

@@ -73,6 +73,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   void AddScan(
       common::Time time, const transform::Rigid3d& tracking_to_pose,
       const sensor::RangeData& range_data_in_pose,
+      const sensor::PointCloud& filtered_point_cloud,
       const transform::Rigid2d& pose, int trajectory_id,
       const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
       EXCLUDES(mutex_);

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc

@@ -48,7 +48,6 @@ ConstraintBuilder::ConstraintBuilder(
     : options_(options),
       thread_pool_(thread_pool),
       sampler_(options.sampling_ratio()),
-      adaptive_voxel_filter_(options.adaptive_voxel_filter_options()),
       ceres_scan_matcher_(options.ceres_scan_matcher_options()) {}
 
 ConstraintBuilder::~ConstraintBuilder() {
@@ -62,7 +61,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 transform::Rigid2d& initial_relative_pose) {
   if (initial_relative_pose.translation().norm() >
       options_.max_constraint_distance()) {
@@ -76,10 +75,10 @@ void ConstraintBuilder::MaybeAddConstraint(
     const int current_computation = current_computation_;
     ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
         submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) {
-          ComputeConstraint(
+          ComputeConstraint(submap_id, submap, node_id,
-              submap_id, submap, node_id, false, /* match_full_submap */
+                            false,   /* match_full_submap */
-              nullptr,                           /* trajectory_connectivity */
+                            nullptr, /* trajectory_connectivity */
-              compressed_point_cloud, initial_relative_pose, constraint);
+                            constant_data, initial_relative_pose, constraint);
           FinishComputation(current_computation);
         });
   }
@@ -88,7 +87,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,
     mapping::TrajectoryConnectivity* const trajectory_connectivity) {
   common::MutexLocker locker(&mutex_);
   constraints_.emplace_back();
@@ -99,7 +98,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
       submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) {
         ComputeConstraint(submap_id, submap, node_id,
                           true, /* match_full_submap */
-                          trajectory_connectivity, compressed_point_cloud,
+                          trajectory_connectivity, constant_data,
                           transform::Rigid2d::Identity(), constraint);
         FinishComputation(current_computation);
       });
@@ -164,15 +163,13 @@ void ConstraintBuilder::ComputeConstraint(
     const mapping::SubmapId& submap_id, const Submap* const submap,
     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::Rigid2d& initial_relative_pose,
     std::unique_ptr<ConstraintBuilder::Constraint>* constraint) {
   const transform::Rigid2d initial_pose =
       ComputeSubmapPose(*submap) * initial_relative_pose;
   const SubmapScanMatcher* const submap_scan_matcher =
       GetSubmapScanMatcher(submap_id);
-  const sensor::PointCloud filtered_point_cloud =
-      adaptive_voxel_filter_.Filter(compressed_point_cloud->Decompress());
 
   // The 'constraint_transform' (submap i <- scan j) is computed from:
   // - a 'filtered_point_cloud' in scan j,
@@ -188,8 +185,8 @@ void ConstraintBuilder::ComputeConstraint(
   // 3. Refine.
   if (match_full_submap) {
     if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
-            filtered_point_cloud, options_.global_localization_min_score(),
+            constant_data->filtered_point_cloud,
-            &score, &pose_estimate)) {
+            options_.global_localization_min_score(), &score, &pose_estimate)) {
       CHECK_GT(score, options_.global_localization_min_score());
       CHECK_GE(node_id.trajectory_id, 0);
       CHECK_GE(submap_id.trajectory_id, 0);
@@ -200,8 +197,8 @@ void ConstraintBuilder::ComputeConstraint(
     }
   } else {
     if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
-            initial_pose, filtered_point_cloud, options_.min_score(), &score,
+            initial_pose, constant_data->filtered_point_cloud,
-            &pose_estimate)) {
+            options_.min_score(), &score, &pose_estimate)) {
       // We've reported a successful local match.
       CHECK_GT(score, options_.min_score());
     } else {
@@ -217,9 +214,9 @@ void ConstraintBuilder::ComputeConstraint(
   // effect that, in the absence of better information, we prefer the original
   // CSM estimate.
   ceres::Solver::Summary unused_summary;
-  ceres_scan_matcher_.Match(pose_estimate, pose_estimate, filtered_point_cloud,
+  ceres_scan_matcher_.Match(
-                            *submap_scan_matcher->probability_grid,
+      pose_estimate, pose_estimate, constant_data->filtered_point_cloud,
-                            &pose_estimate, &unused_summary);
+      *submap_scan_matcher->probability_grid, &pose_estimate, &unused_summary);
 
   const transform::Rigid2d constraint_transform =
       ComputeSubmapPose(*submap).inverse() * pose_estimate;
@@ -232,8 +229,9 @@ void ConstraintBuilder::ComputeConstraint(
 
   if (options_.log_matches()) {
     std::ostringstream info;
-    info << "Node " << node_id << " with " << filtered_point_cloud.size()
+    info << "Node " << node_id << " with "
-         << " points on submap " << submap_id << std::fixed;
+         << constant_data->filtered_point_cloud.size() << " points on submap "
+         << submap_id << std::fixed;
     if (match_full_submap) {
       info << " matches";
     } else {

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h

@@ -80,7 +80,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 transform::Rigid2d& initial_relative_pose);
 
   // Schedules exploring a new constraint between 'submap' identified by
@@ -94,7 +94,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,
       mapping::TrajectoryConnectivity* trajectory_connectivity);
 
   // Must be called after all computations related to one node have been added.
@@ -142,7 +142,7 @@ class ConstraintBuilder {
       const mapping::SubmapId& submap_id, const Submap* submap,
       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::Rigid2d& initial_relative_pose,
       std::unique_ptr<Constraint>* constraint) EXCLUDES(mutex_);
 
@@ -181,7 +181,6 @@ class ConstraintBuilder {
       submap_queued_work_items_ GUARDED_BY(mutex_);
 
   common::FixedRatioSampler sampler_;
-  const sensor::AdaptiveVoxelFilter adaptive_voxel_filter_;
   scan_matching::CeresScanMatcher ceres_scan_matcher_;
 
   // Histogram of scan matcher scores.

cartographer/mapping_2d/sparse_pose_graph_test.cc

@@ -69,11 +69,6 @@ class SparsePoseGraphTest : public ::testing::Test {
             constraint_builder = {
               sampling_ratio = 1.,
               max_constraint_distance = 6.,
-              adaptive_voxel_filter = {
-                max_length = 1e-2,
-                min_num_points = 1000,
-                max_range = 50.,
-              },
               min_score = 0.5,
               global_localization_min_score = 0.6,
               loop_closure_translation_weight = 1.,
@@ -164,7 +159,7 @@ class SparsePoseGraphTest : public ::testing::Test {
 
     sparse_pose_graph_->AddScan(
         common::FromUniversal(0), transform::Rigid3d::Identity(), range_data,
-        pose_estimate, kTrajectoryId, insertion_submaps);
+        range_data.returns, pose_estimate, kTrajectoryId, insertion_submaps);
   }
 
   void MoveRelative(const transform::Rigid2d& movement) {

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -102,14 +102,16 @@ void SparsePoseGraph::AddScan(
       GetLocalToGlobalTransform(trajectory_id) * pose);
   common::MutexLocker locker(&mutex_);
   trajectory_nodes_.Append(
-      trajectory_id,
+      trajectory_id, mapping::TrajectoryNode{
-      mapping::TrajectoryNode{
+                         std::make_shared<const mapping::TrajectoryNode::Data>(
-          std::make_shared<const mapping::TrajectoryNode::Data>(
+                             mapping::TrajectoryNode::Data{
-              mapping::TrajectoryNode::Data{
+                                 time,
-                  time, sensor::Compress(range_data_in_tracking),
+                                 sensor::Compress(range_data_in_tracking),
-                  high_resolution_point_cloud, low_resolution_point_cloud,
+                                 {},
-                  transform::Rigid3d::Identity()}),
+                                 high_resolution_point_cloud,
-          optimized_pose});
+                                 low_resolution_point_cloud,
+                                 transform::Rigid3d::Identity()}),
+                         optimized_pose});
   ++num_trajectory_nodes_;
   trajectory_connectivity_.Add(trajectory_id);
 

configuration_files/sparse_pose_graph.lua

@@ -17,11 +17,6 @@ SPARSE_POSE_GRAPH = {
   constraint_builder = {
     sampling_ratio = 0.3,
     max_constraint_distance = 15.,
-    adaptive_voxel_filter = {
-      max_length = 0.9,
-      min_num_points = 100,
-      max_range = 50.,
-    },
     min_score = 0.55,
     global_localization_min_score = 0.6,
     loop_closure_translation_weight = 1.1e4,

configuration_files/trajectory_builder_2d.lua

@@ -28,6 +28,12 @@ TRAJECTORY_BUILDER_2D = {
     max_range = 50.,
   },
 
+  loop_closure_adaptive_voxel_filter = {
+    max_length = 0.9,
+    min_num_points = 100,
+    max_range = 50.,
+  },
+
   use_online_correlative_scan_matching = false,
   real_time_correlative_scan_matcher = {
     linear_search_window = 0.1,

docs/source/configuration.rst

@@ -105,10 +105,6 @@ double sampling_ratio
 double max_constraint_distance
   Threshold for poses to be considered near a submap.
 
-cartographer.sensor.proto.AdaptiveVoxelFilterOptions adaptive_voxel_filter_options
-  This is only used for 2D.
-  Voxel filter used to compute a sparser point cloud for matching.
-
 double min_score
   Threshold for the scan match score below which a match is not considered.
   Low scores indicate that the scan and map do not look similar.
@@ -136,12 +132,6 @@ cartographer.mapping_2d.scan_matching.proto.CeresScanMatcherOptions ceres_scan_m
 cartographer.mapping_3d.scan_matching.proto.FastCorrelativeScanMatcherOptions fast_correlative_scan_matcher_options_3d
   Not yet documented.
 
-cartographer.sensor.proto.AdaptiveVoxelFilterOptions high_resolution_adaptive_voxel_filter_options
-  Voxel filter used for high resolution, only used for 3D loop closure.
-
-cartographer.sensor.proto.AdaptiveVoxelFilterOptions low_resolution_adaptive_voxel_filter_options
-  Voxel filter used for low resolution, 3D loop closure refinement.
-
 cartographer.mapping_3d.scan_matching.proto.CeresScanMatcherOptions ceres_scan_matcher_options_3d
   Not yet documented.
 
@@ -206,6 +196,10 @@ float voxel_filter_size
 cartographer.sensor.proto.AdaptiveVoxelFilterOptions adaptive_voxel_filter_options
   Voxel filter used to compute a sparser point cloud for matching.
 
+cartographer.sensor.proto.AdaptiveVoxelFilterOptions loop_closure_adaptive_voxel_filter_options
+  Voxel filter used to compute a sparser point cloud for finding loop
+  closures.
+
 bool use_online_correlative_scan_matching
   Whether to solve the online scan matching first using the correlative scan
   matcher to generate a good starting point for Ceres.