Change Submaps to only track the two active Submaps. (#348)

cartographer/mapping_2d/global_trajectory_builder.cc

@@ -34,9 +34,11 @@ int GlobalTrajectoryBuilder::num_submaps() {
 
 GlobalTrajectoryBuilder::SubmapData GlobalTrajectoryBuilder::GetSubmapData(
     const int submap_index) {
-  return {local_trajectory_builder_.submaps()->Get(submap_index),
-          sparse_pose_graph_->GetSubmapTransform(
-              mapping::SubmapId{trajectory_id_, submap_index})};
+  // TODO(hrapp): Get rid of this function and query the sparse pose graph
+  // directly.
+  const mapping::SubmapId submap_id{trajectory_id_, submap_index};
+  return {sparse_pose_graph_->GetSubmap(submap_id),
+          sparse_pose_graph_->GetSubmapTransform(submap_id)};
 }
 
 void GlobalTrajectoryBuilder::AddRangefinderData(

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -27,7 +27,7 @@ namespace mapping_2d {
 LocalTrajectoryBuilder::LocalTrajectoryBuilder(
     const proto::LocalTrajectoryBuilderOptions& options)
     : options_(options),
-      submaps_(options.submaps_options()),
+      active_submaps_(options.submaps_options()),
       motion_filter_(options_.motion_filter_options()),
       real_time_correlative_scan_matcher_(
           options_.real_time_correlative_scan_matcher_options()),
@@ -36,8 +36,6 @@ LocalTrajectoryBuilder::LocalTrajectoryBuilder(
 
 LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
 
-Submaps* LocalTrajectoryBuilder::submaps() { return &submaps_; }
-
 sensor::RangeData LocalTrajectoryBuilder::TransformAndFilterRangeData(
     const transform::Rigid3f& tracking_to_tracking_2d,
     const sensor::RangeData& range_data) const {
@@ -70,8 +68,8 @@ void LocalTrajectoryBuilder::ScanMatch(
     const transform::Rigid3d& tracking_to_tracking_2d,
     const sensor::RangeData& range_data_in_tracking_2d,
     transform::Rigid3d* pose_observation) {
-  const ProbabilityGrid& probability_grid =
-      submaps_.Get(submaps_.matching_index())->probability_grid();
+  std::shared_ptr<const Submap> matching_submap =
+      active_submaps_.submaps().front();
   transform::Rigid2d pose_prediction_2d =
       transform::Project2D(pose_prediction * tracking_to_tracking_2d.inverse());
   // The online correlative scan matcher will refine the initial estimate for
@@ -84,14 +82,15 @@ void LocalTrajectoryBuilder::ScanMatch(
   if (options_.use_online_correlative_scan_matching()) {
     real_time_correlative_scan_matcher_.Match(
         pose_prediction_2d, filtered_point_cloud_in_tracking_2d,
-        probability_grid, &initial_ceres_pose);
+        matching_submap->probability_grid(), &initial_ceres_pose);
   }
 
   transform::Rigid2d tracking_2d_to_map;
   ceres::Solver::Summary summary;
   ceres_scan_matcher_.Match(pose_prediction_2d, initial_ceres_pose,
                             filtered_point_cloud_in_tracking_2d,
-                            probability_grid, &tracking_2d_to_map, &summary);
+                            matching_submap->probability_grid(),
+                            &tracking_2d_to_map, &summary);
 
   *pose_observation =
       transform::Embed3D(tracking_2d_to_map) * tracking_to_tracking_2d;
@@ -177,10 +176,10 @@ LocalTrajectoryBuilder::AddHorizontalRangeData(
   }
 
   std::vector<std::shared_ptr<const Submap>> insertion_submaps;
-  for (int insertion_index : submaps_.insertion_indices()) {
-    insertion_submaps.push_back(submaps_.Get(insertion_index));
+  for (std::shared_ptr<Submap> submap : active_submaps_.submaps()) {
+    insertion_submaps.push_back(submap);
   }
-  submaps_.InsertRangeData(
+  active_submaps_.InsertRangeData(
       TransformRangeData(range_data_in_tracking_2d,
                          transform::Embed3D(pose_estimate_2d.cast<float>())));
 

cartographer/mapping_2d/local_trajectory_builder.h

@@ -62,8 +62,6 @@ class LocalTrajectoryBuilder {
                   const Eigen::Vector3d& angular_velocity);
   void AddOdometerData(common::Time time, const transform::Rigid3d& pose);
 
-  Submaps* submaps();
-
  private:
   sensor::RangeData TransformAndFilterRangeData(
       const transform::Rigid3f& tracking_to_tracking_2d,
@@ -83,7 +81,7 @@ class LocalTrajectoryBuilder {
   void Predict(common::Time time);
 
   const proto::LocalTrajectoryBuilderOptions options_;
-  Submaps submaps_;
+  ActiveSubmaps active_submaps_;
   mapping::GlobalTrajectoryBuilderInterface::PoseEstimate last_pose_estimate_;
 
   // Current 'pose_estimate_' and 'velocity_estimate_' at 'time_'.

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -424,6 +424,12 @@ SparsePoseGraph::GetTrajectoryNodes() {
   return trajectory_nodes_.data();
 }
 
+std::shared_ptr<const Submap> SparsePoseGraph::GetSubmap(
+    const mapping::SubmapId& submap_id) {
+  common::MutexLocker locker(&mutex_);
+  return submap_data_.at(submap_id).submap;
+}
+
 std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
   common::MutexLocker locker(&mutex_);
   return constraints_;

cartographer/mapping_2d/sparse_pose_graph.h

@@ -90,6 +90,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       EXCLUDES(mutex_) override;
   std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
       override EXCLUDES(mutex_);
+  std::shared_ptr<const Submap> GetSubmap(const mapping::SubmapId& submap_id)
+      EXCLUDES(mutex_);
   std::vector<Constraint> constraints() override EXCLUDES(mutex_);
 
  private:

cartographer/mapping_2d/sparse_pose_graph_test.cc

@@ -60,7 +60,7 @@ class SparsePoseGraphTest : public ::testing::Test {
               miss_probability = 0.495,
             },
           })text");
-      submaps_ = common::make_unique<Submaps>(
+      active_submaps_ = common::make_unique<ActiveSubmaps>(
           CreateSubmapsOptions(parameter_dictionary.get()));
     }
 
@@ -150,14 +150,14 @@ class SparsePoseGraphTest : public ::testing::Test {
         point_cloud_,
         transform::Embed3D(current_pose_.inverse().cast<float>()));
     std::vector<std::shared_ptr<const Submap>> insertion_submaps;
-    for (int insertion_index : submaps_->insertion_indices()) {
-      insertion_submaps.push_back(submaps_->Get(insertion_index));
+    for (auto submap : active_submaps_->submaps()) {
+      insertion_submaps.push_back(submap);
     }
     const sensor::RangeData range_data{
         Eigen::Vector3f::Zero(), new_point_cloud, {}};
     const transform::Rigid2d pose_estimate = noise * current_pose_;
     constexpr int kTrajectoryId = 0;
-    submaps_->InsertRangeData(TransformRangeData(
+    active_submaps_->InsertRangeData(TransformRangeData(
         range_data, transform::Embed3D(pose_estimate.cast<float>())));
 
     sparse_pose_graph_->AddScan(
@@ -170,7 +170,7 @@ class SparsePoseGraphTest : public ::testing::Test {
   }
 
   sensor::PointCloud point_cloud_;
-  std::unique_ptr<Submaps> submaps_;
+  std::unique_ptr<ActiveSubmaps> active_submaps_;
   common::ThreadPool thread_pool_;
   std::unique_ptr<SparsePoseGraph> sparse_pose_graph_;
   transform::Rigid2d current_pose_;

cartographer/mapping_2d/submaps.cc

@@ -151,17 +151,16 @@ void Submap::ToResponseProto(
       transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));
 }
 
-Submaps::Submaps(const proto::SubmapsOptions& options)
+ActiveSubmaps::ActiveSubmaps(const proto::SubmapsOptions& options)
     : options_(options),
       range_data_inserter_(options.range_data_inserter_options()) {
   // We always want to have at least one likelihood field which we can return,
   // and will create it at the origin in absence of a better choice.
   AddSubmap(Eigen::Vector2f::Zero());
 }
 
-void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
-  for (const int index : insertion_indices()) {
-    Submap* submap = submaps_[index].get();
+void ActiveSubmaps::InsertRangeData(const sensor::RangeData& range_data) {
+  for (auto& submap : submaps_) {
     CHECK(!submap->finished_);
     range_data_inserter_.Insert(range_data, &submap->probability_grid_);
     ++submap->num_range_data_;
@@ -171,46 +170,32 @@ void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
   }
 }
 
-std::shared_ptr<const Submap> Submaps::Get(int index) const {
-  CHECK_GE(index, 0);
-  CHECK_LT(index, size());
-  return submaps_[index];
+std::vector<std::shared_ptr<Submap>> ActiveSubmaps::submaps() const {
+  return submaps_;
 }
 
-int Submaps::size() const { return submaps_.size(); }
+int ActiveSubmaps::matching_index() const { return matching_submap_index_; }
 
-int Submaps::matching_index() const {
-  if (size() > 1) {
-    return size() - 2;
-  }
-  return size() - 1;
-}
-
-std::vector<int> Submaps::insertion_indices() const {
-  if (size() > 1) {
-    return {size() - 2, size() - 1};
-  }
-  return {size() - 1};
-}
-
-void Submaps::FinishSubmap(int index) {
+void ActiveSubmaps::FinishSubmap() {
   // Crop the finished Submap before inserting a new Submap to reduce peak
   // memory usage a bit.
-  Submap* submap = submaps_[index].get();
+  Submap* submap = submaps_.front().get();
   CHECK(!submap->finished_);
   submap->probability_grid_ =
       ComputeCroppedProbabilityGrid(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",
-                    submap->probability_grid_);
+    WriteDebugImage("submap" + std::to_string(matching_submap_index_) + ".webp",
+                    submaps_.front()->probability_grid_);
   }
+  ++matching_submap_index_;
+  submaps_.erase(submaps_.begin());
 }
 
-void Submaps::AddSubmap(const Eigen::Vector2f& origin) {
-  if (size() > 1) {
-    FinishSubmap(size() - 2);
+void ActiveSubmaps::AddSubmap(const Eigen::Vector2f& origin) {
+  if (submaps_.size() > 1) {
+    FinishSubmap();
   }
   const int num_cells_per_dimension =
       common::RoundToInt(2. * options_.half_length() / options_.resolution()) +
@@ -221,7 +206,7 @@ void Submaps::AddSubmap(const Eigen::Vector2f& origin) {
                     options_.half_length() * Eigen::Vector2d::Ones(),
                 CellLimits(num_cells_per_dimension, num_cells_per_dimension)),
       origin));
-  LOG(INFO) << "Added submap " << size();
+  LOG(INFO) << "Added submap " << matching_submap_index_ + submaps_.size();
 }
 
 }  // namespace mapping_2d

cartographer/mapping_2d/submaps.h

@@ -54,50 +54,43 @@ class Submap : public mapping::Submap {
 
  private:
   // TODO(hrapp): Remove friend declaration.
-  friend class Submaps;
+  friend class ActiveSubmaps;
 
   ProbabilityGrid probability_grid_;
   bool finished_ = false;
 };
 
-// Submaps is a sequence of maps to which scans are matched and into which scans
-// are inserted.
-//
 // Except during initialization when only a single submap exists, there are
 // always two submaps into which scans are inserted: an old submap that is used
 // for matching, and a new one, which will be used for matching next, that is
 // being initialized.
 //
 // Once a certain number of scans have been inserted, the new submap is
 // considered initialized: the old submap is no longer changed, the "new" submap
-// is now the "old" submap and is used for scan-to-map matching. Moreover,
-// a "new" submap gets inserted.
-class Submaps {
+// is now the "old" submap and is used for scan-to-map matching. Moreover, a
+// "new" submap gets created. The "old" submap is forgotten by this object.
+class ActiveSubmaps {
  public:
-  explicit Submaps(const proto::SubmapsOptions& options);
-
-  Submaps(const Submaps&) = delete;
-  Submaps& operator=(const Submaps&) = delete;
+  explicit ActiveSubmaps(const proto::SubmapsOptions& options);
 
-  std::shared_ptr<const Submap> Get(int index) const;
-  int size() const;
+  ActiveSubmaps(const ActiveSubmaps&) = delete;
+  ActiveSubmaps& operator=(const ActiveSubmaps&) = delete;
 
   // Returns the index of the newest initialized Submap which can be
   // used for scan-to-map matching.
   int matching_index() const;
 
-  // Returns the indices of the Submap into which point clouds will
-  // be inserted.
-  std::vector<int> insertion_indices() const;
-
   // Inserts 'range_data' into the Submap collection.
   void InsertRangeData(const sensor::RangeData& range_data);
 
+  std::vector<std::shared_ptr<Submap>> submaps() const;
+
  private:
-  void FinishSubmap(int index);
+  void FinishSubmap();
   void AddSubmap(const Eigen::Vector2f& origin);
 
   const proto::SubmapsOptions options_;
+  int matching_submap_index_ = 0;
   std::vector<std::shared_ptr<Submap>> submaps_;
   RangeDataInserter range_data_inserter_;
 };

cartographer/mapping_2d/submaps_test.cc

@@ -17,6 +17,8 @@
 #include "cartographer/mapping_2d/submaps.h"
 
 #include <map>
+#include <memory>
+#include <set>
 #include <string>
 
 #include "cartographer/common/lua_parameter_dictionary.h"
@@ -45,19 +47,26 @@ TEST(SubmapsTest, TheRightNumberOfScansAreInserted) {
       "miss_probability = 0.495, "
       "},"
       "}");
-  Submaps submaps{CreateSubmapsOptions(parameter_dictionary.get())};
+  ActiveSubmaps submaps{CreateSubmapsOptions(parameter_dictionary.get())};
+  std::set<std::shared_ptr<Submap>> all_submaps;
   for (int i = 0; i != 1000; ++i) {
     submaps.InsertRangeData({Eigen::Vector3f::Zero(), {}, {}});
-    const int matching = submaps.matching_index();
     // Except for the first, maps should only be returned after enough scans.
-    if (matching != 0) {
-      EXPECT_LE(kNumRangeData, submaps.Get(matching)->num_range_data());
+    for (auto submap : submaps.submaps()) {
+      all_submaps.insert(submap);
+    }
+    if (submaps.matching_index() != 0) {
+      EXPECT_LE(kNumRangeData, submaps.submaps().front()->num_range_data());
     }
   }
-  for (int i = 0; i != submaps.size() - 2; ++i) {
-    // Submaps should not be left without the right number of scans in them.
-    EXPECT_EQ(kNumRangeData * 2, submaps.Get(i)->num_range_data());
+  int correct_num_scans = 0;
+  for (const auto& submap : all_submaps) {
+    if (submap->num_range_data() == kNumRangeData * 2) {
+      ++correct_num_scans;
+    }
   }
+  // Submaps should not be left without the right number of scans in them.
+  EXPECT_EQ(correct_num_scans, all_submaps.size() - 2);
 }
 
 }  // namespace

cartographer/mapping_3d/global_trajectory_builder.cc

@@ -36,9 +36,11 @@ int GlobalTrajectoryBuilder::num_submaps() {
 
 GlobalTrajectoryBuilder::SubmapData GlobalTrajectoryBuilder::GetSubmapData(
     const int submap_index) {
-  return {local_trajectory_builder_->submaps()->Get(submap_index),
-          sparse_pose_graph_->GetSubmapTransform(
-              mapping::SubmapId{trajectory_id_, submap_index})};
+  // TODO(hrapp): Get rid of this function and query the sparse pose graph
+  // directly.
+  const mapping::SubmapId submap_id{trajectory_id_, submap_index};
+  return {sparse_pose_graph_->GetSubmap(submap_id),
+          sparse_pose_graph_->GetSubmapTransform(submap_id)};
 }
 
 void GlobalTrajectoryBuilder::AddImuData(