Store histogram in submap (#1277)

The histogram of a submap is now stored in the submap (class and proto) itself. This change allows to accumulate the histogram of a submap in local SLAM by adding up the histogram of each new scan.

The main advantage is that the background thread doesn't have to loop over all `TrajectoryNode`s of a finished submap to compute the submap histogram for the `RotationalScanMatcher`. Instead this chunk of work is moved to the local SLAM thread but is split up into a few computations for each new scan. When running localization, the histogram of a submap can just be read from a map pbstream and does not have to be computed from the nodes.

In summary:
- This change improved the CPU time of offline SLAM by ~7%.
- Increases the readability of the code and performance of the background thread. (see `PoseGraph3D::ComputeConstraint`)
- No negative performance impacts on accuracy or finding loop-closures

However:
- With this change to the submap proto, old maps (pbstreams) are no longer supported and need to be re-created by running offline slam

cartographer/io/internal/mapping_state_serialization.h

@@ -25,9 +25,8 @@ namespace cartographer {
 namespace io {
 
 // The current serialization format version.
-static constexpr int kMappingStateSerializationFormatVersion = 1;
-static constexpr int kFormatVersionWithoutSubmapHistograms =
-    kMappingStateSerializationFormatVersion;
+static constexpr int kMappingStateSerializationFormatVersion = 2;
+static constexpr int kFormatVersionWithoutSubmapHistograms = 1;
 
 // Serialize mapping state to a pbstream.
 void WritePbStream(

cartographer/io/proto_stream_deserializer.cc

@@ -31,7 +31,8 @@ mapping::proto::SerializationHeader ReadHeaderOrDie(
 }
 
 bool IsVersionSupported(const mapping::proto::SerializationHeader& header) {
-  return header.format_version() == kMappingStateSerializationFormatVersion;
+  return header.format_version() == kMappingStateSerializationFormatVersion ||
+         header.format_version() == kFormatVersionWithoutSubmapHistograms;
 }
 
 }  // namespace

cartographer/mapping/3d/submap_3d.cc

@@ -197,12 +197,14 @@ proto::SubmapsOptions3D CreateSubmapsOptions3D(
 }
 
 Submap3D::Submap3D(const float high_resolution, const float low_resolution,
-                   const transform::Rigid3d& local_submap_pose)
+                   const transform::Rigid3d& local_submap_pose,
+                   const Eigen::VectorXf& rotational_scan_matcher_histogram)
     : Submap(local_submap_pose),
       high_resolution_hybrid_grid_(
           absl::make_unique<HybridGrid>(high_resolution)),
       low_resolution_hybrid_grid_(
-          absl::make_unique<HybridGrid>(low_resolution)) {}
+          absl::make_unique<HybridGrid>(low_resolution)),
+      rotational_scan_matcher_histogram_(rotational_scan_matcher_histogram) {}
 
 Submap3D::Submap3D(const proto::Submap3D& proto)
     : Submap(transform::ToRigid3(proto.local_pose())) {
@@ -266,9 +268,11 @@ void Submap3D::ToResponseProto(
                     response->add_textures());
 }
 
-void Submap3D::InsertRangeData(const sensor::RangeData& range_data_in_local,
-                               const RangeDataInserter3D& range_data_inserter,
-                               const float high_resolution_max_range) {
+void Submap3D::InsertData(const sensor::RangeData& range_data_in_local,
+                          const RangeDataInserter3D& range_data_inserter,
+                          const float high_resolution_max_range,
+                          const Eigen::Quaterniond& local_from_gravity_aligned,
+                          const Eigen::VectorXf& scan_histogram_in_gravity) {
   CHECK(!insertion_finished());
   // Transform range data into submap frame.
   const sensor::RangeData transformed_range_data = sensor::TransformRangeData(
@@ -280,6 +284,11 @@ void Submap3D::InsertRangeData(const sensor::RangeData& range_data_in_local,
   range_data_inserter.Insert(transformed_range_data,
                              low_resolution_hybrid_grid_.get());
   set_num_range_data(num_range_data() + 1);
+  const float yaw_in_submap_from_gravity = transform::GetYaw(
+      local_pose().inverse().rotation() * local_from_gravity_aligned);
+  rotational_scan_matcher_histogram_ +=
+      scan_matching::RotationalScanMatcher::RotateHistogram(
+          scan_histogram_in_gravity, yaw_in_submap_from_gravity);
 }
 
 void Submap3D::Finish() {
@@ -296,34 +305,42 @@ std::vector<std::shared_ptr<const Submap3D>> ActiveSubmaps3D::submaps() const {
                                                       submaps_.end());
 }
 
-std::vector<std::shared_ptr<const Submap3D>> ActiveSubmaps3D::InsertRangeData(
+std::vector<std::shared_ptr<const Submap3D>> ActiveSubmaps3D::InsertData(
     const sensor::RangeData& range_data,
-    const Eigen::Quaterniond& local_from_gravity_aligned) {
+    const Eigen::Quaterniond& local_from_gravity_aligned,
+    const Eigen::VectorXf& rotational_scan_matcher_histogram_in_gravity) {
   if (submaps_.empty() ||
       submaps_.back()->num_range_data() == options_.num_range_data()) {
     AddSubmap(transform::Rigid3d(range_data.origin.cast<double>(),
-                                 local_from_gravity_aligned));
+                                 local_from_gravity_aligned),
+              rotational_scan_matcher_histogram_in_gravity.size());
   }
   for (auto& submap : submaps_) {
-    submap->InsertRangeData(range_data, range_data_inserter_,
-                            options_.high_resolution_max_range());
+    submap->InsertData(range_data, range_data_inserter_,
+                       options_.high_resolution_max_range(),
+                       local_from_gravity_aligned,
+                       rotational_scan_matcher_histogram_in_gravity);
   }
   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) {
+void ActiveSubmaps3D::AddSubmap(
+    const transform::Rigid3d& local_submap_pose,
+    const int rotational_scan_matcher_histogram_size) {
   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()->insertion_finished());
     submaps_.erase(submaps_.begin());
   }
-  submaps_.emplace_back(new Submap3D(options_.high_resolution(),
-                                     options_.low_resolution(),
-                                     local_submap_pose));
+  const Eigen::VectorXf initial_rotational_scan_matcher_histogram =
+      Eigen::VectorXf::Zero(rotational_scan_matcher_histogram_size);
+  submaps_.emplace_back(new Submap3D(
+      options_.high_resolution(), options_.low_resolution(), local_submap_pose,
+      initial_rotational_scan_matcher_histogram));
 }
 
 }  // namespace mapping

cartographer/mapping/3d/submap_3d.h

@@ -43,7 +43,9 @@ proto::SubmapsOptions3D CreateSubmapsOptions3D(
 class Submap3D : public Submap {
  public:
   Submap3D(float high_resolution, float low_resolution,
-           const transform::Rigid3d& local_submap_pose);
+           const transform::Rigid3d& local_submap_pose,
+           const Eigen::VectorXf& rotational_scan_matcher_histogram);
+
   explicit Submap3D(const proto::Submap3D& proto);
 
   proto::Submap ToProto(bool include_probability_grid_data) const override;
@@ -64,9 +66,12 @@ class Submap3D : public Submap {
 
   // Insert 'range_data' into this submap using 'range_data_inserter'. The
   // submap must not be finished yet.
-  void InsertRangeData(const sensor::RangeData& range_data,
-                       const RangeDataInserter3D& range_data_inserter,
-                       float high_resolution_max_range);
+  void InsertData(const sensor::RangeData& range_data,
+                  const RangeDataInserter3D& range_data_inserter,
+                  float high_resolution_max_range,
+                  const Eigen::Quaterniond& local_from_gravity_aligned,
+                  const Eigen::VectorXf& scan_histogram_in_gravity);
+
   void Finish();
 
  private:
@@ -97,14 +102,18 @@ class ActiveSubmaps3D {
   // Inserts 'range_data_in_local' into the Submap collection.
   // 'local_from_gravity_aligned' is used for the orientation of new submaps so
   // that the z axis approximately aligns with gravity.
-  std::vector<std::shared_ptr<const Submap3D>> InsertRangeData(
+  // 'rotational_scan_matcher_histogram_in_gravity' will be accumulated in all
+  // submaps of the Submap collection.
+  std::vector<std::shared_ptr<const Submap3D>> InsertData(
       const sensor::RangeData& range_data_in_local,
-      const Eigen::Quaterniond& local_from_gravity_aligned);
+      const Eigen::Quaterniond& local_from_gravity_aligned,
+      const Eigen::VectorXf& rotational_scan_matcher_histogram_in_gravity);
 
   std::vector<std::shared_ptr<const Submap3D>> submaps() const;
 
  private:
-  void AddSubmap(const transform::Rigid3d& local_submap_pose);
+  void AddSubmap(const transform::Rigid3d& local_submap_pose,
+                 int rotational_scan_matcher_histogram_size);
 
   const proto::SubmapsOptions3D options_;
   std::vector<std::shared_ptr<Submap3D>> submaps_;

cartographer/mapping/3d/submap_3d_test.cc

@@ -24,10 +24,13 @@ namespace mapping {
 namespace {
 
 TEST(SubmapsTest, ToFromProto) {
+  Eigen::VectorXf histogram(2);
+  histogram << 1.0f, 2.0f;
   const Submap3D expected(
       0.05, 0.25,
       transform::Rigid3d(Eigen::Vector3d(1., 2., 0.),
-                         Eigen::Quaterniond(0., 0., 0., 1.)));
+                         Eigen::Quaterniond(0., 0., 0., 1.)),
+      histogram);
   const proto::Submap proto =
       expected.ToProto(true /* include_probability_grid_data */);
   EXPECT_FALSE(proto.has_submap_2d());
@@ -41,6 +44,8 @@ TEST(SubmapsTest, ToFromProto) {
   EXPECT_EQ(expected.insertion_finished(), actual.insertion_finished());
   EXPECT_NEAR(expected.high_resolution_hybrid_grid().resolution(), 0.05, 1e-6);
   EXPECT_NEAR(expected.low_resolution_hybrid_grid().resolution(), 0.25, 1e-6);
+  EXPECT_TRUE(expected.rotational_scan_matcher_histogram().isApprox(
+      actual.rotational_scan_matcher_histogram(), 1e-6));
 }
 
 }  // namespace

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

@@ -352,19 +352,19 @@ LocalTrajectoryBuilder3D::InsertIntoSubmap(
   if (motion_filter_.IsSimilar(time, pose_estimate)) {
     return nullptr;
   }
-  const auto local_from_gravity_aligned =
-      pose_estimate.rotation() * gravity_alignment.inverse();
-
-  std::vector<std::shared_ptr<const mapping::Submap3D>> insertion_submaps =
-      active_submaps_.InsertRangeData(filtered_range_data_in_local,
-                                      local_from_gravity_aligned);
-
-  const Eigen::VectorXf rotational_scan_matcher_histogram =
+  const Eigen::VectorXf rotational_scan_matcher_histogram_in_gravity =
       scan_matching::RotationalScanMatcher::ComputeHistogram(
           sensor::TransformPointCloud(
               filtered_range_data_in_tracking.returns,
               transform::Rigid3f::Rotation(gravity_alignment.cast<float>())),
           options_.rotational_histogram_size());
+
+  const Eigen::Quaterniond local_from_gravity_aligned =
+      pose_estimate.rotation() * gravity_alignment.inverse();
+  std::vector<std::shared_ptr<const mapping::Submap3D>> insertion_submaps =
+      active_submaps_.InsertData(filtered_range_data_in_local,
+                                 local_from_gravity_aligned,
+                                 rotational_scan_matcher_histogram_in_gravity);
   return absl::make_unique<InsertionResult>(
       InsertionResult{std::make_shared<const mapping::TrajectoryNode::Data>(
                           mapping::TrajectoryNode::Data{
@@ -373,7 +373,7 @@ LocalTrajectoryBuilder3D::InsertIntoSubmap(
                               {},  // 'filtered_point_cloud' is only used in 2D.
                               high_resolution_point_cloud_in_tracking,
                               low_resolution_point_cloud_in_tracking,
-                              rotational_scan_matcher_histogram,
+                              rotational_scan_matcher_histogram_in_gravity,
                               pose_estimate}),
                       std::move(insertion_submaps)});
 }

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

@@ -246,14 +246,10 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
   const transform::Rigid3d global_submap_pose =
       optimization_problem_->submap_data().at(submap_id).global_pose;
 
-  const transform::Rigid3d global_submap_pose_inverse =
-      global_submap_pose.inverse();
-
   bool maybe_add_local_constraint = false;
   bool maybe_add_global_constraint = false;
   const TrajectoryNode::Data* constant_data;
   const Submap3D* submap;
-  std::vector<TrajectoryNode> submap_nodes;
   {
     absl::MutexLock locker(&mutex_);
     CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished);
@@ -263,14 +259,6 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
       return;
     }
 
-    for (const NodeId& submap_node_id :
-         data_.submap_data.at(submap_id).node_ids) {
-      submap_nodes.push_back(TrajectoryNode{
-          data_.trajectory_nodes.at(submap_node_id).constant_data,
-          global_submap_pose_inverse *
-              data_.trajectory_nodes.at(submap_node_id).global_pose});
-    }
-
     const common::Time node_time = GetLatestNodeTime(node_id, submap_id);
     const common::Time last_connection_time =
         data_.trajectory_connectivity_state.LastConnectionTime(
@@ -299,13 +287,13 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
   }
 
   if (maybe_add_local_constraint) {
-    constraint_builder_.MaybeAddConstraint(
-        submap_id, submap, node_id, constant_data, submap_nodes,
-        global_node_pose, global_submap_pose);
+    constraint_builder_.MaybeAddConstraint(submap_id, submap, node_id,
+                                           constant_data, global_node_pose,
+                                           global_submap_pose);
   } else if (maybe_add_global_constraint) {
     constraint_builder_.MaybeAddGlobalConstraint(
-        submap_id, submap, node_id, constant_data, submap_nodes,
-        global_node_pose.rotation(), global_submap_pose.rotation());
+        submap_id, submap, node_id, constant_data, global_node_pose.rotation(),
+        global_submap_pose.rotation());
   }
 }
 

cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.cc

@@ -109,36 +109,18 @@ struct Candidate3D {
   bool operator>(const Candidate3D& other) const { return score > other.score; }
 };
 
-namespace {
-
-std::vector<std::pair<Eigen::VectorXf, float>> HistogramsAtAnglesFromNodes(
-    const std::vector<TrajectoryNode>& nodes) {
-  std::vector<std::pair<Eigen::VectorXf, float>> histograms_at_angles;
-  for (const auto& node : nodes) {
-    histograms_at_angles.emplace_back(
-        node.constant_data->rotational_scan_matcher_histogram,
-        transform::GetYaw(
-            node.global_pose *
-            transform::Rigid3d::Rotation(
-                node.constant_data->gravity_alignment.inverse())));
-  }
-  return histograms_at_angles;
-}
-
-}  // namespace
-
 FastCorrelativeScanMatcher3D::FastCorrelativeScanMatcher3D(
     const HybridGrid& hybrid_grid,
     const HybridGrid* const low_resolution_hybrid_grid,
-    const std::vector<TrajectoryNode>& nodes,
+    const Eigen::VectorXf* rotational_scan_matcher_histogram,
     const proto::FastCorrelativeScanMatcherOptions3D& options)
     : options_(options),
       resolution_(hybrid_grid.resolution()),
       width_in_voxels_(hybrid_grid.grid_size()),
       precomputation_grid_stack_(
           absl::make_unique<PrecomputationGridStack3D>(hybrid_grid, options)),
       low_resolution_hybrid_grid_(low_resolution_hybrid_grid),
-      rotational_scan_matcher_(HistogramsAtAnglesFromNodes(nodes)) {}
+      rotational_scan_matcher_(rotational_scan_matcher_histogram) {}
 
 FastCorrelativeScanMatcher3D::~FastCorrelativeScanMatcher3D() {}
 

cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.h

@@ -75,7 +75,7 @@ class FastCorrelativeScanMatcher3D {
   FastCorrelativeScanMatcher3D(
       const HybridGrid& hybrid_grid,
       const HybridGrid* low_resolution_hybrid_grid,
-      const std::vector<TrajectoryNode>& nodes,
+      const Eigen::VectorXf* rotational_scan_matcher_histogram,
       const proto::FastCorrelativeScanMatcherOptions3D& options);
   ~FastCorrelativeScanMatcher3D();
 

cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc

@@ -105,11 +105,7 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test {
     hybrid_grid_->FinishUpdate();
 
     return absl::make_unique<FastCorrelativeScanMatcher3D>(
-        *hybrid_grid_, hybrid_grid_.get(),
-        std::vector<TrajectoryNode>(
-            {{std::make_shared<const TrajectoryNode::Data>(
-                  CreateConstantData(point_cloud_)),
-              pose.cast<double>()}}),
+        *hybrid_grid_, hybrid_grid_.get(), &GetRotationalScanMatcherHistogram(),
         options);
   }
 
@@ -120,7 +116,11 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test {
                                 {},
                                 point_cloud_,
                                 low_resolution_point_cloud,
-                                Eigen::VectorXf::Zero(10)};
+                                GetRotationalScanMatcherHistogram()};
+  }
+
+  const Eigen::VectorXf& GetRotationalScanMatcherHistogram() {
+    return rotational_scan_matcher_histogram_;
   }
 
   std::mt19937 prng_ = std::mt19937(42);
@@ -130,6 +130,8 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test {
   const proto::FastCorrelativeScanMatcherOptions3D options_;
   sensor::PointCloud point_cloud_;
   std::unique_ptr<HybridGrid> hybrid_grid_;
+  const Eigen::VectorXf rotational_scan_matcher_histogram_ =
+      Eigen::VectorXf::Zero(10);
 };
 
 constexpr float kMinScore = 0.1f;