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 kMappingStateSerializationFormatVersion = 2;
-static constexpr int kFormatVersionWithoutSubmapHistograms =
+static constexpr int kFormatVersionWithoutSubmapHistograms = 1;
-    kMappingStateSerializationFormatVersion;
 
 // 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,
+void Submap3D::InsertData(const sensor::RangeData& range_data_in_local,
                           const RangeDataInserter3D& range_data_inserter,
-                               const float high_resolution_max_range) {
+                          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,17 +305,21 @@ 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_,
+    submap->InsertData(range_data, range_data_inserter_,
-                            options_.high_resolution_max_range());
+                       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();
@@ -314,16 +327,20 @@ std::vector<std::shared_ptr<const Submap3D>> ActiveSubmaps3D::InsertRangeData(
   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(),
+  const Eigen::VectorXf initial_rotational_scan_matcher_histogram =
-                                     options_.low_resolution(),
+      Eigen::VectorXf::Zero(rotational_scan_matcher_histogram_size);
-                                     local_submap_pose));
+  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,
+  void InsertData(const sensor::RangeData& range_data,
                   const RangeDataInserter3D& range_data_inserter,
-                       float high_resolution_max_range);
+                  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 =
+  const Eigen::VectorXf rotational_scan_matcher_histogram_in_gravity =
-      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 =
       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(
+    constraint_builder_.MaybeAddConstraint(submap_id, submap, node_id,
-        submap_id, submap, node_id, constant_data, submap_nodes,
+                                           constant_data, global_node_pose,
-        global_node_pose, global_submap_pose);
+                                           global_submap_pose);
   } else if (maybe_add_global_constraint) {
     constraint_builder_.MaybeAddGlobalConstraint(
-        submap_id, submap, node_id, constant_data, submap_nodes,
+        submap_id, submap, node_id, constant_data, global_node_pose.rotation(),
-        global_node_pose.rotation(), global_submap_pose.rotation());
+        global_submap_pose.rotation());
   }
 }
 

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

@@ -109,28 +109,10 @@ 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()),
@@ -138,7 +120,7 @@ FastCorrelativeScanMatcher3D::FastCorrelativeScanMatcher3D(
       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(),
+        *hybrid_grid_, hybrid_grid_.get(), &GetRotationalScanMatcherHistogram(),
-        std::vector<TrajectoryNode>(
-            {{std::make_shared<const TrajectoryNode::Data>(
-                  CreateConstantData(point_cloud_)),
-              pose.cast<double>()}}),
         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;

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

@@ -133,6 +133,11 @@ float MatchHistograms(const Eigen::VectorXf& submap_histogram,
 
 }  // namespace
 
+RotationalScanMatcher::RotationalScanMatcher(const Eigen::VectorXf* histogram)
+    : histogram_(histogram) {}
+
+// Rotates the given 'histogram' by the given 'angle'. This might lead to
+// rotations of a fractional bucket which is handled by linearly interpolating.
 Eigen::VectorXf RotationalScanMatcher::RotateHistogram(
     const Eigen::VectorXf& histogram, const float angle) {
   const float rotate_by_buckets = -angle * histogram.size() / M_PI;
@@ -166,19 +171,6 @@ Eigen::VectorXf RotationalScanMatcher::ComputeHistogram(
   return histogram;
 }
 
-RotationalScanMatcher::RotationalScanMatcher(const Eigen::VectorXf& histogram)
-    : histogram_(histogram) {}
-
-RotationalScanMatcher::RotationalScanMatcher(
-    const std::vector<std::pair<Eigen::VectorXf, float>>& histograms_at_angles)
-    : histogram_(
-          Eigen::VectorXf::Zero(histograms_at_angles.at(0).first.size())) {
-  for (const auto& histogram_at_angle : histograms_at_angles) {
-    histogram_ +=
-        RotateHistogram(histogram_at_angle.first, histogram_at_angle.second);
-  }
-}
-
 std::vector<float> RotationalScanMatcher::Match(
     const Eigen::VectorXf& histogram, const float initial_angle,
     const std::vector<float>& angles) const {
@@ -187,7 +179,7 @@ std::vector<float> RotationalScanMatcher::Match(
   for (const float angle : angles) {
     const Eigen::VectorXf scan_histogram =
         RotateHistogram(histogram, initial_angle + angle);
-    result.push_back(MatchHistograms(histogram_, scan_histogram));
+    result.push_back(MatchHistograms(*histogram_, scan_histogram));
   }
   return result;
 }

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

@@ -38,14 +38,7 @@ class RotationalScanMatcher {
   static Eigen::VectorXf ComputeHistogram(const sensor::PointCloud& point_cloud,
                                           int histogram_size);
 
-  explicit RotationalScanMatcher(const Eigen::VectorXf& histogram);
+  explicit RotationalScanMatcher(const Eigen::VectorXf* histogram);
-
-  // Creates a matcher from the given histograms rotated by the given angles.
-  // The angles should be chosen to bring the histograms into approximately the
-  // same frame.
-  explicit RotationalScanMatcher(
-      const std::vector<std::pair<Eigen::VectorXf, float>>&
-          histograms_at_angles);
 
   // Scores how well 'histogram' rotated by 'initial_angle' can be understood as
   // further rotated by certain 'angles' relative to the 'nodes'. Each angle
@@ -55,7 +48,7 @@ class RotationalScanMatcher {
                            const std::vector<float>& angles) const;
 
  private:
-  Eigen::VectorXf histogram_;
+  const Eigen::VectorXf* histogram_;
 };
 
 }  // namespace scan_matching

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

@@ -28,9 +28,7 @@ namespace {
 TEST(RotationalScanMatcher3DTest, OnlySameHistogramIsScoreOne) {
   Eigen::VectorXf histogram(7);
   histogram << 1.f, 43.f, 0.5f, 0.3123f, 23.f, 42.f, 0.f;
-  const std::vector<std::pair<Eigen::VectorXf, float>> histogram_at_angle = {
+  RotationalScanMatcher matcher(&histogram);
-      {histogram, 0.f}};
-  RotationalScanMatcher matcher(histogram_at_angle);
   const auto scores = matcher.Match(histogram, 0.f, {0.f, 1.f});
   ASSERT_EQ(2, scores.size());
   EXPECT_NEAR(1.f, scores[0], 1e-6);
@@ -41,9 +39,9 @@ TEST(RotationalScanMatcher3DTest, InterpolatesAsExpected) {
   constexpr int kNumBuckets = 10;
   constexpr float kAnglePerBucket = M_PI / kNumBuckets;
   constexpr float kNoInitialRotation = 0.f;
-  const std::vector<std::pair<Eigen::VectorXf, float>> histogram_at_angle = {
+  const Eigen::VectorXf histogram_no_initial_rotation =
-      {Eigen::VectorXf::Unit(kNumBuckets, 3), kNoInitialRotation}};
+      Eigen::VectorXf::Unit(kNumBuckets, 3);
-  RotationalScanMatcher matcher(histogram_at_angle);
+  RotationalScanMatcher matcher(&histogram_no_initial_rotation);
   for (float t = 0.f; t < 1.f; t += 0.1f) {
     // 't' is the fraction of overlap and we have to divide by the norm of the
     // histogram to get the expected score.

cartographer/mapping/internal/constraints/constraint_builder_3d.cc

@@ -77,7 +77,6 @@ ConstraintBuilder3D::~ConstraintBuilder3D() {
 void ConstraintBuilder3D::MaybeAddConstraint(
     const SubmapId& submap_id, const Submap3D* const submap,
     const NodeId& node_id, const TrajectoryNode::Data* const constant_data,
-    const std::vector<TrajectoryNode>& submap_nodes,
     const transform::Rigid3d& global_node_pose,
     const transform::Rigid3d& global_submap_pose) {
   if ((global_node_pose.translation() - global_submap_pose.translation())
@@ -94,8 +93,7 @@ void ConstraintBuilder3D::MaybeAddConstraint(
   constraints_.emplace_back();
   kQueueLengthMetric->Set(constraints_.size());
   auto* const constraint = &constraints_.back();
-  const auto* scan_matcher =
+  const auto* scan_matcher = DispatchScanMatcherConstruction(submap_id, submap);
-      DispatchScanMatcherConstruction(submap_id, submap_nodes, submap);
   auto constraint_task = absl::make_unique<common::Task>();
   constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
     ComputeConstraint(submap_id, node_id, false, /* match_full_submap */
@@ -111,7 +109,6 @@ void ConstraintBuilder3D::MaybeAddConstraint(
 void ConstraintBuilder3D::MaybeAddGlobalConstraint(
     const SubmapId& submap_id, const Submap3D* const submap,
     const NodeId& node_id, const TrajectoryNode::Data* const constant_data,
-    const std::vector<TrajectoryNode>& submap_nodes,
     const Eigen::Quaterniond& global_node_rotation,
     const Eigen::Quaterniond& global_submap_rotation) {
   absl::MutexLock locker(&mutex_);
@@ -122,8 +119,7 @@ void ConstraintBuilder3D::MaybeAddGlobalConstraint(
   constraints_.emplace_back();
   kQueueLengthMetric->Set(constraints_.size());
   auto* const constraint = &constraints_.back();
-  const auto* scan_matcher =
+  const auto* scan_matcher = DispatchScanMatcherConstruction(submap_id, submap);
-      DispatchScanMatcherConstruction(submap_id, submap_nodes, submap);
   auto constraint_task = absl::make_unique<common::Task>();
   constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
     ComputeConstraint(submap_id, node_id, true, /* match_full_submap */
@@ -165,8 +161,7 @@ void ConstraintBuilder3D::WhenDone(
 }
 
 const ConstraintBuilder3D::SubmapScanMatcher*
-ConstraintBuilder3D::DispatchScanMatcherConstruction(
+ConstraintBuilder3D::DispatchScanMatcherConstruction(const SubmapId& submap_id,
-    const SubmapId& submap_id, const std::vector<TrajectoryNode>& submap_nodes,
                                                      const Submap3D* submap) {
   if (submap_scan_matchers_.count(submap_id) != 0) {
     return &submap_scan_matchers_.at(submap_id);
@@ -178,13 +173,15 @@ ConstraintBuilder3D::DispatchScanMatcherConstruction(
       &submap->low_resolution_hybrid_grid();
   auto& scan_matcher_options =
       options_.fast_correlative_scan_matcher_options_3d();
+  const Eigen::VectorXf* histogram =
+      &submap->rotational_scan_matcher_histogram();
   auto scan_matcher_task = absl::make_unique<common::Task>();
   scan_matcher_task->SetWorkItem(
-      [&submap_scan_matcher, &scan_matcher_options, submap_nodes]() {
+      [&submap_scan_matcher, &scan_matcher_options, histogram]() {
         submap_scan_matcher.fast_correlative_scan_matcher =
             absl::make_unique<scan_matching::FastCorrelativeScanMatcher3D>(
                 *submap_scan_matcher.high_resolution_hybrid_grid,
-                submap_scan_matcher.low_resolution_hybrid_grid, submap_nodes,
+                submap_scan_matcher.low_resolution_hybrid_grid, histogram,
                 scan_matcher_options);
       });
   submap_scan_matcher.creation_task_handle =

cartographer/mapping/internal/constraints/constraint_builder_3d.h

@@ -78,7 +78,6 @@ class ConstraintBuilder3D {
   void MaybeAddConstraint(const SubmapId& submap_id, const Submap3D* submap,
                           const NodeId& node_id,
                           const TrajectoryNode::Data* const constant_data,
-                          const std::vector<TrajectoryNode>& submap_nodes,
                           const transform::Rigid3d& global_node_pose,
                           const transform::Rigid3d& global_submap_pose);
 
@@ -94,7 +93,6 @@ class ConstraintBuilder3D {
   void MaybeAddGlobalConstraint(
       const SubmapId& submap_id, const Submap3D* submap, const NodeId& node_id,
       const TrajectoryNode::Data* const constant_data,
-      const std::vector<TrajectoryNode>& submap_nodes,
       const Eigen::Quaterniond& global_node_rotation,
       const Eigen::Quaterniond& global_submap_rotation);
 
@@ -126,8 +124,7 @@ class ConstraintBuilder3D {
   // The returned 'grid' and 'fast_correlative_scan_matcher' must only be
   // accessed after 'creation_task_handle' has completed.
   const SubmapScanMatcher* DispatchScanMatcherConstruction(
-      const SubmapId& submap_id,
+      const SubmapId& submap_id, const Submap3D* submap)
-      const std::vector<TrajectoryNode>& submap_nodes, const Submap3D* submap)
       EXCLUSIVE_LOCKS_REQUIRED(mutex_);
 
   // Runs in a background thread and does computations for an additional

cartographer/mapping/internal/constraints/constraint_builder_3d_test.cc

@@ -71,7 +71,6 @@ TEST_F(ConstraintBuilder3DTest, CallsBack) {
 }
 
 TEST_F(ConstraintBuilder3DTest, FindsConstraints) {
-  TrajectoryNode node;
   auto node_data = std::make_shared<TrajectoryNode::Data>();
   node_data->gravity_alignment = Eigen::Quaterniond::Identity();
   node_data->high_resolution_point_cloud.push_back(
@@ -80,23 +79,20 @@ TEST_F(ConstraintBuilder3DTest, FindsConstraints) {
       {Eigen::Vector3f(0.1, 0.2, 0.3)});
   node_data->rotational_scan_matcher_histogram = Eigen::VectorXf::Zero(3);
   node_data->local_pose = transform::Rigid3d::Identity();
-  node.constant_data = node_data;
-  std::vector<TrajectoryNode> submap_nodes = {node};
   SubmapId submap_id{0, 1};
-  Submap3D submap(0.1, 0.1, transform::Rigid3d::Identity());
+  Submap3D submap(0.1, 0.1, transform::Rigid3d::Identity(),
+                  Eigen::VectorXf::Zero(3));
   int expected_nodes = 0;
   for (int i = 0; i < 2; ++i) {
     EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), expected_nodes);
     for (int j = 0; j < 2; ++j) {
       constraint_builder_->MaybeAddConstraint(
-          submap_id, &submap, NodeId{0, 0}, node.constant_data.get(),
+          submap_id, &submap, NodeId{0, 0}, node_data.get(),
-          submap_nodes, transform::Rigid3d::Identity(),
+          transform::Rigid3d::Identity(), transform::Rigid3d::Identity());
-          transform::Rigid3d::Identity());
     }
     constraint_builder_->MaybeAddGlobalConstraint(
-        submap_id, &submap, NodeId{0, 0}, node.constant_data.get(),
+        submap_id, &submap, NodeId{0, 0}, node_data.get(),
-        submap_nodes, Eigen::Quaterniond::Identity(),
+        Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity());
-        Eigen::Quaterniond::Identity());
     constraint_builder_->NotifyEndOfNode();
     thread_pool_.WaitUntilIdle();
     EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), ++expected_nodes);