Keep 'trajectory_nodes_' per trajectory. (#291)

Fixes #256.

PAIR=SirVer

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -97,16 +97,19 @@ void SparsePoseGraph::AddScan(
       GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose));
 
   common::MutexLocker locker(&mutex_);
-  const int flat_scan_index = trajectory_nodes_.size();
+  const int flat_scan_index = num_trajectory_nodes_;
   CHECK_LT(flat_scan_index, std::numeric_limits<int>::max());
 
   constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
       time, range_data_in_pose,
       Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}),
       trajectory_id, tracking_to_pose});
-  trajectory_nodes_.push_back(mapping::TrajectoryNode{
+  trajectory_nodes_.resize(
+      std::max<size_t>(trajectory_nodes_.size(), trajectory_id + 1));
+  trajectory_nodes_[trajectory_id].push_back(mapping::TrajectoryNode{
       &constant_node_data_.back(), optimized_pose,
   });
+  ++num_trajectory_nodes_;
   trajectory_connectivity_.Add(trajectory_id);
 
   if (submap_ids_.count(insertion_submaps.back()) == 0) {
@@ -157,10 +160,31 @@ void SparsePoseGraph::AddImuData(const int trajectory_id, common::Time time,
   });
 }
 
-void SparsePoseGraph::ComputeConstraint(const int scan_index,
+void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
                                         const mapping::SubmapId& submap_id) {
-  const mapping::NodeId node_id = scan_index_to_node_id_.at(scan_index);
+  // Only globally match against submaps not in this trajectory.
-  const transform::Rigid2d relative_pose = optimization_problem_.submap_data()
+  if (node_id.trajectory_id != submap_id.trajectory_id &&
+      global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
+    constraint_builder_.MaybeAddGlobalConstraint(
+        submap_id,
+        submap_states_.at(submap_id.trajectory_id)
+            .at(submap_id.submap_index)
+            .submap,
+        node_id,
+        &trajectory_nodes_.at(node_id.trajectory_id)
+             .at(node_id.node_index)
+             .constant_data->range_data_2d.returns,
+        &trajectory_connectivity_);
+  } else {
+    const bool scan_and_submap_trajectories_connected =
+        reverse_connected_components_.count(node_id.trajectory_id) > 0 &&
+        reverse_connected_components_.count(submap_id.trajectory_id) > 0 &&
+        reverse_connected_components_.at(node_id.trajectory_id) ==
+            reverse_connected_components_.at(submap_id.trajectory_id);
+    if (node_id.trajectory_id == submap_id.trajectory_id ||
+        scan_and_submap_trajectories_connected) {
+      const transform::Rigid2d initial_relative_pose =
+          optimization_problem_.submap_data()
               .at(submap_id.trajectory_id)
               .at(submap_id.submap_index)
               .pose.inverse() *
@@ -168,35 +192,17 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
               .at(node_id.trajectory_id)
               .at(node_id.node_index)
               .point_cloud_pose;
-  const int scan_trajectory_id =
-      trajectory_nodes_[scan_index].constant_data->trajectory_id;
 
-  // Only globally match against submaps not in this trajectory.
-  if (scan_trajectory_id != submap_id.trajectory_id &&
-      global_localization_samplers_[scan_trajectory_id]->Pulse()) {
-    constraint_builder_.MaybeAddGlobalConstraint(
-        submap_id,
-        submap_states_.at(submap_id.trajectory_id)
-            .at(submap_id.submap_index)
-            .submap,
-        node_id, scan_index, &trajectory_connectivity_,
-        &trajectory_nodes_[scan_index].constant_data->range_data_2d.returns);
-  } else {
-    const bool scan_and_submap_trajectories_connected =
-        reverse_connected_components_.count(scan_trajectory_id) > 0 &&
-        reverse_connected_components_.count(submap_id.trajectory_id) > 0 &&
-        reverse_connected_components_.at(scan_trajectory_id) ==
-            reverse_connected_components_.at(submap_id.trajectory_id);
-    if (scan_trajectory_id == submap_id.trajectory_id ||
-        scan_and_submap_trajectories_connected) {
       constraint_builder_.MaybeAddConstraint(
           submap_id,
           submap_states_.at(submap_id.trajectory_id)
               .at(submap_id.submap_index)
               .submap,
-          node_id, scan_index,
+          node_id,
-          &trajectory_nodes_[scan_index].constant_data->range_data_2d.returns,
+          &trajectory_nodes_.at(node_id.trajectory_id)
-          relative_pose);
+               .at(node_id.node_index)
+               .constant_data->range_data_2d.returns,
+          initial_relative_pose);
     }
   }
 }
@@ -209,7 +215,7 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
   const int num_nodes = scan_index_to_node_id_.size();
   for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
     if (submap_state.node_ids.count(scan_index_to_node_id_[scan_index]) == 0) {
-      ComputeConstraint(scan_index, submap_id);
+      ComputeConstraint(scan_index_to_node_id_.at(scan_index), submap_id);
     }
   }
 }
@@ -234,7 +240,9 @@ void SparsePoseGraph::ComputeConstraintsForScan(
   scan_index_to_node_id_.push_back(node_id);
   ++num_nodes_in_trajectory_[matching_id.trajectory_id];
   const mapping::TrajectoryNode::ConstantData* const scan_data =
-      trajectory_nodes_[scan_index].constant_data;
+      trajectory_nodes_.at(node_id.trajectory_id)
+          .at(node_id.node_index)
+          .constant_data;
   CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id);
   optimization_problem_.AddTrajectoryNode(
       matching_id.trajectory_id, scan_data->time, pose, optimized_pose);
@@ -273,7 +281,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
                      .at(submap_index)
                      .node_ids.count(node_id),
                  0);
-        ComputeConstraint(scan_index,
+        ComputeConstraint(node_id,
                           mapping::SubmapId{static_cast<int>(trajectory_id),
                                             static_cast<int>(submap_index)});
       }
@@ -291,7 +299,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
     ComputeConstraintsForOldScans(finished_submap);
     finished_submap_state.finished = true;
   }
-  constraint_builder_.NotifyEndOfScan(scan_index);
+  constraint_builder_.NotifyEndOfScan();
   ++num_scans_since_last_loop_closure_;
   if (options_.optimize_every_n_scans() > 0 &&
       num_scans_since_last_loop_closure_ > options_.optimize_every_n_scans()) {
@@ -335,8 +343,8 @@ void SparsePoseGraph::WaitForAllComputations() {
       constraint_builder_.GetNumFinishedScans();
   while (!locker.AwaitWithTimeout(
       [this]() REQUIRES(mutex_) {
-        return static_cast<size_t>(constraint_builder_.GetNumFinishedScans()) ==
+        return constraint_builder_.GetNumFinishedScans() ==
-               trajectory_nodes_.size();
+               num_trajectory_nodes_;
       },
       common::FromSeconds(1.))) {
     std::ostringstream progress_info;
@@ -344,8 +352,7 @@ void SparsePoseGraph::WaitForAllComputations() {
                   << 100. *
                          (constraint_builder_.GetNumFinishedScans() -
                           num_finished_scans_at_start) /
-                         (trajectory_nodes_.size() -
+                         (num_trajectory_nodes_ - num_finished_scans_at_start)
-                          num_finished_scans_at_start)
                   << "%...";
     std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
   }
@@ -379,30 +386,27 @@ void SparsePoseGraph::RunOptimization() {
   common::MutexLocker locker(&mutex_);
 
   const auto& node_data = optimization_problem_.node_data();
-  const size_t num_optimized_poses = scan_index_to_node_id_.size();
+  for (size_t trajectory_id = 0; trajectory_id != node_data.size();
-  for (size_t i = 0; i != num_optimized_poses; ++i) {
+       ++trajectory_id) {
-    const mapping::NodeId node_id = scan_index_to_node_id_[i];
+    size_t node_index = 0;
-    trajectory_nodes_[i].pose =
+    const size_t num_nodes = trajectory_nodes_.at(trajectory_id).size();
-        transform::Embed3D(node_data.at(node_id.trajectory_id)
+    for (; node_index != node_data[trajectory_id].size(); ++node_index) {
-                               .at(node_id.node_index)
+      trajectory_nodes_[trajectory_id][node_index].pose = transform::Embed3D(
-                               .point_cloud_pose);
+          node_data[trajectory_id][node_index].point_cloud_pose);
     }
     // Extrapolate all point cloud poses that were added later.
-  std::unordered_map<int, transform::Rigid3d> extrapolation_transforms;
-  for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) {
-    const int trajectory_id = trajectory_nodes_[i].constant_data->trajectory_id;
-    if (extrapolation_transforms.count(trajectory_id) == 0) {
     const auto new_submap_transforms = ExtrapolateSubmapTransforms(
         optimization_problem_.submap_data(), trajectory_id);
     const auto old_submap_transforms = ExtrapolateSubmapTransforms(
         optimized_submap_transforms_, trajectory_id);
     CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size());
-      extrapolation_transforms[trajectory_id] =
+    const transform::Rigid3d extrapolation_transform =
-          transform::Rigid3d(new_submap_transforms.back() *
+        new_submap_transforms.back() * old_submap_transforms.back().inverse();
-                             old_submap_transforms.back().inverse());
+    for (; node_index < num_nodes; ++node_index) {
+      trajectory_nodes_[trajectory_id][node_index].pose =
+          extrapolation_transform *
+          trajectory_nodes_[trajectory_id][node_index].pose;
     }
-    trajectory_nodes_[i].pose =
-        extrapolation_transforms[trajectory_id] * trajectory_nodes_[i].pose;
   }
   optimized_submap_transforms_ = optimization_problem_.submap_data();
   connected_components_ = trajectory_connectivity_.ConnectedComponents();
@@ -417,13 +421,7 @@ void SparsePoseGraph::RunOptimization() {
 std::vector<std::vector<mapping::TrajectoryNode>>
 SparsePoseGraph::GetTrajectoryNodes() {
   common::MutexLocker locker(&mutex_);
-  std::vector<std::vector<mapping::TrajectoryNode>> result;
+  return trajectory_nodes_;
-  for (const auto& node : trajectory_nodes_) {
-    result.resize(
-        std::max<size_t>(result.size(), node.constant_data->trajectory_id + 1));
-    result.at(node.constant_data->trajectory_id).push_back(node);
-  }
-  return result;
 }
 
 std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {

cartographer/mapping_2d/sparse_pose_graph.h

@@ -129,7 +129,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       const kalman_filter::Pose2DCovariance& covariance) REQUIRES(mutex_);
 
   // Computes constraints for a scan and submap pair.
-  void ComputeConstraint(const int scan_index,
+  void ComputeConstraint(const mapping::NodeId& node_id,
                          const mapping::SubmapId& submap_id) REQUIRES(mutex_);
 
   // Adds constraints for older scans whenever a new submap is finished.
@@ -201,7 +201,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // Deque to keep references valid for the background computation when adding
   // new data.
   std::deque<mapping::TrajectoryNode::ConstantData> constant_node_data_;
-  std::vector<mapping::TrajectoryNode> trajectory_nodes_ GUARDED_BY(mutex_);
+  std::vector<std::vector<mapping::TrajectoryNode>> trajectory_nodes_
+      GUARDED_BY(mutex_);
+  int num_trajectory_nodes_ = 0 GUARDED_BY(mutex_);
 
   // Current submap transforms used for displaying data.
   std::vector<std::vector<sparse_pose_graph::SubmapData>>

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc

@@ -62,8 +62,7 @@ ConstraintBuilder::~ConstraintBuilder() {
 
 void ConstraintBuilder::MaybeAddConstraint(
     const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
-    const mapping::NodeId& node_id, const int flat_scan_index,
+    const mapping::NodeId& node_id, const sensor::PointCloud* const point_cloud,
-    const sensor::PointCloud* const point_cloud,
     const transform::Rigid2d& initial_relative_pose) {
   if (initial_relative_pose.translation().norm() >
       options_.max_constraint_distance()) {
@@ -71,7 +70,6 @@ void ConstraintBuilder::MaybeAddConstraint(
   }
   if (sampler_.Pulse()) {
     common::MutexLocker locker(&mutex_);
-    CHECK_LE(flat_scan_index, current_computation_);
     constraints_.emplace_back();
     auto* const constraint = &constraints_.back();
     ++pending_computations_[current_computation_];
@@ -89,11 +87,9 @@ void ConstraintBuilder::MaybeAddConstraint(
 
 void ConstraintBuilder::MaybeAddGlobalConstraint(
     const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
-    const mapping::NodeId& node_id, const int flat_scan_index,
+    const mapping::NodeId& node_id, const sensor::PointCloud* const point_cloud,
-    mapping::TrajectoryConnectivity* trajectory_connectivity,
+    mapping::TrajectoryConnectivity* const trajectory_connectivity) {
-    const sensor::PointCloud* const point_cloud) {
   common::MutexLocker locker(&mutex_);
-  CHECK_LE(flat_scan_index, current_computation_);
   constraints_.emplace_back();
   auto* const constraint = &constraints_.back();
   ++pending_computations_[current_computation_];
@@ -108,9 +104,8 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
       });
 }
 
-void ConstraintBuilder::NotifyEndOfScan(const int flat_scan_index) {
+void ConstraintBuilder::NotifyEndOfScan() {
   common::MutexLocker locker(&mutex_);
-  CHECK_EQ(current_computation_, flat_scan_index);
   ++current_computation_;
 }
 

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h

@@ -72,37 +72,32 @@ class ConstraintBuilder {
   ConstraintBuilder& operator=(const ConstraintBuilder&) = delete;
 
   // Schedules exploring a new constraint between 'submap' identified by
-  // 'submap_id', and the 'point_cloud' for 'flat_scan_index'.
+  // 'submap_id', and the 'point_cloud' for 'node_id'. The 'initial_pose' is
-  // The 'initial_pose' is relative to the 'submap'.
+  // relative to the 'submap'.
   //
   // The pointees of 'submap' and 'point_cloud' must stay valid until all
   // computations are finished.
   void MaybeAddConstraint(const mapping::SubmapId& submap_id,
                           const mapping::Submap* submap,
                           const mapping::NodeId& node_id,
-                          const int flat_scan_index,
                           const sensor::PointCloud* point_cloud,
                           const transform::Rigid2d& initial_relative_pose);
 
   // Schedules exploring a new constraint between 'submap' identified by
-  // 'submap_id' and the 'point_cloud' for 'flat_scan_index'. This performs
+  // 'submap_id' and the 'point_cloud' for 'node_id'. This performs full-submap
-  // full-submap matching.
+  // matching.
   //
-  // The scan at 'flat_scan_index' should be from trajectory
+  // The 'trajectory_connectivity' is updated if the full-submap match succeeds.
-  // 'node_id.trajectory_id'. The 'trajectory_connectivity' is updated if the
-  // full-submap match succeeds.
   //
   // The pointees of 'submap' and 'point_cloud' must stay valid until all
   // computations are finished.
   void MaybeAddGlobalConstraint(
       const mapping::SubmapId& submap_id, const mapping::Submap* submap,
-      const mapping::NodeId& node_id, const int flat_scan_index,
+      const mapping::NodeId& node_id, const sensor::PointCloud* point_cloud,
-      mapping::TrajectoryConnectivity* trajectory_connectivity,
+      mapping::TrajectoryConnectivity* trajectory_connectivity);
-      const sensor::PointCloud* point_cloud);
 
-  // Must be called after all computations related to 'flat_scan_index' are
+  // Must be called after all computations related to one node have been added.
-  // added.
+  void NotifyEndOfScan();
-  void NotifyEndOfScan(const int flat_scan_index);
 
   // Registers the 'callback' to be called with the results, after all
   // computations triggered by MaybeAddConstraint() have finished.

cartographer/mapping_3d/global_trajectory_builder.cc

@@ -46,8 +46,8 @@ void GlobalTrajectoryBuilder::AddImuData(
 void GlobalTrajectoryBuilder::AddRangefinderData(
     const common::Time time, const Eigen::Vector3f& origin,
     const sensor::PointCloud& ranges) {
-  auto insertion_result = local_trajectory_builder_->AddRangefinderData(
+  auto insertion_result =
-      time, origin, ranges, sparse_pose_graph_->GetNextTrajectoryNodeIndex());
+      local_trajectory_builder_->AddRangefinderData(time, origin, ranges);
 
   if (insertion_result == nullptr) {
     return;

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, int next_trajectory_node_index) {
+    const sensor::PointCloud& ranges) {
   if (!pose_tracker_) {
     LOG(INFO) << "PoseTracker not yet initialized.";
     return nullptr;
@@ -114,18 +114,15 @@ KalmanLocalTrajectoryBuilder::AddRangefinderData(
   if (num_accumulated_ >= options_.scans_per_accumulation()) {
     num_accumulated_ = 0;
     return AddAccumulatedRangeData(
-        time,
+        time, sensor::TransformRangeData(accumulated_range_data_,
-        sensor::TransformRangeData(accumulated_range_data_,
+                                         tracking_delta.inverse()));
-                                   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,
@@ -184,7 +181,7 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
                                   pose_observation.cast<float>())};
 
   return InsertIntoSubmap(time, filtered_range_data, pose_observation,
-                          covariance_estimate, trajectory_node_index);
+                          covariance_estimate);
 }
 
 void KalmanLocalTrajectoryBuilder::AddOdometerData(
@@ -213,8 +210,7 @@ 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;
   }
@@ -224,10 +220,8 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
   for (int insertion_index : submaps_->insertion_indices()) {
     insertion_submaps.push_back(submaps_->Get(insertion_index));
   }
-  submaps_->InsertRangeData(
+  submaps_->InsertRangeData(sensor::TransformRangeData(
-      sensor::TransformRangeData(range_data_in_tracking,
+      range_data_in_tracking, pose_observation.cast<float>()));
-                                 pose_observation.cast<float>()),
-      trajectory_node_index);
   return std::unique_ptr<InsertionResult>(new InsertionResult{
       time, range_data_in_tracking, pose_observation, covariance_estimate,
       matching_submap, insertion_submaps});

cartographer/mapping_3d/kalman_local_trajectory_builder.h

@@ -50,8 +50,7 @@ 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,
+      const sensor::PointCloud& ranges) override;
-      int next_trajectory_node_index) override;
   void AddOdometerData(common::Time time,
                        const transform::Rigid3d& pose) override;
   const mapping_3d::Submaps* submaps() const override;
@@ -59,14 +58,12 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface {
 
  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(
       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

@@ -260,8 +260,7 @@ 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, num_poses) !=
+              node.time, range_data.origin, range_data.returns) != nullptr) {
-          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

@@ -54,7 +54,7 @@ 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, int next_trajectory_node_index) = 0;
+      const sensor::PointCloud& ranges) = 0;
   virtual void AddOdometerData(common::Time time,
                                const transform::Rigid3d& pose) = 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 int next_trajectory_node_index) {
+    const sensor::PointCloud& ranges) {
   CHECK_GT(ranges.size(), 0);
 
   // TODO(hrapp): Handle misses.
@@ -188,7 +188,7 @@ OptimizingLocalTrajectoryBuilder::AddRangefinderData(
   ++num_accumulated_;
 
   RemoveObsoleteSensorData();
-  return MaybeOptimize(time, next_trajectory_node_index);
+  return MaybeOptimize(time);
 }
 
 void OptimizingLocalTrajectoryBuilder::RemoveObsoleteSensorData() {
@@ -215,8 +215,7 @@ void OptimizingLocalTrajectoryBuilder::RemoveObsoleteSensorData() {
 }
 
 std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
-OptimizingLocalTrajectoryBuilder::MaybeOptimize(
+OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
-    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) {
@@ -349,15 +348,13 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(
   }
 
   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,
@@ -375,8 +372,7 @@ 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&
@@ -387,8 +383,7 @@ OptimizingLocalTrajectoryBuilder::pose_estimate() const {
 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,10 +393,8 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap(
   for (int insertion_index : submaps_->insertion_indices()) {
     insertion_submaps.push_back(submaps_->Get(insertion_index));
   }
-  submaps_->InsertRangeData(
+  submaps_->InsertRangeData(sensor::TransformRangeData(
-      sensor::TransformRangeData(range_data_in_tracking,
+      range_data_in_tracking, pose_observation.cast<float>()));
-                                 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,8 +53,7 @@ class OptimizingLocalTrajectoryBuilder
                   const Eigen::Vector3d& angular_velocity) override;
   std::unique_ptr<InsertionResult> AddRangefinderData(
       common::Time time, const Eigen::Vector3f& origin,
-      const sensor::PointCloud& ranges,
+      const sensor::PointCloud& ranges) override;
-      int next_trajectory_node_index) override;
   void AddOdometerData(common::Time time,
                        const transform::Rigid3d& pose) override;
   const mapping_3d::Submaps* submaps() const override;
@@ -102,16 +101,13 @@ 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(
+  std::unique_ptr<InsertionResult> MaybeOptimize(common::Time time);
-      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

@@ -96,15 +96,18 @@ void SparsePoseGraph::AddScan(
       GetLocalToGlobalTransform(trajectory_id) * pose);
 
   common::MutexLocker locker(&mutex_);
-  const int flat_scan_index = trajectory_nodes_.size();
+  const int flat_scan_index = num_trajectory_nodes_;
   CHECK_LT(flat_scan_index, std::numeric_limits<int>::max());
 
   constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
       time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}},
       sensor::Compress(range_data_in_tracking), trajectory_id,
       transform::Rigid3d::Identity()});
-  trajectory_nodes_.push_back(
+  trajectory_nodes_.resize(
+      std::max<size_t>(trajectory_nodes_.size(), trajectory_id + 1));
+  trajectory_nodes_[trajectory_id].push_back(
       mapping::TrajectoryNode{&constant_node_data_.back(), optimized_pose});
+  ++num_trajectory_nodes_;
   trajectory_connectivity_.Add(trajectory_id);
 
   if (submap_ids_.count(insertion_submaps.back()) == 0) {
@@ -135,11 +138,6 @@ void SparsePoseGraph::AddScan(
   });
 }
 
-int SparsePoseGraph::GetNextTrajectoryNodeIndex() {
-  common::MutexLocker locker(&mutex_);
-  return trajectory_nodes_.size();
-}
-
 void SparsePoseGraph::AddWorkItem(std::function<void()> work_item) {
   if (scan_queue_ == nullptr) {
     work_item();
@@ -158,43 +156,64 @@ void SparsePoseGraph::AddImuData(const int trajectory_id, common::Time time,
   });
 }
 
-void SparsePoseGraph::ComputeConstraint(const int scan_index,
+void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
                                         const mapping::SubmapId& submap_id) {
-  const mapping::NodeId node_id = scan_index_to_node_id_.at(scan_index);
+  const transform::Rigid3d inverse_submap_pose =
-  const transform::Rigid3d relative_pose = optimization_problem_.submap_data()
+      optimization_problem_.submap_data()
           .at(submap_id.trajectory_id)
           .at(submap_id.submap_index)
-                                               .pose.inverse() *
+          .pose.inverse();
-                                           optimization_problem_.node_data()
+
-                                               .at(node_id.trajectory_id)
+  std::vector<mapping::TrajectoryNode> submap_nodes;
-                                               .at(node_id.node_index)
+  for (const mapping::NodeId& submap_node_id :
-                                               .point_cloud_pose;
+       submap_states_.at(submap_id.trajectory_id)
-  const int scan_trajectory_id =
+           .at(submap_id.submap_index)
-      trajectory_nodes_[scan_index].constant_data->trajectory_id;
+           .node_ids) {
+    submap_nodes.push_back(mapping::TrajectoryNode{
+        trajectory_nodes_.at(submap_node_id.trajectory_id)
+            .at(submap_node_id.node_index)
+            .constant_data,
+        inverse_submap_pose * trajectory_nodes_.at(submap_node_id.trajectory_id)
+                                  .at(submap_node_id.node_index)
+                                  .pose});
+  }
 
   // Only globally match against submaps not in this trajectory.
-  if (scan_trajectory_id != submap_id.trajectory_id &&
+  if (node_id.trajectory_id != submap_id.trajectory_id &&
-      global_localization_samplers_[scan_trajectory_id]->Pulse()) {
+      global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
     constraint_builder_.MaybeAddGlobalConstraint(
         submap_id,
         submap_states_.at(submap_id.trajectory_id)
             .at(submap_id.submap_index)
             .submap,
-        node_id, scan_index, &trajectory_connectivity_, trajectory_nodes_);
+        node_id,
+        &trajectory_nodes_.at(node_id.trajectory_id)
+             .at(node_id.node_index)
+             .constant_data->range_data_3d.returns,
+        submap_nodes, &trajectory_connectivity_);
   } else {
     const bool scan_and_submap_trajectories_connected =
-        reverse_connected_components_.count(scan_trajectory_id) > 0 &&
+        reverse_connected_components_.count(node_id.trajectory_id) > 0 &&
         reverse_connected_components_.count(submap_id.trajectory_id) > 0 &&
-        reverse_connected_components_.at(scan_trajectory_id) ==
+        reverse_connected_components_.at(node_id.trajectory_id) ==
             reverse_connected_components_.at(submap_id.trajectory_id);
-    if (scan_trajectory_id == submap_id.trajectory_id ||
+    if (node_id.trajectory_id == submap_id.trajectory_id ||
         scan_and_submap_trajectories_connected) {
+      const transform::Rigid3d initial_relative_pose =
+          inverse_submap_pose * optimization_problem_.node_data()
+                                    .at(node_id.trajectory_id)
+                                    .at(node_id.node_index)
+                                    .point_cloud_pose;
       constraint_builder_.MaybeAddConstraint(
           submap_id,
           submap_states_.at(submap_id.trajectory_id)
               .at(submap_id.submap_index)
               .submap,
-          node_id, scan_index, trajectory_nodes_, relative_pose);
+          node_id,
+          &trajectory_nodes_.at(node_id.trajectory_id)
+               .at(node_id.node_index)
+               .constant_data->range_data_3d.returns,
+          submap_nodes, initial_relative_pose);
     }
   }
 }
@@ -206,7 +225,7 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) {
   const int num_nodes = scan_index_to_node_id_.size();
   for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
     if (submap_state.node_ids.count(scan_index_to_node_id_[scan_index]) == 0) {
-      ComputeConstraint(scan_index, submap_id);
+      ComputeConstraint(scan_index_to_node_id_.at(scan_index), submap_id);
     }
   }
 }
@@ -231,7 +250,9 @@ void SparsePoseGraph::ComputeConstraintsForScan(
   scan_index_to_node_id_.push_back(node_id);
   ++num_nodes_in_trajectory_[matching_id.trajectory_id];
   const mapping::TrajectoryNode::ConstantData* const scan_data =
-      trajectory_nodes_[scan_index].constant_data;
+      trajectory_nodes_.at(node_id.trajectory_id)
+          .at(node_id.node_index)
+          .constant_data;
   CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id);
   optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id,
                                           scan_data->time, optimized_pose);
@@ -266,7 +287,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
                      .at(submap_index)
                      .node_ids.count(node_id),
                  0);
-        ComputeConstraint(scan_index,
+        ComputeConstraint(node_id,
                           mapping::SubmapId{static_cast<int>(trajectory_id),
                                             static_cast<int>(submap_index)});
       }
@@ -284,7 +305,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
     ComputeConstraintsForOldScans(finished_submap);
     finished_submap_state.finished = true;
   }
-  constraint_builder_.NotifyEndOfScan(scan_index);
+  constraint_builder_.NotifyEndOfScan();
   ++num_scans_since_last_loop_closure_;
   if (options_.optimize_every_n_scans() > 0 &&
       num_scans_since_last_loop_closure_ > options_.optimize_every_n_scans()) {
@@ -328,8 +349,8 @@ void SparsePoseGraph::WaitForAllComputations() {
       constraint_builder_.GetNumFinishedScans();
   while (!locker.AwaitWithTimeout(
       [this]() REQUIRES(mutex_) {
-        return static_cast<size_t>(constraint_builder_.GetNumFinishedScans()) ==
+        return constraint_builder_.GetNumFinishedScans() ==
-               trajectory_nodes_.size();
+               num_trajectory_nodes_;
       },
       common::FromSeconds(1.))) {
     std::ostringstream progress_info;
@@ -337,8 +358,7 @@ void SparsePoseGraph::WaitForAllComputations() {
                   << 100. *
                          (constraint_builder_.GetNumFinishedScans() -
                           num_finished_scans_at_start) /
-                         (trajectory_nodes_.size() -
+                         (num_trajectory_nodes_ - num_finished_scans_at_start)
-                          num_finished_scans_at_start)
                   << "%...";
     std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
   }
@@ -372,29 +392,27 @@ void SparsePoseGraph::RunOptimization() {
   common::MutexLocker locker(&mutex_);
 
   const auto& node_data = optimization_problem_.node_data();
-  const size_t num_optimized_poses = scan_index_to_node_id_.size();
+  for (size_t trajectory_id = 0; trajectory_id != node_data.size();
-  for (size_t i = 0; i != num_optimized_poses; ++i) {
+       ++trajectory_id) {
-    const mapping::NodeId node_id = scan_index_to_node_id_[i];
+    size_t node_index = 0;
-    trajectory_nodes_[i].pose = node_data.at(node_id.trajectory_id)
+    const size_t num_nodes = trajectory_nodes_.at(trajectory_id).size();
-                                    .at(node_id.node_index)
+    for (; node_index != node_data[trajectory_id].size(); ++node_index) {
-                                    .point_cloud_pose;
+      trajectory_nodes_[trajectory_id][node_index].pose =
+          node_data[trajectory_id][node_index].point_cloud_pose;
     }
     // Extrapolate all point cloud poses that were added later.
-  std::unordered_map<int, transform::Rigid3d> extrapolation_transforms;
-  for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) {
-    const int trajectory_id = trajectory_nodes_[i].constant_data->trajectory_id;
-    if (extrapolation_transforms.count(trajectory_id) == 0) {
     const auto new_submap_transforms = ExtrapolateSubmapTransforms(
         optimization_problem_.submap_data(), trajectory_id);
     const auto old_submap_transforms = ExtrapolateSubmapTransforms(
         optimized_submap_transforms_, trajectory_id);
     CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size());
-      extrapolation_transforms[trajectory_id] =
+    const transform::Rigid3d extrapolation_transform =
-          transform::Rigid3d(new_submap_transforms.back() *
+        new_submap_transforms.back() * old_submap_transforms.back().inverse();
-                             old_submap_transforms.back().inverse());
+    for (; node_index < num_nodes; ++node_index) {
+      trajectory_nodes_[trajectory_id][node_index].pose =
+          extrapolation_transform *
+          trajectory_nodes_[trajectory_id][node_index].pose;
     }
-    trajectory_nodes_[i].pose =
-        extrapolation_transforms[trajectory_id] * trajectory_nodes_[i].pose;
   }
   optimized_submap_transforms_ = optimization_problem_.submap_data();
   connected_components_ = trajectory_connectivity_.ConnectedComponents();
@@ -409,13 +427,7 @@ void SparsePoseGraph::RunOptimization() {
 std::vector<std::vector<mapping::TrajectoryNode>>
 SparsePoseGraph::GetTrajectoryNodes() {
   common::MutexLocker locker(&mutex_);
-  std::vector<std::vector<mapping::TrajectoryNode>> result;
+  return trajectory_nodes_;
-  for (const auto& node : trajectory_nodes_) {
-    result.resize(
-        std::max<size_t>(result.size(), node.constant_data->trajectory_id + 1));
-    result.at(node.constant_data->trajectory_id).push_back(node);
-  }
-  return result;
 }
 
 std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {

cartographer/mapping_3d/sparse_pose_graph.h

@@ -73,10 +73,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
                const std::vector<const Submap*>& insertion_submaps)
       EXCLUDES(mutex_);
 
-  // The index into the flat vector of trajectory nodes used internally 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(int trajectory_id, common::Time time,
                   const Eigen::Vector3d& linear_acceleration,
@@ -131,7 +127,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       const kalman_filter::PoseCovariance& covariance) REQUIRES(mutex_);
 
   // Computes constraints for a scan and submap pair.
-  void ComputeConstraint(const int scan_index,
+  void ComputeConstraint(const mapping::NodeId& node_id,
                          const mapping::SubmapId& submap_id) REQUIRES(mutex_);
 
   // Adds constraints for older scans whenever a new submap is finished.
@@ -202,7 +198,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // Deque to keep references valid for the background computation when adding
   // new data.
   std::deque<mapping::TrajectoryNode::ConstantData> constant_node_data_;
-  std::vector<mapping::TrajectoryNode> trajectory_nodes_ GUARDED_BY(mutex_);
+  std::vector<std::vector<mapping::TrajectoryNode>> trajectory_nodes_
+      GUARDED_BY(mutex_);
+  int num_trajectory_nodes_ = 0 GUARDED_BY(mutex_);
 
   // Current submap transforms used for displaying data.
   std::vector<std::vector<sparse_pose_graph::SubmapData>>

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc

@@ -39,25 +39,6 @@ namespace cartographer {
 namespace mapping_3d {
 namespace sparse_pose_graph {
 
-namespace {
-
-std::vector<mapping::TrajectoryNode> ComputeSubmapNodes(
-    const std::vector<mapping::TrajectoryNode>& trajectory_nodes,
-    const Submap* const submap, const int flat_scan_index,
-    const transform::Rigid3d& initial_relative_pose) {
-  std::vector<mapping::TrajectoryNode> submap_nodes;
-  for (const int node_index : submap->trajectory_node_indices) {
-    submap_nodes.push_back(mapping::TrajectoryNode{
-        trajectory_nodes[node_index].constant_data,
-        transform::Rigid3d(initial_relative_pose *
-                           trajectory_nodes[flat_scan_index].pose.inverse() *
-                           trajectory_nodes[node_index].pose)});
-  }
-  return submap_nodes;
-}
-
-}  // namespace
-
 ConstraintBuilder::ConstraintBuilder(
     const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions& options,
     common::ThreadPool* const thread_pool)
@@ -77,31 +58,27 @@ ConstraintBuilder::~ConstraintBuilder() {
 
 void ConstraintBuilder::MaybeAddConstraint(
     const mapping::SubmapId& submap_id, const Submap* const submap,
-    const mapping::NodeId& node_id, const int flat_scan_index,
+    const mapping::NodeId& node_id,
-    const std::vector<mapping::TrajectoryNode>& trajectory_nodes,
+    const sensor::CompressedPointCloud* const compressed_point_cloud,
+    const std::vector<mapping::TrajectoryNode>& submap_nodes,
     const transform::Rigid3d& initial_relative_pose) {
   if (initial_relative_pose.translation().norm() >
       options_.max_constraint_distance()) {
     return;
   }
   if (sampler_.Pulse()) {
-    const auto submap_nodes = ComputeSubmapNodes(
-        trajectory_nodes, submap, flat_scan_index, initial_relative_pose);
     common::MutexLocker locker(&mutex_);
-    CHECK_LE(flat_scan_index, current_computation_);
     constraints_.emplace_back();
     auto* const constraint = &constraints_.back();
     ++pending_computations_[current_computation_];
     const int current_computation = current_computation_;
-    const auto* const point_cloud =
-        &trajectory_nodes[flat_scan_index].constant_data->range_data_3d.returns;
     ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
         submap_id, submap_nodes, &submap->high_resolution_hybrid_grid,
         [=]() EXCLUDES(mutex_) {
-          ComputeConstraint(submap_id, submap, node_id,
+          ComputeConstraint(
-                            false,   /* match_full_submap */
+              submap_id, submap, node_id, false, /* match_full_submap */
               nullptr,                           /* trajectory_connectivity */
-                            point_cloud, initial_relative_pose, constraint);
+              compressed_point_cloud, initial_relative_pose, constraint);
           FinishComputation(current_computation);
         });
   }
@@ -109,34 +86,28 @@ void ConstraintBuilder::MaybeAddConstraint(
 
 void ConstraintBuilder::MaybeAddGlobalConstraint(
     const mapping::SubmapId& submap_id, const Submap* const submap,
-    const mapping::NodeId& node_id, const int flat_scan_index,
+    const mapping::NodeId& node_id,
-    mapping::TrajectoryConnectivity* trajectory_connectivity,
+    const sensor::CompressedPointCloud* const compressed_point_cloud,
-    const std::vector<mapping::TrajectoryNode>& trajectory_nodes) {
+    const std::vector<mapping::TrajectoryNode>& submap_nodes,
-  const auto submap_nodes =
+    mapping::TrajectoryConnectivity* const trajectory_connectivity) {
-      ComputeSubmapNodes(trajectory_nodes, submap, flat_scan_index,
-                         transform::Rigid3d::Identity());
   common::MutexLocker locker(&mutex_);
-  CHECK_LE(flat_scan_index, current_computation_);
   constraints_.emplace_back();
   auto* const constraint = &constraints_.back();
   ++pending_computations_[current_computation_];
   const int current_computation = current_computation_;
-  const auto* const point_cloud =
-      &trajectory_nodes[flat_scan_index].constant_data->range_data_3d.returns;
   ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
       submap_id, submap_nodes, &submap->high_resolution_hybrid_grid,
       [=]() EXCLUDES(mutex_) {
         ComputeConstraint(submap_id, submap, node_id,
                           true, /* match_full_submap */
-                          trajectory_connectivity, point_cloud,
+                          trajectory_connectivity, compressed_point_cloud,
                           transform::Rigid3d::Identity(), constraint);
         FinishComputation(current_computation);
       });
 }
 
-void ConstraintBuilder::NotifyEndOfScan(const int flat_scan_index) {
+void ConstraintBuilder::NotifyEndOfScan() {
   common::MutexLocker locker(&mutex_);
-  CHECK_EQ(current_computation_, flat_scan_index);
   ++current_computation_;
 }
 

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h

@@ -68,35 +68,35 @@ class ConstraintBuilder {
   ConstraintBuilder& operator=(const ConstraintBuilder&) = delete;
 
   // Schedules exploring a new constraint between 'submap' identified by
-  // 'submap_id', and the 'range_data_3d.returns' in 'trajectory_nodes' for
+  // 'submap_id', and the 'point_cloud' for 'node_id'.
-  // 'flat_scan_index'. The 'initial_relative_pose' is relative to the 'submap'.
+  // The 'initial_relative_pose' is relative to the 'submap'.
   //
-  // The pointees of 'submap' and 'range_data_3d.returns' must stay valid until
+  // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
   // all computations are finished.
   void MaybeAddConstraint(
       const mapping::SubmapId& submap_id, const Submap* submap,
-      const mapping::NodeId& node_id, int flat_scan_index,
+      const mapping::NodeId& node_id,
-      const std::vector<mapping::TrajectoryNode>& trajectory_nodes,
+      const sensor::CompressedPointCloud* compressed_point_cloud,
+      const std::vector<mapping::TrajectoryNode>& submap_nodes,
       const transform::Rigid3d& initial_relative_pose);
 
   // Schedules exploring a new constraint between 'submap' identified by
-  // 'submap_id' and the 'range_data_3d.returns' in 'trajectory_nodes' for
+  // 'submap_id' and the 'compressed_point_cloud' for 'node_id'.
-  // 'flat_scan_index'. This performs full-submap matching.
+  // This performs full-submap matching.
   //
-  // The scan at 'flat_scan_index' should be from 'node_id.trajectory_id'.
   // The 'trajectory_connectivity' is updated if the full-submap match succeeds.
   //
-  // The pointees of 'submap' and 'range_data_3d.returns' must stay valid until
+  // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
   // all computations are finished.
   void MaybeAddGlobalConstraint(
       const mapping::SubmapId& submap_id, const Submap* submap,
-      const mapping::NodeId& node_id, int flat_scan_index,
+      const mapping::NodeId& node_id,
-      mapping::TrajectoryConnectivity* trajectory_connectivity,
+      const sensor::CompressedPointCloud* compressed_point_cloud,
-      const std::vector<mapping::TrajectoryNode>& trajectory_nodes);
+      const std::vector<mapping::TrajectoryNode>& submap_nodes,
+      mapping::TrajectoryConnectivity* trajectory_connectivity);
 
-  // Must be called after all computations related to 'flat_scan_index' are
+  // Must be called after all computations related to one node have been added.
-  // added.
+  void NotifyEndOfScan();
-  void NotifyEndOfScan(int flat_scan_index);
 
   // Registers the 'callback' to be called with the results, after all
   // computations triggered by MaybeAddConstraint() have finished.
@@ -133,14 +133,14 @@ class ConstraintBuilder {
   // Runs in a background thread and does computations for an additional
   // constraint, assuming 'submap' and 'point_cloud' do not change anymore.
   // If 'match_full_submap' is true, and global localization succeeds, will
-  // connect 'scan_trajectory_id' and 'submap_id.trajectory_id' in
+  // connect 'node_id.trajectory_id' and 'submap_id.trajectory_id' in
   // 'trajectory_connectivity'.
   // As output, it may create a new Constraint in 'constraint'.
   void ComputeConstraint(
       const mapping::SubmapId& submap_id, const Submap* submap,
       const mapping::NodeId& node_id, bool match_full_submap,
       mapping::TrajectoryConnectivity* trajectory_connectivity,
-      const sensor::CompressedPointCloud* const compressed_point_cloud,
+      const sensor::CompressedPointCloud* compressed_point_cloud,
       const transform::Rigid3d& initial_relative_pose,
       std::unique_ptr<Constraint>* constraint) EXCLUDES(mutex_);
 

cartographer/mapping_3d/submaps.cc

@@ -273,8 +273,7 @@ void Submaps::SubmapToProto(
                              global_submap_pose.translation().z())));
 }
 
-void Submaps::InsertRangeData(const sensor::RangeData& range_data,
+void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
-                              const int trajectory_node_index) {
   for (const int index : insertion_indices()) {
     Submap* submap = submaps_[index].get();
     range_data_inserter_.Insert(
@@ -284,7 +283,6 @@ void Submaps::InsertRangeData(const sensor::RangeData& range_data,
     range_data_inserter_.Insert(range_data,
                                 &submap->low_resolution_hybrid_grid);
     ++submap->num_range_data;
-    submap->trajectory_node_indices.push_back(trajectory_node_index);
   }
   const Submap* const last_submap = Get(size() - 1);
   if (last_submap->num_range_data == options_.num_range_data()) {

cartographer/mapping_3d/submaps.h

@@ -23,6 +23,7 @@
 
 #include "Eigen/Geometry"
 #include "cartographer/common/port.h"
+#include "cartographer/mapping/id.h"
 #include "cartographer/mapping/proto/submap_visualization.pb.h"
 #include "cartographer/mapping/submaps.h"
 #include "cartographer/mapping_2d/probability_grid.h"
@@ -52,8 +53,6 @@ struct Submap : public mapping::Submap {
   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;
 };
 
 // A container of Submaps.
@@ -71,8 +70,7 @@ 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;