Create first submap with first scan (#1253)

The first submap is now created with first call to `ActiveSubmaps:InsertRangeData`.

Originally the first submap was created at the origin. Creating the first submap with the first range data insertion allows to align the first submap with the gravity estimate of the first scan. This change makes the interface of ActiveSubmaps most consistent, as `ActiveSubmaps::submaps()` will return the correct active submaps after insertion.

The change affects the result of cartographer. The results were verified and provide the same quality as current master.

cartographer/mapping/2d/submap_2d.cc

@@ -127,23 +127,26 @@ void Submap2D::Finish() {
 }
 
 ActiveSubmaps2D::ActiveSubmaps2D(const proto::SubmapsOptions2D& options)
-    : options_(options), range_data_inserter_(CreateRangeDataInserter()) {
-  // 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());
+    : options_(options), range_data_inserter_(CreateRangeDataInserter()) {}
+
+std::vector<std::shared_ptr<const Submap2D>> ActiveSubmaps2D::submaps() const {
+  return std::vector<std::shared_ptr<const Submap2D>>(submaps_.begin(),
+                                                      submaps_.end());
 }
 
-std::vector<std::shared_ptr<Submap2D>> ActiveSubmaps2D::submaps() const {
-  return submaps_;
+std::vector<std::shared_ptr<const Submap2D>> ActiveSubmaps2D::InsertRangeData(
+    const sensor::RangeData& range_data) {
+  if (submaps_.empty() ||
+      submaps_.back()->num_range_data() == options_.num_range_data()) {
+    AddSubmap(range_data.origin.head<2>());
   }
-
-void ActiveSubmaps2D::InsertRangeData(const sensor::RangeData& range_data) {
   for (auto& submap : submaps_) {
     submap->InsertRangeData(range_data, range_data_inserter_.get());
   }
-  if (submaps_.back()->num_range_data() == options_.num_range_data()) {
-    AddSubmap(range_data.origin.head<2>());
+  if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) {
+    submaps_.front()->Finish();
   }
+  return submaps();
 }
 
 std::unique_ptr<RangeDataInserterInterface>
@@ -164,17 +167,12 @@ std::unique_ptr<GridInterface> ActiveSubmaps2D::CreateGrid(
                 CellLimits(kInitialSubmapSize, kInitialSubmapSize)));
 }
 
-void ActiveSubmaps2D::FinishSubmap() {
-  Submap2D* submap = submaps_.front().get();
-  submap->Finish();
-  submaps_.erase(submaps_.begin());
-}
-
 void ActiveSubmaps2D::AddSubmap(const Eigen::Vector2f& origin) {
-  if (submaps_.size() > 1) {
+  if (submaps_.size() >= 2) {
     // This will crop the finished Submap before inserting a new Submap to
     // reduce peak memory usage a bit.
-    FinishSubmap();
+    CHECK(submaps_.front()->finished());
+    submaps_.erase(submaps_.begin());
   }
 
   submaps_.push_back(common::make_unique<Submap2D>(

cartographer/mapping/2d/submap_2d.h

@@ -63,10 +63,11 @@ class Submap2D : public Submap {
   std::unique_ptr<Grid2D> grid_;
 };
 
-// Except during initialization when only a single submap exists, there are
-// always two submaps into which range data is inserted: an old submap that is
-// used for matching, and a new one, which will be used for matching next, that
-// is being initialized.
+// The first active submap will be created on the insertion of the first range
+// data. Except during this initialization when no or only one single submap
+// exists, there are always two submaps into which range data is 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 range data have been inserted, the new submap is
 // considered initialized: the old submap is no longer changed, the "new" submap
@@ -80,9 +81,10 @@ class ActiveSubmaps2D {
   ActiveSubmaps2D& operator=(const ActiveSubmaps2D&) = delete;
 
   // Inserts 'range_data' into the Submap collection.
-  void InsertRangeData(const sensor::RangeData& range_data);
+  std::vector<std::shared_ptr<const Submap2D>> InsertRangeData(
+      const sensor::RangeData& range_data);
 
-  std::vector<std::shared_ptr<Submap2D>> submaps() const;
+  std::vector<std::shared_ptr<const Submap2D>> submaps() const;
 
  private:
   std::unique_ptr<RangeDataInserterInterface> CreateRangeDataInserter();

cartographer/mapping/2d/submap_2d_test.cc

@@ -66,12 +66,13 @@ TEST(Submap2DTest, TheRightNumberOfRangeDataAreInserted) {
       "},"
       "}");
   ActiveSubmaps2D submaps{CreateSubmapsOptions2D(parameter_dictionary.get())};
-  std::set<std::shared_ptr<Submap2D>> all_submaps;
+  std::set<std::shared_ptr<const Submap2D>> all_submaps;
   for (int i = 0; i != 1000; ++i) {
+    auto insertion_submaps =
         submaps.InsertRangeData({Eigen::Vector3f::Zero(), {}, {}});
     // Except for the first, maps should only be returned after enough range
     // data.
-    for (const auto& submap : submaps.submaps()) {
+    for (const auto& submap : insertion_submaps) {
       all_submaps.insert(submap);
     }
     if (submaps.submaps().size() > 1) {
@@ -79,14 +80,19 @@ TEST(Submap2DTest, TheRightNumberOfRangeDataAreInserted) {
     }
   }
   EXPECT_EQ(2, submaps.submaps().size());
-  int correct_num_range_data = 0;
+  int correct_num_finished_submaps = 0;
+  int num_unfinished_submaps = 0;
   for (const auto& submap : all_submaps) {
     if (submap->num_range_data() == kNumRangeData * 2) {
-      ++correct_num_range_data;
+      ++correct_num_finished_submaps;
+    } else {
+      EXPECT_EQ(kNumRangeData, submap->num_range_data());
+      ++num_unfinished_submaps;
     }
   }
   // Submaps should not be left without the right number of range data in them.
-  EXPECT_EQ(correct_num_range_data, all_submaps.size() - 2);
+  EXPECT_EQ(correct_num_finished_submaps, all_submaps.size() - 1);
+  EXPECT_EQ(1, num_unfinished_submaps);
 }
 
 TEST(Submap2DTest, ToFromProto) {

cartographer/mapping/3d/submap_3d.cc

@@ -281,35 +281,36 @@ void Submap3D::Finish() {
 
 ActiveSubmaps3D::ActiveSubmaps3D(const proto::SubmapsOptions3D& options)
     : options_(options),
-      range_data_inserter_(options.range_data_inserter_options()) {
-  // We always want to have at least one submap which we can return and will
-  // create it at the origin in absence of a better choice.
-  //
-  // TODO(whess): Start with no submaps, so that all of them can be
-  // approximately gravity aligned.
-  AddSubmap(transform::Rigid3d::Identity());
+      range_data_inserter_(options.range_data_inserter_options()) {}
+
+std::vector<std::shared_ptr<const Submap3D>> ActiveSubmaps3D::submaps() const {
+  return std::vector<std::shared_ptr<const Submap3D>>(submaps_.begin(),
+                                                      submaps_.end());
 }
 
-std::vector<std::shared_ptr<Submap3D>> ActiveSubmaps3D::submaps() const {
-  return submaps_;
-}
-
-void ActiveSubmaps3D::InsertRangeData(
+std::vector<std::shared_ptr<const Submap3D>> ActiveSubmaps3D::InsertRangeData(
     const sensor::RangeData& range_data,
     const Eigen::Quaterniond& gravity_alignment) {
+  if (submaps_.empty() ||
+      submaps_.back()->num_range_data() == options_.num_range_data()) {
+    AddSubmap(transform::Rigid3d(range_data.origin.cast<double>(),
+                                 gravity_alignment));
+  }
   for (auto& submap : submaps_) {
     submap->InsertRangeData(range_data, range_data_inserter_,
                             options_.high_resolution_max_range());
   }
-  if (submaps_.back()->num_range_data() == options_.num_range_data()) {
-    AddSubmap(transform::Rigid3d(range_data.origin.cast<double>(),
-                                 gravity_alignment));
+  if (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) {
+    submaps_.front()->Finish();
   }
+  return submaps();
 }
 
 void ActiveSubmaps3D::AddSubmap(const transform::Rigid3d& local_submap_pose) {
-  if (submaps_.size() > 1) {
-    submaps_.front()->Finish();
+  if (submaps_.size() >= 2) {
+    // This will crop the finished Submap before inserting a new Submap to
+    // reduce peak memory usage a bit.
+    CHECK(submaps_.front()->finished());
     submaps_.erase(submaps_.begin());
   }
   submaps_.emplace_back(new Submap3D(options_.high_resolution(),

cartographer/mapping/3d/submap_3d.h

@@ -72,10 +72,11 @@ class Submap3D : public Submap {
   std::unique_ptr<HybridGrid> low_resolution_hybrid_grid_;
 };
 
-// Except during initialization when only a single submap exists, there are
-// always two submaps into which range data is inserted: an old submap that is
-// used for matching, and a new one, which will be used for matching next, that
-// is being initialized.
+// The first active submap will be created on the insertion of the first range
+// data. Except during this initialization when no or only one single submap
+// exists, there are always two submaps into which range data is 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 range data have been inserted, the new submap is
 // considered initialized: the old submap is no longer changed, the "new" submap
@@ -91,10 +92,11 @@ class ActiveSubmaps3D {
   // Inserts 'range_data' into the Submap collection. 'gravity_alignment' is
   // used for the orientation of new submaps so that the z axis approximately
   // aligns with gravity.
-  void InsertRangeData(const sensor::RangeData& range_data,
+  std::vector<std::shared_ptr<const Submap3D>> InsertRangeData(
+      const sensor::RangeData& range_data,
       const Eigen::Quaterniond& gravity_alignment);
 
-  std::vector<std::shared_ptr<Submap3D>> submaps() const;
+  std::vector<std::shared_ptr<const Submap3D>> submaps() const;
 
  private:
   void AddSubmap(const transform::Rigid3d& local_submap_pose);

cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc

@@ -63,6 +63,9 @@ LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter(
 std::unique_ptr<transform::Rigid2d> LocalTrajectoryBuilder2D::ScanMatch(
     const common::Time time, const transform::Rigid2d& pose_prediction,
     const sensor::PointCloud& filtered_gravity_aligned_point_cloud) {
+  if (active_submaps_.submaps().empty()) {
+    return common::make_unique<transform::Rigid2d>(pose_prediction);
+  }
   std::shared_ptr<const Submap2D> matching_submap =
       active_submaps_.submaps().front();
   // The online correlative scan matcher will refine the initial estimate for
@@ -258,15 +261,8 @@ LocalTrajectoryBuilder2D::InsertIntoSubmap(
   if (motion_filter_.IsSimilar(time, pose_estimate)) {
     return nullptr;
   }
-
-  // Querying the active submaps must be done here before calling
-  // InsertRangeData() since the queried values are valid for next insertion.
-  std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;
-  for (const std::shared_ptr<Submap2D>& submap : active_submaps_.submaps()) {
-    insertion_submaps.push_back(submap);
-  }
+  std::vector<std::shared_ptr<const Submap2D>> insertion_submaps =
       active_submaps_.InsertRangeData(range_data_in_local);
-
   return common::make_unique<InsertionResult>(InsertionResult{
       std::make_shared<const TrajectoryNode::Data>(TrajectoryNode::Data{
           time,

cartographer/mapping/internal/2d/pose_graph_2d_test.cc

@@ -171,17 +171,16 @@ class PoseGraph2DTest : public ::testing::Test {
     const sensor::PointCloud new_point_cloud = sensor::TransformPointCloud(
         point_cloud_,
         transform::Embed3D(current_pose_.inverse().cast<float>()));
-    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;
-    for (const 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;
     active_submaps_->InsertRangeData(TransformRangeData(
         range_data, transform::Embed3D(pose_estimate.cast<float>())));
-
+    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;
+    for (const auto& submap : active_submaps_->submaps()) {
+      insertion_submaps.push_back(submap);
+    }
     pose_graph_->AddNode(
         std::make_shared<const TrajectoryNode::Data>(
             TrajectoryNode::Data{common::FromUniversal(0),

cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc

@@ -61,6 +61,9 @@ std::unique_ptr<transform::Rigid3d> LocalTrajectoryBuilder3D::ScanMatch(
     const transform::Rigid3d& pose_prediction,
     const sensor::PointCloud& low_resolution_point_cloud_in_tracking,
     const sensor::PointCloud& high_resolution_point_cloud_in_tracking) {
+  if (active_submaps_.submaps().empty()) {
+    return common::make_unique<transform::Rigid3d>(pose_prediction);
+  }
   std::shared_ptr<const mapping::Submap3D> matching_submap =
       active_submaps_.submaps().front();
   transform::Rigid3d initial_ceres_pose =
@@ -248,7 +251,6 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData(
     LOG(WARNING) << "Dropped empty high resolution point cloud data.";
     return nullptr;
   }
-
   sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter(
       options_.low_resolution_adaptive_voxel_filter_options());
   const sensor::PointCloud low_resolution_point_cloud_in_tracking =
@@ -332,13 +334,7 @@ LocalTrajectoryBuilder3D::InsertIntoSubmap(
   if (motion_filter_.IsSimilar(time, pose_estimate)) {
     return nullptr;
   }
-  // Querying the active submaps must be done here before calling
-  // InsertRangeData() since the queried values are valid for next insertion.
-  std::vector<std::shared_ptr<const mapping::Submap3D>> insertion_submaps;
-  for (const std::shared_ptr<mapping::Submap3D>& submap :
-       active_submaps_.submaps()) {
-    insertion_submaps.push_back(submap);
-  }
+  std::vector<std::shared_ptr<const mapping::Submap3D>> insertion_submaps =
       active_submaps_.InsertRangeData(filtered_range_data_in_local,
                                       gravity_alignment);
   const Eigen::VectorXf rotational_scan_matcher_histogram =