Simplify submap versions. (#235)

We now only keep track of the number of inserted range data.

cartographer/mapping/map_builder.cc

@@ -141,8 +141,7 @@ string MapBuilder::SubmapToProto(const int trajectory_id,
            " submaps in this trajectory.";
   }
 
-  response->set_submap_version(
+  response->set_submap_version(submaps->Get(submap_index)->num_range_data);
-      submaps->Get(submap_index)->end_range_data_index);
   const std::vector<transform::Rigid3d> submap_transforms =
       sparse_pose_graph_->GetSubmapTransforms(*submaps);
   CHECK_EQ(submap_transforms.size(), submaps->size());

cartographer/mapping/submaps.h

@@ -64,14 +64,11 @@ inline uint8 ProbabilityToLogOddsInteger(const float probability) {
 }
 
 // An individual submap, which has an initial position 'origin', keeps track of
-// which range data were inserted into it, and sets the
+// how many range data were inserted into it, and sets the
 // 'finished_probability_grid' to be used for loop closing once the map no
 // longer changes.
 struct Submap {
-  Submap(const Eigen::Vector3f& origin, int begin_range_data_index)
+  Submap(const Eigen::Vector3f& origin) : origin(origin) {}
-      : origin(origin),
-        begin_range_data_index(begin_range_data_index),
-        end_range_data_index(begin_range_data_index) {}
 
   transform::Rigid3d local_pose() const {
     return transform::Rigid3d::Translation(origin.cast<double>());
@@ -80,10 +77,8 @@ struct Submap {
   // Origin of this submap.
   Eigen::Vector3f origin;
 
-  // This Submap contains RangeData with indices in the range
+  // Number of RangeData inserted.
-  // ['begin_range_data_index', 'end_range_data_index').
+  int num_range_data = 0;
-  int begin_range_data_index;
-  int end_range_data_index;
 
   // The 'finished_probability_grid' when this submap is finished and will not
   // change anymore. Otherwise, this is nullptr and the next call to

cartographer/mapping_2d/submaps.cc

@@ -101,10 +101,8 @@ proto::SubmapsOptions CreateSubmapsOptions(
   return options;
 }
 
-Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin,
+Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin)
-               const int begin_range_data_index)
+    : mapping::Submap(Eigen::Vector3f(origin.x(), origin.y(), 0.)),
-    : mapping::Submap(Eigen::Vector3f(origin.x(), origin.y(), 0.),
-                      begin_range_data_index),
       probability_grid(limits) {}
 
 Submaps::Submaps(const proto::SubmapsOptions& options)
@@ -116,16 +114,14 @@ Submaps::Submaps(const proto::SubmapsOptions& options)
 }
 
 void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
-  CHECK_LT(num_range_data_, std::numeric_limits<int>::max());
-  ++num_range_data_;
   for (const int index : insertion_indices()) {
     Submap* submap = submaps_[index].get();
     CHECK(submap->finished_probability_grid == nullptr);
     range_data_inserter_.Insert(range_data, &submap->probability_grid);
-    submap->end_range_data_index = num_range_data_;
+    ++submap->num_range_data;
   }
-  ++num_range_data_in_last_submap_;
+  const Submap* const last_submap = Get(size() - 1);
-  if (num_range_data_in_last_submap_ == options_.num_range_data()) {
+  if (last_submap->num_range_data == options_.num_range_data()) {
     AddSubmap(range_data.origin.head<2>());
   }
 }
@@ -173,9 +169,8 @@ void Submaps::AddSubmap(const Eigen::Vector2f& origin) {
                 origin.cast<double>() +
                     options_.half_length() * Eigen::Vector2d::Ones(),
                 CellLimits(num_cells_per_dimension, num_cells_per_dimension)),
-      origin, num_range_data_));
+      origin));
   LOG(INFO) << "Added submap " << size();
-  num_range_data_in_last_submap_ = 0;
 }
 
 }  // namespace mapping_2d

cartographer/mapping_2d/submaps.h

@@ -42,8 +42,7 @@ proto::SubmapsOptions CreateSubmapsOptions(
     common::LuaParameterDictionary* parameter_dictionary);
 
 struct Submap : public mapping::Submap {
-  Submap(const MapLimits& limits, const Eigen::Vector2f& origin,
+  Submap(const MapLimits& limits, const Eigen::Vector2f& origin);
-         int begin_range_data_index);
 
   ProbabilityGrid probability_grid;
 };
@@ -74,12 +73,6 @@ class Submaps : public mapping::Submaps {
 
   std::vector<std::unique_ptr<Submap>> submaps_;
   RangeDataInserter range_data_inserter_;
-
-  // Number of RangeData inserted.
-  int num_range_data_ = 0;
-
-  // Number of RangeData inserted since the last Submap was added.
-  int num_range_data_in_last_submap_ = 0;
 };
 
 }  // namespace mapping_2d

cartographer/mapping_2d/submaps_test.cc

@@ -46,23 +46,17 @@ TEST(SubmapsTest, TheRightNumberOfScansAreInserted) {
       "},"
       "}");
   Submaps submaps{CreateSubmapsOptions(parameter_dictionary.get())};
-  auto num_inserted = [&submaps](const int i) {
-    return submaps.Get(i)->end_range_data_index -
-           submaps.Get(i)->begin_range_data_index;
-  };
   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, num_inserted(matching));
+      EXPECT_LE(kNumRangeData, submaps.Get(matching)->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, num_inserted(i));
+    EXPECT_EQ(kNumRangeData * 2, submaps.Get(i)->num_range_data);
-    EXPECT_EQ(i * kNumRangeData, submaps.Get(i)->begin_range_data_index);
-    EXPECT_EQ((i + 2) * kNumRangeData, submaps.Get(i)->end_range_data_index);
   }
 }
 

cartographer/mapping_3d/global_trajectory_builder.cc

@@ -45,19 +45,18 @@ void GlobalTrajectoryBuilder::AddImuData(
 void GlobalTrajectoryBuilder::AddRangefinderData(
     const common::Time time, const Eigen::Vector3f& origin,
     const sensor::PointCloud& ranges) {
-  auto insertion_result =
+  auto insertion_result = local_trajectory_builder_->AddRangefinderData(
-      local_trajectory_builder_->AddRangefinderData(time, origin, ranges);
+      time, origin, ranges, sparse_pose_graph_->GetNextTrajectoryNodeIndex());
 
   if (insertion_result == nullptr) {
     return;
   }
 
-  const int trajectory_node_index = sparse_pose_graph_->AddScan(
+  sparse_pose_graph_->AddScan(
       insertion_result->time, insertion_result->range_data_in_tracking,
       insertion_result->pose_observation, insertion_result->covariance_estimate,
       insertion_result->submaps, insertion_result->matching_submap,
       insertion_result->insertion_submaps);
-  local_trajectory_builder_->AddTrajectoryNodeIndex(trajectory_node_index);
 }
 
 void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,

cartographer/mapping_3d/kalman_local_trajectory_builder.cc

@@ -72,7 +72,7 @@ void KalmanLocalTrajectoryBuilder::AddImuData(
 std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
 KalmanLocalTrajectoryBuilder::AddRangefinderData(
     const common::Time time, const Eigen::Vector3f& origin,
-    const sensor::PointCloud& ranges) {
+    const sensor::PointCloud& ranges, int next_trajectory_node_index) {
   if (!pose_tracker_) {
     LOG(INFO) << "PoseTracker not yet initialized.";
     return nullptr;
@@ -114,15 +114,18 @@ KalmanLocalTrajectoryBuilder::AddRangefinderData(
   if (num_accumulated_ >= options_.scans_per_accumulation()) {
     num_accumulated_ = 0;
     return AddAccumulatedRangeData(
-        time, sensor::TransformRangeData(accumulated_range_data_,
+        time,
-                                         tracking_delta.inverse()));
+        sensor::TransformRangeData(accumulated_range_data_,
+                                   tracking_delta.inverse()),
+        next_trajectory_node_index);
   }
   return nullptr;
 }
 
 std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
 KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
-    const common::Time time, const sensor::RangeData& range_data_in_tracking) {
+    const common::Time time, const sensor::RangeData& range_data_in_tracking,
+    const int trajectory_node_index) {
   const sensor::RangeData filtered_range_data = {
       range_data_in_tracking.origin,
       sensor::VoxelFiltered(range_data_in_tracking.returns,
@@ -181,7 +184,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
                                   pose_observation.cast<float>())};
 
   return InsertIntoSubmap(time, filtered_range_data, pose_observation,
-                          covariance_estimate);
+                          covariance_estimate, trajectory_node_index);
 }
 
 void KalmanLocalTrajectoryBuilder::AddOdometerData(
@@ -206,16 +209,12 @@ KalmanLocalTrajectoryBuilder::pose_estimate() const {
   return last_pose_estimate_;
 }
 
-void KalmanLocalTrajectoryBuilder::AddTrajectoryNodeIndex(
-    int trajectory_node_index) {
-  submaps_->AddTrajectoryNodeIndex(trajectory_node_index);
-}
-
 std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
 KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
     const common::Time time, const sensor::RangeData& range_data_in_tracking,
     const transform::Rigid3d& pose_observation,
-    const kalman_filter::PoseCovariance& covariance_estimate) {
+    const kalman_filter::PoseCovariance& covariance_estimate,
+    const int trajectory_node_index) {
   if (motion_filter_.IsSimilar(time, pose_observation)) {
     return nullptr;
   }
@@ -225,8 +224,10 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
   for (int insertion_index : submaps_->insertion_indices()) {
     insertion_submaps.push_back(submaps_->Get(insertion_index));
   }
-  submaps_->InsertRangeData(sensor::TransformRangeData(
+  submaps_->InsertRangeData(
-      range_data_in_tracking, pose_observation.cast<float>()));
+      sensor::TransformRangeData(range_data_in_tracking,
+                                 pose_observation.cast<float>()),
+      trajectory_node_index);
   return std::unique_ptr<InsertionResult>(new InsertionResult{
       time, range_data_in_tracking, pose_observation, covariance_estimate,
       submaps_.get(), matching_submap, insertion_submaps});

cartographer/mapping_3d/kalman_local_trajectory_builder.h

@@ -50,21 +50,23 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface {
                   const Eigen::Vector3d& angular_velocity) override;
   std::unique_ptr<InsertionResult> AddRangefinderData(
       common::Time time, const Eigen::Vector3f& origin,
-      const sensor::PointCloud& ranges) override;
+      const sensor::PointCloud& ranges,
+      int next_trajectory_node_index) override;
   void AddOdometerData(common::Time time,
                        const transform::Rigid3d& pose) override;
-  void AddTrajectoryNodeIndex(int trajectory_node_index) override;
   const mapping_3d::Submaps* submaps() const override;
   const PoseEstimate& pose_estimate() const override;
 
  private:
   std::unique_ptr<InsertionResult> AddAccumulatedRangeData(
-      common::Time time, const sensor::RangeData& range_data_in_tracking);
+      common::Time time, const sensor::RangeData& range_data_in_tracking,
+      int trajectory_node_index);
 
   std::unique_ptr<InsertionResult> InsertIntoSubmap(
-      const common::Time time, const sensor::RangeData& range_data_in_tracking,
+      common::Time time, const sensor::RangeData& range_data_in_tracking,
       const transform::Rigid3d& pose_observation,
-      const kalman_filter::PoseCovariance& covariance_estimate);
+      const kalman_filter::PoseCovariance& covariance_estimate,
+      int trajectory_node_index);
 
   const proto::LocalTrajectoryBuilderOptions options_;
   std::unique_ptr<mapping_3d::Submaps> submaps_;

cartographer/mapping_3d/kalman_local_trajectory_builder_test.cc

@@ -261,7 +261,8 @@ class KalmanLocalTrajectoryBuilderTest : public ::testing::Test {
       AddLinearOnlyImuObservation(node.time, node.pose);
       const auto range_data = GenerateRangeData(node.pose);
       if (local_trajectory_builder_->AddRangefinderData(
-              node.time, range_data.origin, range_data.returns) != nullptr) {
+              node.time, range_data.origin, range_data.returns, num_poses) !=
+          nullptr) {
         const auto pose_estimate = local_trajectory_builder_->pose_estimate();
         EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1));
         ++num_poses;

cartographer/mapping_3d/local_trajectory_builder_interface.h

@@ -55,14 +55,10 @@ class LocalTrajectoryBuilderInterface {
                           const Eigen::Vector3d& angular_velocity) = 0;
   virtual std::unique_ptr<InsertionResult> AddRangefinderData(
       common::Time time, const Eigen::Vector3f& origin,
-      const sensor::PointCloud& ranges) = 0;
+      const sensor::PointCloud& ranges, int next_trajectory_node_index) = 0;
   virtual void AddOdometerData(common::Time time,
                                const transform::Rigid3d& pose) = 0;
 
-  // Register a 'trajectory_node_index' from the SparsePoseGraph corresponding
-  // to the latest inserted laser scan. This is used to remember which
-  // trajectory node should be used to visualize a Submap.
-  virtual void AddTrajectoryNodeIndex(int trajectory_node_index) = 0;
   virtual const mapping_3d::Submaps* submaps() const = 0;
   virtual const PoseEstimate& pose_estimate() const = 0;
 

cartographer/mapping_3d/optimizing_local_trajectory_builder.cc

@@ -135,7 +135,7 @@ void OptimizingLocalTrajectoryBuilder::AddOdometerData(
 std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
 OptimizingLocalTrajectoryBuilder::AddRangefinderData(
     const common::Time time, const Eigen::Vector3f& origin,
-    const sensor::PointCloud& ranges) {
+    const sensor::PointCloud& ranges, const int next_trajectory_node_index) {
   CHECK_GT(ranges.size(), 0);
 
   // TODO(hrapp): Handle misses.
@@ -188,7 +188,7 @@ OptimizingLocalTrajectoryBuilder::AddRangefinderData(
   ++num_accumulated_;
 
   RemoveObsoleteSensorData();
-  return MaybeOptimize(time);
+  return MaybeOptimize(time, next_trajectory_node_index);
 }
 
 void OptimizingLocalTrajectoryBuilder::RemoveObsoleteSensorData() {
@@ -215,7 +215,8 @@ void OptimizingLocalTrajectoryBuilder::RemoveObsoleteSensorData() {
 }
 
 std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
-OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
+OptimizingLocalTrajectoryBuilder::MaybeOptimize(
+    const common::Time time, const int trajectory_node_index) {
   // TODO(hrapp): Make the number of optimizations configurable.
   if (num_accumulated_ < options_.scans_per_accumulation() &&
       num_accumulated_ % 10 != 0) {
@@ -348,13 +349,15 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
   }
 
   return AddAccumulatedRangeData(time, optimized_pose,
-                                 accumulated_range_data_in_tracking);
+                                 accumulated_range_data_in_tracking,
+                                 trajectory_node_index);
 }
 
 std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
 OptimizingLocalTrajectoryBuilder::AddAccumulatedRangeData(
     const common::Time time, const transform::Rigid3d& optimized_pose,
-    const sensor::RangeData& range_data_in_tracking) {
+    const sensor::RangeData& range_data_in_tracking,
+    const int trajectory_node_index) {
   const sensor::RangeData filtered_range_data = {
       range_data_in_tracking.origin,
       sensor::VoxelFiltered(range_data_in_tracking.returns,
@@ -372,7 +375,8 @@ OptimizingLocalTrajectoryBuilder::AddAccumulatedRangeData(
       sensor::TransformPointCloud(filtered_range_data.returns,
                                   optimized_pose.cast<float>())};
 
-  return InsertIntoSubmap(time, filtered_range_data, optimized_pose);
+  return InsertIntoSubmap(time, filtered_range_data, optimized_pose,
+                          trajectory_node_index);
 }
 
 const OptimizingLocalTrajectoryBuilder::PoseEstimate&
@@ -380,15 +384,11 @@ OptimizingLocalTrajectoryBuilder::pose_estimate() const {
   return last_pose_estimate_;
 }
 
-void OptimizingLocalTrajectoryBuilder::AddTrajectoryNodeIndex(
-    int trajectory_node_index) {
-  submaps_->AddTrajectoryNodeIndex(trajectory_node_index);
-}
-
 std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
 OptimizingLocalTrajectoryBuilder::InsertIntoSubmap(
     const common::Time time, const sensor::RangeData& range_data_in_tracking,
-    const transform::Rigid3d& pose_observation) {
+    const transform::Rigid3d& pose_observation,
+    const int trajectory_node_index) {
   if (motion_filter_.IsSimilar(time, pose_observation)) {
     return nullptr;
   }
@@ -398,8 +398,10 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap(
   for (int insertion_index : submaps_->insertion_indices()) {
     insertion_submaps.push_back(submaps_->Get(insertion_index));
   }
-  submaps_->InsertRangeData(sensor::TransformRangeData(
+  submaps_->InsertRangeData(
-      range_data_in_tracking, pose_observation.cast<float>()));
+      sensor::TransformRangeData(range_data_in_tracking,
+                                 pose_observation.cast<float>()),
+      trajectory_node_index);
 
   const kalman_filter::PoseCovariance kCovariance =
       1e-7 * kalman_filter::PoseCovariance::Identity();

cartographer/mapping_3d/optimizing_local_trajectory_builder.h

@@ -53,10 +53,10 @@ class OptimizingLocalTrajectoryBuilder
                   const Eigen::Vector3d& angular_velocity) override;
   std::unique_ptr<InsertionResult> AddRangefinderData(
       common::Time time, const Eigen::Vector3f& origin,
-      const sensor::PointCloud& ranges) override;
+      const sensor::PointCloud& ranges,
-  void AddOdometerData(const common::Time time,
+      int next_trajectory_node_index) override;
+  void AddOdometerData(common::Time time,
                        const transform::Rigid3d& pose) override;
-  void AddTrajectoryNodeIndex(int trajectory_node_index) override;
   const mapping_3d::Submaps* submaps() const override;
   const PoseEstimate& pose_estimate() const override;
 
@@ -102,13 +102,16 @@ class OptimizingLocalTrajectoryBuilder
 
   std::unique_ptr<InsertionResult> AddAccumulatedRangeData(
       common::Time time, const transform::Rigid3d& pose_observation,
-      const sensor::RangeData& range_data_in_tracking);
+      const sensor::RangeData& range_data_in_tracking,
+      const int next_trajectory_node_index);
 
   std::unique_ptr<InsertionResult> InsertIntoSubmap(
       const common::Time time, const sensor::RangeData& range_data_in_tracking,
-      const transform::Rigid3d& pose_observation);
+      const transform::Rigid3d& pose_observation,
+      const int next_trajectory_node_index);
 
-  std::unique_ptr<InsertionResult> MaybeOptimize(common::Time time);
+  std::unique_ptr<InsertionResult> MaybeOptimize(
+      common::Time time, const int next_trajectory_node_index);
 
   const proto::LocalTrajectoryBuilderOptions options_;
   const ceres::Solver::Options ceres_solver_options_;

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -83,7 +83,7 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
   }
 }
 
-int SparsePoseGraph::AddScan(
+void SparsePoseGraph::AddScan(
     common::Time time, const sensor::RangeData& range_data_in_tracking,
     const transform::Rigid3d& pose,
     const kalman_filter::PoseCovariance& covariance, const Submaps* submaps,
@@ -126,7 +126,11 @@ int SparsePoseGraph::AddScan(
     ComputeConstraintsForScan(j, matching_submap, insertion_submaps,
                               finished_submap, pose, covariance);
   });
-  return j;
+}
+
+int SparsePoseGraph::GetNextTrajectoryNodeIndex() {
+  common::MutexLocker locker(&mutex_);
+  return trajectory_nodes_.size();
 }
 
 void SparsePoseGraph::AddWorkItem(std::function<void()> work_item) {
@@ -285,25 +289,30 @@ void SparsePoseGraph::WaitForAllComputations() {
   common::MutexLocker locker(&mutex_);
   const int num_finished_scans_at_start =
       constraint_builder_.GetNumFinishedScans();
-  while (!locker.AwaitWithTimeout([this]() REQUIRES(mutex_) {
+  while (!locker.AwaitWithTimeout(
-    return static_cast<size_t>(constraint_builder_.GetNumFinishedScans()) ==
+      [this]() REQUIRES(mutex_) {
-           trajectory_nodes_.size();
+        return static_cast<size_t>(constraint_builder_.GetNumFinishedScans()) ==
-  }, common::FromSeconds(1.))) {
+               trajectory_nodes_.size();
+      },
+      common::FromSeconds(1.))) {
     std::ostringstream progress_info;
     progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
-                  << 100. * (constraint_builder_.GetNumFinishedScans() -
+                  << 100. *
-                             num_finished_scans_at_start) /
+                         (constraint_builder_.GetNumFinishedScans() -
+                          num_finished_scans_at_start) /
                          (trajectory_nodes_.size() -
-                          num_finished_scans_at_start) << "%...";
+                          num_finished_scans_at_start)
+                  << "%...";
     std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
   }
   std::cout << "\r\x1b[KOptimizing: Done.     " << std::endl;
-  constraint_builder_.WhenDone([this, &notification](
+  constraint_builder_.WhenDone(
-      const sparse_pose_graph::ConstraintBuilder::Result& result) {
+      [this, &notification](
-    constraints_.insert(constraints_.end(), result.begin(), result.end());
+          const sparse_pose_graph::ConstraintBuilder::Result& result) {
-    common::MutexLocker locker(&mutex_);
+        constraints_.insert(constraints_.end(), result.begin(), result.end());
-    notification = true;
+        common::MutexLocker locker(&mutex_);
-  });
+        notification = true;
+      });
   locker.Await([&notification]() { return notification; });
 }
 
@@ -336,13 +345,13 @@ void SparsePoseGraph::RunOptimization() {
       const mapping::Submaps* trajectory =
           trajectory_nodes_[i].constant_data->trajectory;
       if (extrapolation_transforms.count(trajectory) == 0) {
-        extrapolation_transforms[trajectory] =
+        extrapolation_transforms[trajectory] = transform::Rigid3d(
-            transform::Rigid3d(ExtrapolateSubmapTransforms(submap_transforms_,
+            ExtrapolateSubmapTransforms(submap_transforms_, *trajectory)
-                                                           *trajectory).back() *
+                .back() *
-                               ExtrapolateSubmapTransforms(
+            ExtrapolateSubmapTransforms(optimized_submap_transforms_,
-                                   optimized_submap_transforms_, *trajectory)
+                                        *trajectory)
-                                   .back()
+                .back()
-                                   .inverse());
+                .inverse());
       }
       trajectory_nodes_[i].pose =
           extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose;

cartographer/mapping_3d/sparse_pose_graph.h

@@ -69,16 +69,19 @@ 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'. The index into the vector of trajectory nodes as
+  // 'insertion_submaps'.
-  // used with GetTrajectoryNodes() is returned.
+  void AddScan(common::Time time,
-  int AddScan(common::Time time,
+               const sensor::RangeData& range_data_in_tracking,
-              const sensor::RangeData& range_data_in_tracking,
+               const transform::Rigid3d& pose,
-              const transform::Rigid3d& pose,
+               const kalman_filter::PoseCovariance& pose_covariance,
-              const kalman_filter::PoseCovariance& pose_covariance,
+               const Submaps* submaps, const Submap* matching_submap,
-              const Submaps* submaps, const Submap* matching_submap,
+               const std::vector<const Submap*>& insertion_submaps)
-              const std::vector<const Submap*>& insertion_submaps)
       EXCLUDES(mutex_);
 
+  // The index into the vector of trajectory nodes as used with
+  // GetTrajectoryNodes() for the next node added with AddScan() is returned.
+  int GetNextTrajectoryNodeIndex() EXCLUDES(mutex_);
+
   // Adds new IMU data to be used in the optimization.
   void AddImuData(const mapping::Submaps* trajectory, common::Time time,
                   const Eigen::Vector3d& linear_acceleration,

cartographer/mapping_3d/submaps.cc

@@ -217,8 +217,8 @@ proto::SubmapsOptions CreateSubmapsOptions(
 }
 
 Submap::Submap(const float high_resolution, const float low_resolution,
-               const Eigen::Vector3f& origin, const int begin_range_data_index)
+               const Eigen::Vector3f& origin)
-    : mapping::Submap(origin, begin_range_data_index),
+    : mapping::Submap(origin),
       high_resolution_hybrid_grid(high_resolution, origin),
       low_resolution_hybrid_grid(low_resolution, origin) {}
 
@@ -274,9 +274,8 @@ void Submaps::SubmapToProto(
                              global_submap_pose.translation().z())));
 }
 
-void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
+void Submaps::InsertRangeData(const sensor::RangeData& range_data,
-  CHECK_LT(num_range_data_, std::numeric_limits<int>::max());
+                              const int trajectory_node_index) {
-  ++num_range_data_;
   for (const int index : insertion_indices()) {
     Submap* submap = submaps_[index].get();
     range_data_inserter_.Insert(
@@ -285,10 +284,11 @@ void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
         &submap->high_resolution_hybrid_grid);
     range_data_inserter_.Insert(range_data,
                                 &submap->low_resolution_hybrid_grid);
-    submap->end_range_data_index = num_range_data_;
+    ++submap->num_range_data;
+    submap->trajectory_node_indices.push_back(trajectory_node_index);
   }
-  ++num_range_data_in_last_submap_;
+  const Submap* const last_submap = Get(size() - 1);
-  if (num_range_data_in_last_submap_ == options_.num_range_data()) {
+  if (last_submap->num_range_data == options_.num_range_data()) {
     AddSubmap(range_data.origin);
   }
 }
@@ -301,16 +301,6 @@ const HybridGrid& Submaps::low_resolution_matching_grid() const {
   return submaps_[matching_index()]->low_resolution_hybrid_grid;
 }
 
-void Submaps::AddTrajectoryNodeIndex(const int trajectory_node_index) {
-  for (int i = 0; i != size(); ++i) {
-    Submap& submap = *submaps_[i];
-    if (submap.end_range_data_index == num_range_data_ &&
-        submap.begin_range_data_index <= num_range_data_ - 1) {
-      submap.trajectory_node_indices.push_back(trajectory_node_index);
-    }
-  }
-}
-
 void Submaps::AddSubmap(const Eigen::Vector3f& origin) {
   if (size() > 1) {
     Submap* submap = submaps_[size() - 2].get();
@@ -318,10 +308,8 @@ void Submaps::AddSubmap(const Eigen::Vector3f& origin) {
     submap->finished = true;
   }
   submaps_.emplace_back(new Submap(options_.high_resolution(),
-                                   options_.low_resolution(), origin,
+                                   options_.low_resolution(), origin));
-                                   num_range_data_));
   LOG(INFO) << "Added submap " << size();
-  num_range_data_in_last_submap_ = 0;
 }
 
 std::vector<Submaps::PixelData> Submaps::AccumulatePixelData(

cartographer/mapping_3d/submaps.h

@@ -47,11 +47,12 @@ proto::SubmapsOptions CreateSubmapsOptions(
 
 struct Submap : public mapping::Submap {
   Submap(float high_resolution, float low_resolution,
-         const Eigen::Vector3f& origin, int begin_range_data_index);
+         const Eigen::Vector3f& origin);
 
   HybridGrid high_resolution_hybrid_grid;
   HybridGrid low_resolution_hybrid_grid;
   bool finished = false;
+  // Indices into the nodes of the SparsePoseGraph used to visualize the submap.
   std::vector<int> trajectory_node_indices;
 };
 
@@ -71,7 +72,8 @@ class Submaps : public mapping::Submaps {
       mapping::proto::SubmapQuery::Response* response) const override;
 
   // Inserts 'range_data' into the Submap collection.
-  void InsertRangeData(const sensor::RangeData& range_data);
+  void InsertRangeData(const sensor::RangeData& range_data,
+                       int trajectory_node_index);
 
   // Returns the 'high_resolution' HybridGrid to be used for matching.
   const HybridGrid& high_resolution_matching_grid() const;
@@ -79,9 +81,6 @@ class Submaps : public mapping::Submaps {
   // Returns the 'low_resolution' HybridGrid to be used for matching.
   const HybridGrid& low_resolution_matching_grid() const;
 
-  // Adds a node to be used when visualizing the submap.
-  void AddTrajectoryNodeIndex(int trajectory_node_index);
-
  private:
   struct PixelData {
     int min_z = INT_MAX;
@@ -112,12 +111,6 @@ class Submaps : public mapping::Submaps {
 
   std::vector<std::unique_ptr<Submap>> submaps_;
   RangeDataInserter range_data_inserter_;
-
-  // Number of RangeData inserted.
-  int num_range_data_ = 0;
-
-  // Number of RangeData inserted since the last Submap was added.
-  int num_range_data_in_last_submap_ = 0;
 };
 
 }  // namespace mapping_3d