Make use of Submap in 2D similar to 3D. (#343)

Use mapping_2d::Submap instead of mapping::Submap in 2D.
Use `finished()` instead of `finished_probability_grid()` in 2D.

PAIR=wohe

cartographer/mapping/submaps.h

@@ -67,13 +67,6 @@ class Submap {
   // Number of RangeData inserted.
   size_t num_range_data() const { return num_range_data_; }
 
-  // The 'finished_probability_grid' when this submap is finished and will not
-  // change anymore. Otherwise, this is nullptr and the next call to
-  // InsertRangeData() will change the submap.
-  const mapping_2d::ProbabilityGrid* finished_probability_grid() const {
-    return finished_probability_grid_;
-  }
-
   // Fills data into the 'response'.
   virtual void ToResponseProto(
       const transform::Rigid3d& global_submap_pose,
@@ -83,9 +76,8 @@ class Submap {
   const transform::Rigid3d local_pose_;
 
  protected:
-  // TODO(hrapp): All of this should be private.
+  // TODO(hrapp): This should be private.
   int num_range_data_ = 0;
-  const mapping_2d::ProbabilityGrid* finished_probability_grid_ = nullptr;
 };
 
 // Submaps is a sequence of maps to which scans are matched and into which scans

cartographer/mapping_2d/global_trajectory_builder.cc

@@ -45,13 +45,19 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
   std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> insertion_result =
       local_trajectory_builder_.AddHorizontalRangeData(
           time, sensor::RangeData{origin, ranges, {}});
-  if (insertion_result != nullptr) {
+  if (insertion_result == nullptr) {
-    sparse_pose_graph_->AddScan(
+    return;
-        insertion_result->time, insertion_result->tracking_to_tracking_2d,
-        insertion_result->range_data_in_tracking_2d,
-        insertion_result->pose_estimate_2d, trajectory_id_,
-        insertion_result->matching_submap, insertion_result->insertion_submaps);
   }
+  const Submap* const finished_submap =
+      insertion_result->insertion_submaps.front()->finished()
+          ? insertion_result->insertion_submaps.front()
+          : nullptr;
+  sparse_pose_graph_->AddScan(
+      insertion_result->time, insertion_result->tracking_to_tracking_2d,
+      insertion_result->range_data_in_tracking_2d,
+      insertion_result->pose_estimate_2d, trajectory_id_,
+      insertion_result->matching_submap, insertion_result->insertion_submaps,
+      finished_submap);
 }
 
 void GlobalTrajectoryBuilder::AddImuData(

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -176,9 +176,8 @@ LocalTrajectoryBuilder::AddHorizontalRangeData(
     return nullptr;
   }
 
-  const mapping::Submap* const matching_submap =
+  const Submap* const matching_submap = submaps_.Get(submaps_.matching_index());
-      submaps_.Get(submaps_.matching_index());
+  std::vector<const Submap*> insertion_submaps;
-  std::vector<const mapping::Submap*> insertion_submaps;
   for (int insertion_index : submaps_.insertion_indices()) {
     insertion_submaps.push_back(submaps_.Get(insertion_index));
   }

cartographer/mapping_2d/local_trajectory_builder.h

@@ -41,8 +41,8 @@ class LocalTrajectoryBuilder {
  public:
   struct InsertionResult {
     common::Time time;
-    const mapping::Submap* matching_submap;
+    const Submap* matching_submap;
-    std::vector<const mapping::Submap*> insertion_submaps;
+    std::vector<const Submap*> insertion_submaps;
     transform::Rigid3d tracking_to_tracking_2d;
     sensor::RangeData range_data_in_tracking_2d;
     transform::Rigid2d pose_estimate_2d;

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -53,7 +53,7 @@ SparsePoseGraph::~SparsePoseGraph() {
 }
 
 void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
-    const std::vector<const mapping::Submap*>& insertion_submaps) {
+    const std::vector<const Submap*>& insertion_submaps) {
   CHECK(!insertion_submaps.empty());
   const mapping::SubmapId first_submap_id = GetSubmapId(insertion_submaps[0]);
   const int trajectory_id = first_submap_id.trajectory_id;
@@ -91,8 +91,9 @@ void 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 int trajectory_id, const mapping::Submap* const matching_submap,
+    const int trajectory_id, const Submap* const matching_submap,
-    const std::vector<const mapping::Submap*>& insertion_submaps) {
+    const std::vector<const Submap*>& insertion_submaps,
+    const Submap* const finished_submap) {
   const transform::Rigid3d optimized_pose(
       GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose));
 
@@ -115,10 +116,6 @@ void SparsePoseGraph::AddScan(
     submap_ids_.emplace(insertion_submaps.back(), submap_id);
     submap_data_.at(submap_id).submap = insertion_submaps.back();
   }
-  const mapping::Submap* const finished_submap =
-      insertion_submaps.front()->finished_probability_grid() != nullptr
-          ? insertion_submaps.front()
-          : nullptr;
 
   // Make sure we have a sampler for this trajectory.
   if (!global_localization_samplers_[trajectory_id]) {
@@ -188,8 +185,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
   }
 }
 
-void SparsePoseGraph::ComputeConstraintsForOldScans(
+void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) {
-    const mapping::Submap* submap) {
   const auto submap_id = GetSubmapId(submap);
   const auto& submap_data = submap_data_.at(submap_id);
 
@@ -209,9 +205,8 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
 }
 
 void SparsePoseGraph::ComputeConstraintsForScan(
-    const mapping::Submap* matching_submap,
+    const Submap* matching_submap, std::vector<const Submap*> insertion_submaps,
-    std::vector<const mapping::Submap*> insertion_submaps,
+    const Submap* finished_submap, const transform::Rigid2d& pose) {
-    const mapping::Submap* finished_submap, const transform::Rigid2d& pose) {
   GrowSubmapTransformsAsNeeded(insertion_submaps);
   const mapping::SubmapId matching_id = GetSubmapId(matching_submap);
   const transform::Rigid2d optimized_pose =
@@ -231,7 +226,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
   const auto& scan_data = trajectory_nodes_.at(node_id).constant_data;
   optimization_problem_.AddTrajectoryNode(
       matching_id.trajectory_id, scan_data->time, pose, optimized_pose);
-  for (const mapping::Submap* submap : insertion_submaps) {
+  for (const Submap* submap : insertion_submaps) {
     const mapping::SubmapId submap_id = GetSubmapId(submap);
     CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
     submap_data_.at(submap_id).node_ids.emplace(node_id);

cartographer/mapping_2d/sparse_pose_graph.h

@@ -66,13 +66,15 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // that will later be optimized. The 'tracking_to_pose' is remembered so
   // that the optimized pose can be embedded into 3D. The 'pose' was determined
   // by scan matching against the 'matching_submap' and the scan was inserted
-  // into the 'insertion_submaps'.
+  // into the 'insertion_submaps'. If not 'nullptr', 'finished_submap' is a
+  // freshly finished submap. It is also contained in 'insertion_submaps' for
+  // the last time.
   void AddScan(common::Time time, const transform::Rigid3d& tracking_to_pose,
                const sensor::RangeData& range_data_in_pose,
                const transform::Rigid2d& pose, int trajectory_id,
-               const mapping::Submap* matching_submap,
+               const Submap* matching_submap,
-               const std::vector<const mapping::Submap*>& insertion_submaps)
+               const std::vector<const Submap*>& insertion_submaps,
-      EXCLUDES(mutex_);
+               const Submap* finished_submap) EXCLUDES(mutex_);
 
   // Adds new IMU data to be used in the optimization.
   void AddImuData(int trajectory_id, common::Time time,
@@ -97,7 +99,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // Likewise, all new scans are matched against submaps which are finished.
   enum class SubmapState { kActive, kFinished, kTrimmed };
   struct SubmapData {
-    const mapping::Submap* submap = nullptr;
+    const Submap* submap = nullptr;
 
     // IDs of the scans that were inserted into this map together with
     // constraints for them. They are not to be matched again when this submap
@@ -110,8 +112,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // Handles a new work item.
   void AddWorkItem(std::function<void()> work_item) REQUIRES(mutex_);
 
-  mapping::SubmapId GetSubmapId(const mapping::Submap* submap) const
+  mapping::SubmapId GetSubmapId(const Submap* submap) const REQUIRES(mutex_) {
-      REQUIRES(mutex_) {
     const auto iterator = submap_ids_.find(submap);
     CHECK(iterator != submap_ids_.end());
     return iterator->second;
@@ -120,14 +121,13 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // Grows the optimization problem to have an entry for every element of
   // 'insertion_submaps'.
   void GrowSubmapTransformsAsNeeded(
-      const std::vector<const mapping::Submap*>& insertion_submaps)
+      const std::vector<const Submap*>& insertion_submaps) REQUIRES(mutex_);
-      REQUIRES(mutex_);
 
   // Adds constraints for a scan, and starts scan matching in the background.
-  void ComputeConstraintsForScan(
+  void ComputeConstraintsForScan(const Submap* matching_submap,
-      const mapping::Submap* matching_submap,
+                                 std::vector<const Submap*> insertion_submaps,
-      std::vector<const mapping::Submap*> insertion_submaps,
+                                 const Submap* finished_submap,
-      const mapping::Submap* finished_submap, const transform::Rigid2d& pose)
+                                 const transform::Rigid2d& pose)
       REQUIRES(mutex_);
 
   // Computes constraints for a scan and submap pair.
@@ -135,8 +135,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
                          const mapping::SubmapId& submap_id) REQUIRES(mutex_);
 
   // Adds constraints for older scans whenever a new submap is finished.
-  void ComputeConstraintsForOldScans(const mapping::Submap* submap)
+  void ComputeConstraintsForOldScans(const Submap* submap) REQUIRES(mutex_);
-      REQUIRES(mutex_);
 
   // Registers the callback to run the optimization once all constraints have
   // been computed, that will also do all work that queue up in 'scan_queue_'.
@@ -185,8 +184,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
 
   // Submaps get assigned an ID and state as soon as they are seen, even
   // before they take part in the background computations.
-  std::map<const mapping::Submap*, mapping::SubmapId> submap_ids_
+  std::map<const Submap*, mapping::SubmapId> submap_ids_ GUARDED_BY(mutex_);
-      GUARDED_BY(mutex_);
   mapping::NestedVectorsById<SubmapData, mapping::SubmapId> submap_data_
       GUARDED_BY(mutex_);
 

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc

@@ -38,7 +38,7 @@ namespace cartographer {
 namespace mapping_2d {
 namespace sparse_pose_graph {
 
-transform::Rigid2d ComputeSubmapPose(const mapping::Submap& submap) {
+transform::Rigid2d ComputeSubmapPose(const Submap& submap) {
   return transform::Project2D(submap.local_pose());
 }
 
@@ -60,7 +60,7 @@ ConstraintBuilder::~ConstraintBuilder() {
 }
 
 void ConstraintBuilder::MaybeAddConstraint(
-    const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
+    const mapping::SubmapId& submap_id, const Submap* const submap,
     const mapping::NodeId& node_id, const sensor::PointCloud* const point_cloud,
     const transform::Rigid2d& initial_relative_pose) {
   if (initial_relative_pose.translation().norm() >
@@ -74,7 +74,7 @@ void ConstraintBuilder::MaybeAddConstraint(
     ++pending_computations_[current_computation_];
     const int current_computation = current_computation_;
     ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
-        submap_id, submap->finished_probability_grid(), [=]() EXCLUDES(mutex_) {
+        submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) {
           ComputeConstraint(submap_id, submap, node_id,
                             false,   /* match_full_submap */
                             nullptr, /* trajectory_connectivity */
@@ -85,7 +85,7 @@ void ConstraintBuilder::MaybeAddConstraint(
 }
 
 void ConstraintBuilder::MaybeAddGlobalConstraint(
-    const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
+    const mapping::SubmapId& submap_id, const Submap* const submap,
     const mapping::NodeId& node_id, const sensor::PointCloud* const point_cloud,
     mapping::TrajectoryConnectivity* const trajectory_connectivity) {
   common::MutexLocker locker(&mutex_);
@@ -94,7 +94,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
   ++pending_computations_[current_computation_];
   const int current_computation = current_computation_;
   ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
-      submap_id, submap->finished_probability_grid(), [=]() EXCLUDES(mutex_) {
+      submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) {
         ComputeConstraint(submap_id, submap, node_id,
                           true, /* match_full_submap */
                           trajectory_connectivity, point_cloud,
@@ -159,7 +159,7 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
 }
 
 void ConstraintBuilder::ComputeConstraint(
-    const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
+    const mapping::SubmapId& submap_id, const Submap* const submap,
     const mapping::NodeId& node_id, bool match_full_submap,
     mapping::TrajectoryConnectivity* trajectory_connectivity,
     const sensor::PointCloud* const point_cloud,

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h

@@ -35,7 +35,7 @@
 #include "cartographer/mapping/trajectory_connectivity.h"
 #include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
 #include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h"
-
+#include "cartographer/mapping_2d/submaps.h"
 #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
 #include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h"
 #include "cartographer/sensor/point_cloud.h"
@@ -47,7 +47,7 @@ namespace sparse_pose_graph {
 
 // Returns (map <- submap) where 'submap' is a coordinate system at the origin
 // of the Submap.
-transform::Rigid2d ComputeSubmapPose(const mapping::Submap& submap);
+transform::Rigid2d ComputeSubmapPose(const Submap& submap);
 
 // Asynchronously computes constraints.
 //
@@ -78,8 +78,7 @@ class ConstraintBuilder {
   // The pointees of 'submap' and 'point_cloud' must stay valid until all
   // computations are finished.
   void MaybeAddConstraint(const mapping::SubmapId& submap_id,
-                          const mapping::Submap* submap,
+                          const Submap* submap, const mapping::NodeId& node_id,
-                          const mapping::NodeId& node_id,
                           const sensor::PointCloud* point_cloud,
                           const transform::Rigid2d& initial_relative_pose);
 
@@ -92,7 +91,7 @@ class ConstraintBuilder {
   // The pointees of 'submap' and 'point_cloud' must stay valid until all
   // computations are finished.
   void MaybeAddGlobalConstraint(
-      const mapping::SubmapId& submap_id, const mapping::Submap* submap,
+      const mapping::SubmapId& submap_id, const Submap* submap,
       const mapping::NodeId& node_id, const sensor::PointCloud* point_cloud,
       mapping::TrajectoryConnectivity* trajectory_connectivity);
 
@@ -138,7 +137,7 @@ class ConstraintBuilder {
   // 'trajectory_connectivity'.
   // As output, it may create a new Constraint in 'constraint'.
   void ComputeConstraint(
-      const mapping::SubmapId& submap_id, const mapping::Submap* submap,
+      const mapping::SubmapId& submap_id, const Submap* submap,
       const mapping::NodeId& node_id, bool match_full_submap,
       mapping::TrajectoryConnectivity* trajectory_connectivity,
       const sensor::PointCloud* point_cloud,

cartographer/mapping_2d/sparse_pose_graph_test.cc

@@ -149,9 +149,9 @@ class SparsePoseGraphTest : public ::testing::Test {
     const sensor::PointCloud new_point_cloud = sensor::TransformPointCloud(
         point_cloud_,
         transform::Embed3D(current_pose_.inverse().cast<float>()));
-    const mapping::Submap* const matching_submap =
+    const Submap* const matching_submap =
         submaps_->Get(submaps_->matching_index());
-    std::vector<const mapping::Submap*> insertion_submaps;
+    std::vector<const Submap*> insertion_submaps;
     for (int insertion_index : submaps_->insertion_indices()) {
       insertion_submaps.push_back(submaps_->Get(insertion_index));
     }
@@ -161,9 +161,14 @@ class SparsePoseGraphTest : public ::testing::Test {
     constexpr int kTrajectoryId = 0;
     submaps_->InsertRangeData(TransformRangeData(
         range_data, transform::Embed3D(pose_estimate.cast<float>())));
-    sparse_pose_graph_->AddScan(
+
-        common::FromUniversal(0), transform::Rigid3d::Identity(), range_data,
+    const Submap* const finished_submap = insertion_submaps.front()->finished()
-        pose_estimate, kTrajectoryId, matching_submap, insertion_submaps);
+                                              ? insertion_submaps.front()
+                                              : nullptr;
+    sparse_pose_graph_->AddScan(common::FromUniversal(0),
+                                transform::Rigid3d::Identity(), range_data,
+                                pose_estimate, kTrajectoryId, matching_submap,
+                                insertion_submaps, finished_submap);
   }
 
   void MoveRelative(const transform::Rigid2d& movement) {

cartographer/mapping_2d/submaps.cc

@@ -162,7 +162,7 @@ Submaps::Submaps(const proto::SubmapsOptions& options)
 void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
   for (const int index : insertion_indices()) {
     Submap* submap = submaps_[index].get();
-    CHECK(submap->finished_probability_grid_ == nullptr);
+    CHECK(!submap->finished_);
     range_data_inserter_.Insert(range_data, &submap->probability_grid_);
     ++submap->num_range_data_;
   }
@@ -184,10 +184,10 @@ void Submaps::FinishSubmap(int index) {
   // Crop the finished Submap before inserting a new Submap to reduce peak
   // memory usage a bit.
   Submap* submap = submaps_[index].get();
-  CHECK(submap->finished_probability_grid_ == nullptr);
+  CHECK(!submap->finished_);
   submap->probability_grid_ =
       ComputeCroppedProbabilityGrid(submap->probability_grid_);
-  submap->finished_probability_grid_ = &submap->probability_grid_;
+  submap->finished_ = true;
   if (options_.output_debug_images()) {
     // Output the Submap that won't be changed from now on.
     WriteDebugImage("submap" + std::to_string(index) + ".webp",

cartographer/mapping_2d/submaps.h

@@ -46,6 +46,7 @@ class Submap : public mapping::Submap {
   Submap(const MapLimits& limits, const Eigen::Vector2f& origin);
 
   const ProbabilityGrid& probability_grid() const { return probability_grid_; }
+  const bool finished() const { return finished_; }
 
   void ToResponseProto(
       const transform::Rigid3d& global_submap_pose,
@@ -56,6 +57,7 @@ class Submap : public mapping::Submap {
   friend class Submaps;
 
   ProbabilityGrid probability_grid_;
+  bool finished_ = false;
 };
 
 // A container of Submaps.

cartographer/mapping_3d/global_trajectory_builder.cc

@@ -55,15 +55,19 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
     const sensor::PointCloud& ranges) {
   auto insertion_result =
       local_trajectory_builder_->AddRangefinderData(time, origin, ranges);
-
   if (insertion_result == nullptr) {
     return;
   }
 
+  const Submap* const finished_submap =
+      insertion_result->insertion_submaps.front()->finished()
+          ? insertion_result->insertion_submaps.front()
+          : nullptr;
   sparse_pose_graph_->AddScan(
       insertion_result->time, insertion_result->range_data_in_tracking,
       insertion_result->pose_observation, trajectory_id_,
-      insertion_result->matching_submap, insertion_result->insertion_submaps);
+      insertion_result->matching_submap, insertion_result->insertion_submaps,
+      finished_submap);
 }
 
 void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -90,7 +90,8 @@ void SparsePoseGraph::AddScan(
     common::Time time, const sensor::RangeData& range_data_in_tracking,
     const transform::Rigid3d& pose, const int trajectory_id,
     const Submap* const matching_submap,
-    const std::vector<const Submap*>& insertion_submaps) {
+    const std::vector<const Submap*>& insertion_submaps,
+    const Submap* const finished_submap) {
   const transform::Rigid3d optimized_pose(
       GetLocalToGlobalTransform(trajectory_id) * pose);
 
@@ -113,9 +114,6 @@ void SparsePoseGraph::AddScan(
     submap_ids_.emplace(insertion_submaps.back(), submap_id);
     submap_data_.at(submap_id).submap = insertion_submaps.back();
   }
-  const Submap* const finished_submap = insertion_submaps.front()->finished()
-                                            ? insertion_submaps.front()
-                                            : nullptr;
 
   // Make sure we have a sampler for this trajectory.
   if (!global_localization_samplers_[trajectory_id]) {

cartographer/mapping_3d/sparse_pose_graph.h

@@ -63,13 +63,15 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // Adds a new 'range_data_in_tracking' observation at 'time', and a 'pose'
   // that will later be optimized. The 'pose' was determined by scan matching
   // against the 'matching_submap' and the scan was inserted into the
-  // 'insertion_submaps'.
+  // 'insertion_submaps'. If not 'nullptr', 'finished_submap' is a freshly
+  // finished submap. It is also contained in 'insertion_submaps' for the last
+  // time.
   void AddScan(common::Time time,
                const sensor::RangeData& range_data_in_tracking,
                const transform::Rigid3d& pose, int trajectory_id,
                const Submap* matching_submap,
-               const std::vector<const Submap*>& insertion_submaps)
+               const std::vector<const Submap*>& insertion_submaps,
-      EXCLUDES(mutex_);
+               const Submap* finished_submap) EXCLUDES(mutex_);
 
   // Adds new IMU data to be used in the optimization.
   void AddImuData(int trajectory_id, common::Time time,