Adds mapping:NodeId for Constraint. (#279)

Also extracted mapping::SubmapId into an id.h file.

Related to #256.

PAIR=SirVer

cartographer/mapping/id.h

@@ -0,0 +1,61 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_MAPPING_ID_H_
+#define CARTOGRAPHER_MAPPING_ID_H_
+
+#include <ostream>
+#include <tuple>
+
+namespace cartographer {
+namespace mapping {
+
+// Uniquely identifies a trajectory node using a combination of a unique
+// trajectory ID and a zero-based index of the node inside that trajectory.
+struct NodeId {
+  int trajectory_id;
+  int node_index;
+
+  bool operator<(const NodeId& other) const {
+    return std::forward_as_tuple(trajectory_id, node_index) <
+           std::forward_as_tuple(other.trajectory_id, other.node_index);
+  }
+};
+
+inline std::ostream& operator<<(std::ostream& os, const NodeId& v) {
+  return os << "(" << v.trajectory_id << ", " << v.node_index << ")";
+}
+
+// Uniquely identifies a submap using a combination of a unique trajectory ID
+// and a zero-based index of the submap inside that trajectory.
+struct SubmapId {
+  int trajectory_id;
+  int submap_index;
+
+  bool operator<(const SubmapId& other) const {
+    return std::forward_as_tuple(trajectory_id, submap_index) <
+           std::forward_as_tuple(other.trajectory_id, other.submap_index);
+  }
+};
+
+inline std::ostream& operator<<(std::ostream& os, const SubmapId& v) {
+  return os << "(" << v.trajectory_id << ", " << v.submap_index << ")";
+}
+
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_ID_H_

cartographer/mapping/sparse_pose_graph.cc

@@ -98,9 +98,9 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
         constraint.submap_id.submap_index);
 
     constraint_proto->mutable_scan_id()->set_trajectory_id(
-        grouped_node_indices[constraint.j].first);
+        constraint.node_id.trajectory_id);
     constraint_proto->mutable_scan_id()->set_scan_index(
-        grouped_node_indices[constraint.j].second);
+        constraint.node_id.node_index);
 
     constraint_proto->set_tag(mapping::ToProto(constraint.tag));
   }

cartographer/mapping/sparse_pose_graph.h

@@ -23,6 +23,7 @@
 #include <vector>
 
 #include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/mapping/id.h"
 #include "cartographer/mapping/proto/sparse_pose_graph.pb.h"
 #include "cartographer/mapping/proto/sparse_pose_graph_options.pb.h"
 #include "cartographer/mapping/submaps.h"
@@ -59,9 +60,7 @@ class SparsePoseGraph {
     };
 
     mapping::SubmapId submap_id;  // 'i' in the paper.
-
+    mapping::NodeId node_id;      // 'j' in the paper.
-    // Scan index.
-    int j;
 
     // Pose of the scan 'j' relative to submap 'i'.
     Pose pose;

cartographer/mapping/submaps.h

@@ -14,18 +14,6 @@
  * limitations under the License.
  */
 
-// Submaps is a sequence of maps to which scans are matched and into which scans
-// are inserted.
-//
-// Except during initialization when only a single submap exists, there are
-// always two submaps into which scans are inserted: an old submap that is used
-// for matching, and a new one, which will be used for matching next, that is
-// being initialized.
-//
-// Once a certain number of scans have been inserted, the new submap is
-// considered initialized: the old submap is no longer changed, the "new" submap
-// is now the "old" submap and is used for scan-to-map matching. Moreover,
-// a "new" submap gets inserted.
 #ifndef CARTOGRAPHER_MAPPING_SUBMAPS_H_
 #define CARTOGRAPHER_MAPPING_SUBMAPS_H_
 
@@ -35,6 +23,7 @@
 #include "Eigen/Geometry"
 #include "cartographer/common/math.h"
 #include "cartographer/common/port.h"
+#include "cartographer/mapping/id.h"
 #include "cartographer/mapping/probability_values.h"
 #include "cartographer/mapping/proto/submap_visualization.pb.h"
 #include "cartographer/mapping/trajectory_node.h"
@@ -63,22 +52,6 @@ inline uint8 ProbabilityToLogOddsInteger(const float probability) {
   return value;
 }
 
-// Uniquely identifies a submap using a combination of a unique trajectory ID
-// and a zero-based index of the submap inside that trajectory.
-struct SubmapId {
-  int trajectory_id;
-  int submap_index;
-
-  bool operator<(const SubmapId& other) const {
-    return std::forward_as_tuple(trajectory_id, submap_index) <
-           std::forward_as_tuple(other.trajectory_id, other.submap_index);
-  }
-};
-
-inline std::ostream& operator<<(std::ostream& os, const SubmapId& v) {
-  return os << "(" << v.trajectory_id << ", " << v.submap_index << ")";
-}
-
 // An individual submap, which has an initial position 'origin', keeps track of
 // how many range data were inserted into it, and sets the
 // 'finished_probability_grid' to be used for loop closing once the map no
@@ -102,7 +75,18 @@ struct Submap {
   const mapping_2d::ProbabilityGrid* finished_probability_grid = nullptr;
 };
 
-// A container of Submaps.
+// Submaps is a sequence of maps to which scans are matched and into which scans
+// are inserted.
+//
+// Except during initialization when only a single submap exists, there are
+// always two submaps into which scans are inserted: an old submap that is used
+// for matching, and a new one, which will be used for matching next, that is
+// being initialized.
+//
+// Once a certain number of scans have been inserted, the new submap is
+// considered initialized: the old submap is no longer changed, the "new" submap
+// is now the "old" submap and is used for scan-to-map matching. Moreover,
+// a "new" submap gets inserted.
 class Submaps {
  public:
   static constexpr uint8 kUnknownLogOdds = 0;

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -100,8 +100,11 @@ void SparsePoseGraph::AddScan(
   common::MutexLocker locker(&mutex_);
   trajectory_ids_.emplace(trajectory, trajectory_ids_.size());
   const int trajectory_id = trajectory_ids_.at(trajectory);
-  const int j = trajectory_nodes_.size();
+  const int flat_scan_index = trajectory_nodes_.size();
-  CHECK_LT(j, std::numeric_limits<int>::max());
+  CHECK_LT(flat_scan_index, std::numeric_limits<int>::max());
+  scan_index_to_node_id_.push_back(
+      mapping::NodeId{trajectory_id, num_nodes_in_trajectory_[trajectory_id]});
+  ++num_nodes_in_trajectory_[trajectory_id];
 
   constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
       time, range_data_in_pose,
@@ -135,7 +138,7 @@ void SparsePoseGraph::AddScan(
   }
 
   AddWorkItem([=]() REQUIRES(mutex_) {
-    ComputeConstraintsForScan(j, matching_submap, insertion_submaps,
+    ComputeConstraintsForScan(flat_scan_index, matching_submap, insertion_submaps,
                               finished_submap, pose, covariance);
   });
 }
@@ -180,8 +183,9 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
   if (scan_trajectory_id != submap_trajectory_id &&
       global_localization_samplers_[scan_trajectory_id]->Pulse()) {
     constraint_builder_.MaybeAddGlobalConstraint(
-        submap_id, submap_states_[submap_index].submap, scan_index,
+        submap_id, submap_states_[submap_index].submap,
-        scan_trajectory_id, &trajectory_connectivity_,
+        scan_index_to_node_id_.at(scan_index), scan_index,
+        &trajectory_connectivity_,
         &trajectory_nodes_[scan_index].constant_data->range_data_2d.returns);
   } else {
     const bool scan_and_submap_trajectories_connected =
@@ -192,7 +196,8 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
     if (scan_trajectory_id == submap_trajectory_id ||
         scan_and_submap_trajectories_connected) {
       constraint_builder_.MaybeAddConstraint(
-          submap_id, submap_states_[submap_index].submap, scan_index,
+          submap_id, submap_states_[submap_index].submap,
+          scan_index_to_node_id_.at(scan_index), scan_index,
           &trajectory_nodes_[scan_index].constant_data->range_data_2d.returns,
           relative_pose);
     }
@@ -243,7 +248,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
     constexpr double kFakeOrientationCovariance = 1e-6;
     constraints_.push_back(Constraint{
         submap_states_[submap_index].id,
-        scan_index,
+        scan_index_to_node_id_.at(scan_index),
         {transform::Embed3D(constraint_transform),
          common::ComputeSpdMatrixSqrtInverse(
              kalman_filter::Embed3D(covariance, kFakePositionCovariance,

cartographer/mapping_2d/sparse_pose_graph.h

@@ -14,16 +14,6 @@
  * limitations under the License.
  */
 
-// Implements the loop closure method called Sparse Pose Adjustment (SPA) from
-// Konolige, Kurt, et al. "Efficient sparse pose adjustment for 2d mapping."
-// Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference
-// on (pp. 22--29). IEEE, 2010.
-//
-// It is extended for submapping:
-// Each scan has been matched against one or more submaps (adding a constraint
-// for each match), both poses of scans and of submaps are to be optimized.
-// All constraints are between a submap i and a scan j.
-
 #ifndef CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_H_
 #define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_H_
 
@@ -53,7 +43,15 @@
 namespace cartographer {
 namespace mapping_2d {
 
-// Implements the SPA loop closure method.
+// Implements the loop closure method called Sparse Pose Adjustment (SPA) from
+// Konolige, Kurt, et al. "Efficient sparse pose adjustment for 2d mapping."
+// Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference
+// on (pp. 22--29). IEEE, 2010.
+//
+// It is extended for submapping:
+// Each scan has been matched against one or more submaps (adding a constraint
+// for each match), both poses of scans and of submaps are to be optimized.
+// All constraints are between a submap i and a scan j.
 class SparsePoseGraph : public mapping::SparsePoseGraph {
  public:
   SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options,
@@ -199,6 +197,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   std::vector<SubmapState> submap_states_ GUARDED_BY(mutex_);
   std::map<int, int> num_submaps_in_trajectory_ GUARDED_BY(mutex_);
 
+  // Mapping to flat indices to aid the transition to per-trajectory data
+  // structures.
+  std::map<int, int> num_nodes_in_trajectory_ GUARDED_BY(mutex_);
+  std::vector<mapping::NodeId> scan_index_to_node_id_ GUARDED_BY(mutex_);
+
   // Connectivity structure of our trajectories by IDs.
   std::vector<std::vector<int>> connected_components_;
   // Trajectory ID to connected component ID.

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc

@@ -62,7 +62,8 @@ ConstraintBuilder::~ConstraintBuilder() {
 
 void ConstraintBuilder::MaybeAddConstraint(
     const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
-    const int scan_index, const sensor::PointCloud* const point_cloud,
+    const mapping::NodeId& node_id, const int flat_scan_index,
+    const sensor::PointCloud* const point_cloud,
     const transform::Rigid2d& initial_relative_pose) {
   if (initial_relative_pose.translation().norm() >
       options_.max_constraint_distance()) {
@@ -70,15 +71,14 @@ void ConstraintBuilder::MaybeAddConstraint(
   }
   if (sampler_.Pulse()) {
     common::MutexLocker locker(&mutex_);
-    CHECK_LE(scan_index, current_computation_);
+    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_;
     ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
         submap_id, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) {
-          ComputeConstraint(submap_id, submap, scan_index,
+          ComputeConstraint(submap_id, submap, node_id,
-                            -1,      /* scan_trajectory_id */
                             false,   /* match_full_submap */
                             nullptr, /* trajectory_connectivity */
                             point_cloud, initial_relative_pose, constraint);
@@ -89,18 +89,18 @@ void ConstraintBuilder::MaybeAddConstraint(
 
 void ConstraintBuilder::MaybeAddGlobalConstraint(
     const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
-    const int scan_index, const int scan_trajectory_id,
+    const mapping::NodeId& node_id, const int flat_scan_index,
     mapping::TrajectoryConnectivity* trajectory_connectivity,
     const sensor::PointCloud* const point_cloud) {
   common::MutexLocker locker(&mutex_);
-  CHECK_LE(scan_index, current_computation_);
+  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_;
   ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
       submap_id, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) {
-        ComputeConstraint(submap_id, submap, scan_index, scan_trajectory_id,
+        ComputeConstraint(submap_id, submap, node_id,
                           true, /* match_full_submap */
                           trajectory_connectivity, point_cloud,
                           transform::Rigid2d::Identity(), constraint);
@@ -108,9 +108,9 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
       });
 }
 
-void ConstraintBuilder::NotifyEndOfScan(const int scan_index) {
+void ConstraintBuilder::NotifyEndOfScan(const int flat_scan_index) {
   common::MutexLocker locker(&mutex_);
-  CHECK_EQ(current_computation_, scan_index);
+  CHECK_EQ(current_computation_, flat_scan_index);
   ++current_computation_;
 }
 
@@ -166,7 +166,7 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
 
 void ConstraintBuilder::ComputeConstraint(
     const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
-    const int scan_index, const int scan_trajectory_id, bool match_full_submap,
+    const mapping::NodeId& node_id, bool match_full_submap,
     mapping::TrajectoryConnectivity* trajectory_connectivity,
     const sensor::PointCloud* const point_cloud,
     const transform::Rigid2d& initial_relative_pose,
@@ -191,9 +191,9 @@ void ConstraintBuilder::ComputeConstraint(
             filtered_point_cloud, options_.global_localization_min_score(),
             &score, &pose_estimate)) {
       CHECK_GT(score, options_.global_localization_min_score());
-      CHECK_GE(scan_trajectory_id, 0);
+      CHECK_GE(node_id.trajectory_id, 0);
       CHECK_GE(submap_id.trajectory_id, 0);
-      trajectory_connectivity->Connect(scan_trajectory_id,
+      trajectory_connectivity->Connect(node_id.trajectory_id,
                                        submap_id.trajectory_id);
     } else {
       return;
@@ -229,7 +229,7 @@ void ConstraintBuilder::ComputeConstraint(
   constexpr double kFakeOrientationCovariance = 1e-6;
   constraint->reset(new Constraint{
       submap_id,
-      scan_index,
+      node_id,
       {transform::Embed3D(constraint_transform),
        common::ComputeSpdMatrixSqrtInverse(
            kalman_filter::Embed3D(covariance, kFakePositionCovariance,
@@ -241,9 +241,9 @@ void ConstraintBuilder::ComputeConstraint(
     const transform::Rigid2d difference =
         initial_pose.inverse() * pose_estimate;
     std::ostringstream info;
-    info << "Scan index " << scan_index << " with "
+    info << "Node " << node_id << " with " << filtered_point_cloud.size()
-         << filtered_point_cloud.size() << " points on submap " << submap_id
+         << " points on submap " << submap_id << " differs by translation "
-         << " differs by translation " << std::fixed << std::setprecision(2)
+         << std::fixed << std::setprecision(2)
          << difference.translation().norm() << " rotation "
          << std::setprecision(3) << std::abs(difference.normalized_angle())
          << " with score " << std::setprecision(1) << 100. * score

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h

@@ -72,33 +72,37 @@ class ConstraintBuilder {
   ConstraintBuilder& operator=(const ConstraintBuilder&) = delete;
 
   // Schedules exploring a new constraint between 'submap' identified by
-  // 'submap_id', and the 'point_cloud' for 'scan_index'.
+  // 'submap_id', and the 'point_cloud' for 'flat_scan_index'.
   // The 'initial_pose' is 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, int scan_index,
+                          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 'scan_index'. This performs
+  // 'submap_id' and the 'point_cloud' for 'flat_scan_index'. This performs
   // full-submap matching.
   //
-  // The scan at 'scan_index' should be from trajectory 'scan_trajectory_id'.
+  // 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,
-      int scan_index, int scan_trajectory_id,
+      const mapping::NodeId& node_id, const int flat_scan_index,
       mapping::TrajectoryConnectivity* trajectory_connectivity,
       const sensor::PointCloud* point_cloud);
 
-  // Must be called after all computations related to 'scan_index' are added.
+  // Must be called after all computations related to 'flat_scan_index' are
-  void NotifyEndOfScan(const int scan_index);
+  // added.
+  void NotifyEndOfScan(const int flat_scan_index);
 
   // Registers the 'callback' to be called with the results, after all
   // computations triggered by MaybeAddConstraint() have finished.
@@ -132,12 +136,12 @@ 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 mapping::Submap* submap,
-      int scan_index, int scan_trajectory_id, bool match_full_submap,
+      const mapping::NodeId& node_id, bool match_full_submap,
       mapping::TrajectoryConnectivity* trajectory_connectivity,
       const sensor::PointCloud* point_cloud,
       const transform::Rigid2d& initial_relative_pose,

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc

@@ -98,13 +98,20 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
     return;
   }
 
+  // Compute the indices of consecutive nodes for each trajectory.
+  std::vector<std::vector<size_t>> nodes_per_trajectory(submap_data_.size());
+  for (size_t j = 0; j != node_data_.size(); ++j) {
+    nodes_per_trajectory.at(node_data_[j].trajectory_id).push_back(j);
+  }
+
   ceres::Problem::Options problem_options;
   ceres::Problem problem(problem_options);
 
   // Set the starting point.
   // TODO(hrapp): Move ceres data into SubmapData.
   std::vector<std::deque<std::array<double, 3>>> C_submaps(submap_data_.size());
-  std::vector<std::array<double, 3>> C_point_clouds(node_data_.size());
+  std::vector<std::deque<std::array<double, 3>>> C_nodes(
+      nodes_per_trajectory.size());
   for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
        ++trajectory_id) {
     for (size_t submap_index = 0;
@@ -123,15 +130,21 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
       }
     }
   }
-  for (size_t j = 0; j != node_data_.size(); ++j) {
+  for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size();
-    C_point_clouds[j] = FromPose(node_data_[j].point_cloud_pose);
+       ++trajectory_id) {
-    problem.AddParameterBlock(C_point_clouds[j].data(), 3);
+    C_nodes[trajectory_id].resize(nodes_per_trajectory[trajectory_id].size());
+    for (size_t node_index = 0;
+         node_index != nodes_per_trajectory[trajectory_id].size();
+         ++node_index) {
+      C_nodes[trajectory_id][node_index] =
+          FromPose(node_data_[nodes_per_trajectory[trajectory_id][node_index]]
+                       .point_cloud_pose);
+      problem.AddParameterBlock(C_nodes[trajectory_id][node_index].data(), 3);
+    }
   }
 
   // Add cost functions for intra- and inter-submap constraints.
   for (const Constraint& constraint : constraints) {
-    CHECK_GE(constraint.j, 0);
-    CHECK_LT(constraint.j, node_data_.size());
     problem.AddResidualBlock(
         new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
             new SpaCostFunction(constraint.pose)),
@@ -142,7 +155,9 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
         C_submaps.at(constraint.submap_id.trajectory_id)
             .at(constraint.submap_id.submap_index)
             .data(),
-        C_point_clouds[constraint.j].data());
+        C_nodes.at(constraint.node_id.trajectory_id)
+            .at(constraint.node_id.node_index)
+            .data());
   }
 
   // Add penalties for changes between consecutive scans.
@@ -151,32 +166,32 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
       options_.consecutive_scan_translation_penalty_factor(),
       options_.consecutive_scan_rotation_penalty_factor());
 
-  // The poses in 'node_data_' are interleaved from multiple trajectories
+  for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size();
-  // (although the points from a given trajectory are in time order).
+       ++trajectory_id) {
-  // 'last_pose_indices[trajectory_id]' is the index of the most-recent pose on
+    if (nodes_per_trajectory[trajectory_id].empty()) {
-  // 'trajectory_id'.
+      continue;
-  std::map<int, int> last_pose_indices;
+    }
-
+    for (size_t node_index = 1;
-  for (size_t j = 0; j != node_data_.size(); ++j) {
+         node_index != nodes_per_trajectory[trajectory_id].size();
-    const int trajectory_id = node_data_[j].trajectory_id;
+         ++node_index) {
-    // This pose has a predecessor.
-    if (last_pose_indices.count(trajectory_id) != 0) {
-      const int last_pose_index = last_pose_indices[trajectory_id];
       constexpr double kUnusedPositionPenalty = 1.;
       constexpr double kUnusedOrientationPenalty = 1.;
       problem.AddResidualBlock(
-          new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
+          new ceres::AutoDiffCostFunction<
-              new SpaCostFunction(Constraint::Pose{
+              SpaCostFunction, 3, 3, 3>(new SpaCostFunction(Constraint::Pose{
-                  transform::Embed3D(node_data_[last_pose_index]
+              transform::Embed3D(
-                                         .initial_point_cloud_pose.inverse() *
+                  node_data_[nodes_per_trajectory[trajectory_id]
-                                     node_data_[j].initial_point_cloud_pose),
+                                                 [node_index - 1]]
-                  kalman_filter::Embed3D(consecutive_pose_change_penalty_matrix,
+                      .initial_point_cloud_pose.inverse() *
-                                         kUnusedPositionPenalty,
+                  node_data_[nodes_per_trajectory[trajectory_id][node_index]]
-                                         kUnusedOrientationPenalty)})),
+                      .initial_point_cloud_pose),
-          nullptr /* loss function */, C_point_clouds[last_pose_index].data(),
+              kalman_filter::Embed3D(consecutive_pose_change_penalty_matrix,
-          C_point_clouds[j].data());
+                                     kUnusedPositionPenalty,
+                                     kUnusedOrientationPenalty)})),
+          nullptr /* loss function */,
+          C_nodes[trajectory_id][node_index - 1].data(),
+          C_nodes[trajectory_id][node_index].data());
     }
-    last_pose_indices[trajectory_id] = j;
   }
 
   // Solve.
@@ -198,9 +213,14 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
           ToPose(C_submaps[trajectory_id][submap_index]);
     }
   }
-
+  for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size();
-  for (size_t j = 0; j != node_data_.size(); ++j) {
+       ++trajectory_id) {
-    node_data_[j].point_cloud_pose = ToPose(C_point_clouds[j]);
+    for (size_t node_index = 0;
+         node_index != nodes_per_trajectory[trajectory_id].size();
+         ++node_index) {
+      node_data_[nodes_per_trajectory[trajectory_id][node_index]]
+          .point_cloud_pose = ToPose(C_nodes[trajectory_id][node_index]);
+    }
   }
 }
 

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -98,8 +98,11 @@ void SparsePoseGraph::AddScan(
   common::MutexLocker locker(&mutex_);
   trajectory_ids_.emplace(trajectory, trajectory_ids_.size());
   const int trajectory_id = trajectory_ids_.at(trajectory);
-  const int j = trajectory_nodes_.size();
+  const int flat_scan_index = trajectory_nodes_.size();
-  CHECK_LT(j, std::numeric_limits<int>::max());
+  CHECK_LT(flat_scan_index, std::numeric_limits<int>::max());
+  scan_index_to_node_id_.push_back(
+      mapping::NodeId{trajectory_id, num_nodes_in_trajectory_[trajectory_id]});
+  ++num_nodes_in_trajectory_[trajectory_id];
 
   constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
       time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}},
@@ -130,7 +133,7 @@ void SparsePoseGraph::AddScan(
   }
 
   AddWorkItem([=]() REQUIRES(mutex_) {
-    ComputeConstraintsForScan(j, matching_submap, insertion_submaps,
+    ComputeConstraintsForScan(flat_scan_index, matching_submap, insertion_submaps,
                               finished_submap, pose, covariance);
   });
 }
@@ -180,8 +183,9 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
   if (scan_trajectory_id != submap_trajectory_id &&
       global_localization_samplers_[scan_trajectory_id]->Pulse()) {
     constraint_builder_.MaybeAddGlobalConstraint(
-        submap_id, submap_states_[submap_index].submap, scan_index,
+        submap_id, submap_states_[submap_index].submap,
-        scan_trajectory_id, &trajectory_connectivity_, trajectory_nodes_);
+        scan_index_to_node_id_.at(scan_index), scan_index,
+        &trajectory_connectivity_, trajectory_nodes_);
   } else {
     const bool scan_and_submap_trajectories_connected =
         reverse_connected_components_.count(scan_trajectory_id) > 0 &&
@@ -191,8 +195,9 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
     if (scan_trajectory_id == submap_trajectory_id ||
         scan_and_submap_trajectories_connected) {
       constraint_builder_.MaybeAddConstraint(
-          submap_id, submap_states_[submap_index].submap, scan_index,
+          submap_id, submap_states_[submap_index].submap,
-          trajectory_nodes_, relative_pose);
+          scan_index_to_node_id_.at(scan_index), scan_index, trajectory_nodes_,
+          relative_pose);
     }
   }
 }
@@ -238,7 +243,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
         submap->local_pose().inverse() * pose;
     constraints_.push_back(
         Constraint{submap_states_[submap_index].id,
-                   scan_index,
+                   scan_index_to_node_id_.at(scan_index),
                    {constraint_transform,
                     common::ComputeSpdMatrixSqrtInverse(
                         covariance, options_.constraint_builder_options()

cartographer/mapping_3d/sparse_pose_graph.h

@@ -14,16 +14,6 @@
  * limitations under the License.
  */
 
-// Implements the loop closure method called Sparse Pose Adjustment (SPA) from
-// Konolige, Kurt, et al. "Efficient sparse pose adjustment for 2d mapping."
-// Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference
-// on (pp. 22--29). IEEE, 2010.
-//
-// It is extended for submapping in 3D:
-// Each scan has been matched against one or more submaps (adding a constraint
-// for each match), both poses of scans and of submaps are to be optimized.
-// All constraints are between a submap i and a scan j.
-
 #ifndef CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_H_
 #define CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_H_
 
@@ -53,7 +43,15 @@
 namespace cartographer {
 namespace mapping_3d {
 
-// Implements the SPA loop closure method.
+// Implements the loop closure method called Sparse Pose Adjustment (SPA) from
+// Konolige, Kurt, et al. "Efficient sparse pose adjustment for 2d mapping."
+// Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference
+// on (pp. 22--29). IEEE, 2010.
+//
+// It is extended for submapping in 3D:
+// Each scan has been matched against one or more submaps (adding a constraint
+// for each match), both poses of scans and of submaps are to be optimized.
+// All constraints are between a submap i and a scan j.
 class SparsePoseGraph : public mapping::SparsePoseGraph {
  public:
   SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options,
@@ -199,6 +197,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   std::vector<SubmapState> submap_states_ GUARDED_BY(mutex_);
   std::map<int, int> num_submaps_in_trajectory_ GUARDED_BY(mutex_);
 
+  // Mapping to flat indices to aid the transition to per-trajectory data
+  // structures.
+  std::map<int, int> num_nodes_in_trajectory_ GUARDED_BY(mutex_);
+  std::vector<mapping::NodeId> scan_index_to_node_id_ GUARDED_BY(mutex_);
+
   // Connectivity structure of our trajectories by IDs.
   std::vector<std::vector<int>> connected_components_;
   // Trajectory ID to connected component ID.

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc

@@ -43,14 +43,14 @@ namespace {
 
 std::vector<mapping::TrajectoryNode> ComputeSubmapNodes(
     const std::vector<mapping::TrajectoryNode>& trajectory_nodes,
-    const Submap* const submap, int scan_index,
+    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[scan_index].pose.inverse() *
+                           trajectory_nodes[flat_scan_index].pose.inverse() *
                            trajectory_nodes[node_index].pose)});
   }
   return submap_nodes;
@@ -77,7 +77,7 @@ ConstraintBuilder::~ConstraintBuilder() {
 
 void ConstraintBuilder::MaybeAddConstraint(
     const mapping::SubmapId& submap_id, const Submap* const submap,
-    const int scan_index,
+    const mapping::NodeId& node_id, const int flat_scan_index,
     const std::vector<mapping::TrajectoryNode>& trajectory_nodes,
     const transform::Rigid3d& initial_relative_pose) {
   if (initial_relative_pose.translation().norm() >
@@ -86,20 +86,19 @@ void ConstraintBuilder::MaybeAddConstraint(
   }
   if (sampler_.Pulse()) {
     const auto submap_nodes = ComputeSubmapNodes(
-        trajectory_nodes, submap, scan_index, initial_relative_pose);
+        trajectory_nodes, submap, flat_scan_index, initial_relative_pose);
     common::MutexLocker locker(&mutex_);
-    CHECK_LE(scan_index, current_computation_);
+    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[scan_index].constant_data->range_data_3d.returns;
+        &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, scan_index,
+          ComputeConstraint(submap_id, submap, node_id,
-                            -1,      /* scan_trajectory_id */
                             false,   /* match_full_submap */
                             nullptr, /* trajectory_connectivity */
                             point_cloud, initial_relative_pose, constraint);
@@ -110,23 +109,24 @@ void ConstraintBuilder::MaybeAddConstraint(
 
 void ConstraintBuilder::MaybeAddGlobalConstraint(
     const mapping::SubmapId& submap_id, const Submap* const submap,
-    const int scan_index, const int scan_trajectory_id,
+    const mapping::NodeId& node_id, const int flat_scan_index,
     mapping::TrajectoryConnectivity* trajectory_connectivity,
     const std::vector<mapping::TrajectoryNode>& trajectory_nodes) {
-  const auto submap_nodes = ComputeSubmapNodes(
+  const auto submap_nodes =
-      trajectory_nodes, submap, scan_index, transform::Rigid3d::Identity());
+      ComputeSubmapNodes(trajectory_nodes, submap, flat_scan_index,
+                         transform::Rigid3d::Identity());
   common::MutexLocker locker(&mutex_);
-  CHECK_LE(scan_index, current_computation_);
+  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[scan_index].constant_data->range_data_3d.returns;
+      &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, scan_index, scan_trajectory_id,
+        ComputeConstraint(submap_id, submap, node_id,
                           true, /* match_full_submap */
                           trajectory_connectivity, point_cloud,
                           transform::Rigid3d::Identity(), constraint);
@@ -134,9 +134,9 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
       });
 }
 
-void ConstraintBuilder::NotifyEndOfScan(const int scan_index) {
+void ConstraintBuilder::NotifyEndOfScan(const int flat_scan_index) {
   common::MutexLocker locker(&mutex_);
-  CHECK_EQ(current_computation_, scan_index);
+  CHECK_EQ(current_computation_, flat_scan_index);
   ++current_computation_;
 }
 
@@ -197,7 +197,7 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
 
 void ConstraintBuilder::ComputeConstraint(
     const mapping::SubmapId& submap_id, const Submap* const submap,
-    const int scan_index, const int scan_trajectory_id, bool match_full_submap,
+    const mapping::NodeId& node_id, bool match_full_submap,
     mapping::TrajectoryConnectivity* trajectory_connectivity,
     const sensor::CompressedPointCloud* const compressed_point_cloud,
     const transform::Rigid3d& initial_relative_pose,
@@ -223,9 +223,9 @@ void ConstraintBuilder::ComputeConstraint(
             initial_pose.rotation(), filtered_point_cloud, point_cloud,
             options_.global_localization_min_score(), &score, &pose_estimate)) {
       CHECK_GT(score, options_.global_localization_min_score());
-      CHECK_GE(scan_trajectory_id, 0);
+      CHECK_GE(node_id.trajectory_id, 0);
       CHECK_GE(submap_id.trajectory_id, 0);
-      trajectory_connectivity->Connect(scan_trajectory_id,
+      trajectory_connectivity->Connect(node_id.trajectory_id,
                                        submap_id.trajectory_id);
     } else {
       return;
@@ -258,7 +258,7 @@ void ConstraintBuilder::ComputeConstraint(
       submap->local_pose().inverse() * pose_estimate;
   constraint->reset(new OptimizationProblem::Constraint{
       submap_id,
-      scan_index,
+      node_id,
       {constraint_transform,
        1. / std::sqrt(options_.lower_covariance_eigenvalue_bound()) *
            kalman_filter::PoseCovariance::Identity()},
@@ -268,9 +268,9 @@ void ConstraintBuilder::ComputeConstraint(
     const transform::Rigid3d difference =
         initial_pose.inverse() * pose_estimate;
     std::ostringstream info;
-    info << "Scan index " << scan_index << " with "
+    info << "Node " << node_id << " with " << filtered_point_cloud.size()
-         << filtered_point_cloud.size() << " points on submap " << submap_id
+         << " points on submap " << submap_id << " differs by translation "
-         << " differs by translation " << std::fixed << std::setprecision(2)
+         << std::fixed << std::setprecision(2)
          << difference.translation().norm() << " rotation "
          << std::setprecision(3) << transform::GetAngle(difference)
          << " with score " << std::setprecision(1) << 100. * score << "%.";

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h

@@ -69,32 +69,34 @@ class ConstraintBuilder {
 
   // Schedules exploring a new constraint between 'submap' identified by
   // 'submap_id', and the 'range_data_3d.returns' in 'trajectory_nodes' for
-  // 'scan_index'. The 'initial_relative_pose' is relative to the 'submap'.
+  // 'flat_scan_index'. The 'initial_relative_pose' is relative to the 'submap'.
   //
   // The pointees of 'submap' and 'range_data_3d.returns' must stay valid until
   // all computations are finished.
   void MaybeAddConstraint(
-      const mapping::SubmapId& submap_id, const Submap* submap, int scan_index,
+      const mapping::SubmapId& submap_id, const Submap* submap,
+      const mapping::NodeId& node_id, int flat_scan_index,
       const std::vector<mapping::TrajectoryNode>& trajectory_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
-  // 'scan_index'. This performs full-submap matching.
+  // 'flat_scan_index'. This performs full-submap matching.
   //
-  // The scan at 'scan_index' should be from trajectory 'scan_trajectory_id'.
+  // 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
   // all computations are finished.
   void MaybeAddGlobalConstraint(
-      const mapping::SubmapId& submap_id, const Submap* submap, int scan_index,
+      const mapping::SubmapId& submap_id, const Submap* submap,
-      int scan_trajectory_id,
+      const mapping::NodeId& node_id, int flat_scan_index,
       mapping::TrajectoryConnectivity* trajectory_connectivity,
       const std::vector<mapping::TrajectoryNode>& trajectory_nodes);
 
-  // Must be called after all computations related to 'scan_index' are added.
+  // Must be called after all computations related to 'flat_scan_index' are
-  void NotifyEndOfScan(int scan_index);
+  // added.
+  void NotifyEndOfScan(int flat_scan_index);
 
   // Registers the 'callback' to be called with the results, after all
   // computations triggered by MaybeAddConstraint() have finished.
@@ -135,8 +137,8 @@ class ConstraintBuilder {
   // 'trajectory_connectivity'.
   // As output, it may create a new Constraint in 'constraint'.
   void ComputeConstraint(
-      const mapping::SubmapId& submap_id, const Submap* submap, int scan_index,
+      const mapping::SubmapId& submap_id, const Submap* submap,
-      int scan_trajectory_id, bool match_full_submap,
+      const mapping::NodeId& node_id, bool match_full_submap,
       mapping::TrajectoryConnectivity* trajectory_connectivity,
       const sensor::CompressedPointCloud* const compressed_point_cloud,
       const transform::Rigid3d& initial_relative_pose,

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc

@@ -136,7 +136,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
   CHECK(!submap_data_[0].empty());
   // TODO(hrapp): Move ceres data into SubmapData.
   std::vector<std::deque<CeresPose>> C_submaps(submap_data_.size());
-  std::deque<CeresPose> C_point_clouds;
+  std::vector<std::deque<CeresPose>> C_nodes(nodes_per_trajectory.size());
   for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
        ++trajectory_id) {
     for (size_t submap_index = 0;
@@ -158,16 +158,21 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
       }
     }
   }
-  for (size_t j = 0; j != node_data_.size(); ++j) {
+  for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size();
-    C_point_clouds.emplace_back(
+       ++trajectory_id) {
-        node_data_[j].point_cloud_pose, translation_parameterization(),
+    for (size_t node_index = 0;
-        common::make_unique<ceres::QuaternionParameterization>(), &problem);
+         node_index != nodes_per_trajectory[trajectory_id].size();
+         ++node_index) {
+      C_nodes[trajectory_id].emplace_back(
+          node_data_[nodes_per_trajectory[trajectory_id][node_index]]
+              .point_cloud_pose,
+          translation_parameterization(),
+          common::make_unique<ceres::QuaternionParameterization>(), &problem);
+    }
   }
 
   // Add cost functions for intra- and inter-submap constraints.
   for (const Constraint& constraint : constraints) {
-    CHECK_GE(constraint.j, 0);
-    CHECK_LT(constraint.j, node_data_.size());
     problem.AddResidualBlock(
         new ceres::AutoDiffCostFunction<SpaCostFunction, 6, 4, 3, 4, 3>(
             new SpaCostFunction(constraint.pose)),
@@ -181,36 +186,44 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
         C_submaps.at(constraint.submap_id.trajectory_id)
             .at(constraint.submap_id.submap_index)
             .translation(),
-        C_point_clouds[constraint.j].rotation(),
+        C_nodes.at(constraint.node_id.trajectory_id)
-        C_point_clouds[constraint.j].translation());
+            .at(constraint.node_id.node_index)
+            .rotation(),
+        C_nodes.at(constraint.node_id.trajectory_id)
+            .at(constraint.node_id.node_index)
+            .translation());
   }
 
   // Add constraints based on IMU observations of angular velocities and
   // linear acceleration.
   for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size();
        ++trajectory_id) {
-    const std::vector<size_t>& node_indices =
+    const std::vector<size_t>& flat_indices =
         nodes_per_trajectory[trajectory_id];
     const std::deque<ImuData>& imu_data = imu_data_.at(trajectory_id);
-    CHECK(!node_indices.empty());
+    CHECK(!flat_indices.empty());
     CHECK(!imu_data.empty());
 
     // Skip IMU data before the first node of this trajectory.
     auto it = imu_data.cbegin();
     while ((it + 1) != imu_data.cend() &&
-           (it + 1)->time <= node_data_[node_indices[0]].time) {
+           (it + 1)->time <= node_data_[flat_indices[0]].time) {
       ++it;
     }
 
-    for (size_t j = 1; j < node_indices.size(); ++j) {
+    for (size_t node_index = 1; node_index < flat_indices.size();
+         ++node_index) {
       auto it2 = it;
       const IntegrateImuResult<double> result =
-          IntegrateImu(imu_data, node_data_[node_indices[j - 1]].time,
+          IntegrateImu(imu_data, node_data_[flat_indices[node_index - 1]].time,
-                       node_data_[node_indices[j]].time, &it);
+                       node_data_[flat_indices[node_index]].time, &it);
-      if (j + 1 < node_indices.size()) {
+      if (node_index + 1 < flat_indices.size()) {
-        const common::Time first_time = node_data_[node_indices[j - 1]].time;
+        const common::Time first_time =
-        const common::Time second_time = node_data_[node_indices[j]].time;
+            node_data_[flat_indices[node_index - 1]].time;
-        const common::Time third_time = node_data_[node_indices[j + 1]].time;
+        const common::Time second_time =
+            node_data_[flat_indices[node_index]].time;
+        const common::Time third_time =
+            node_data_[flat_indices[node_index + 1]].time;
         const common::Duration first_duration = second_time - first_time;
         const common::Duration second_duration = third_time - second_time;
         const common::Time first_center = first_time + first_duration / 2;
@@ -235,18 +248,18 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
                     options_.acceleration_weight(), delta_velocity,
                     common::ToSeconds(first_duration),
                     common::ToSeconds(second_duration))),
-            nullptr, C_point_clouds[node_indices[j]].rotation(),
+            nullptr, C_nodes[trajectory_id].at(node_index).rotation(),
-            C_point_clouds[node_indices[j - 1]].translation(),
+            C_nodes[trajectory_id].at(node_index - 1).translation(),
-            C_point_clouds[node_indices[j]].translation(),
+            C_nodes[trajectory_id].at(node_index).translation(),
-            C_point_clouds[node_indices[j + 1]].translation(),
+            C_nodes[trajectory_id].at(node_index + 1).translation(),
             &gravity_constant_);
       }
       problem.AddResidualBlock(
           new ceres::AutoDiffCostFunction<RotationCostFunction, 3, 4, 4>(
               new RotationCostFunction(options_.rotation_weight(),
                                        result.delta_rotation)),
-          nullptr, C_point_clouds[node_indices[j - 1]].rotation(),
+          nullptr, C_nodes[trajectory_id].at(node_index - 1).rotation(),
-          C_point_clouds[node_indices[j]].rotation());
+          C_nodes[trajectory_id].at(node_index).rotation());
     }
   }
 
@@ -270,9 +283,14 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
           C_submaps[trajectory_id][submap_index].ToRigid();
     }
   }
-
+  for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size();
-  for (size_t j = 0; j != node_data_.size(); ++j) {
+       ++trajectory_id) {
-    node_data_[j].point_cloud_pose = C_point_clouds[j].ToRigid();
+    for (size_t node_index = 0;
+         node_index != nodes_per_trajectory[trajectory_id].size();
+         ++node_index) {
+      node_data_[nodes_per_trajectory[trajectory_id][node_index]]
+          .point_cloud_pose = C_nodes[trajectory_id][node_index].ToRigid();
+    }
   }
 }
 

cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc

@@ -132,13 +132,13 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
   std::vector<OptimizationProblem::Constraint> constraints;
   for (int j = 0; j != kNumNodes; ++j) {
     constraints.push_back(OptimizationProblem::Constraint{
-        mapping::SubmapId{0, 0}, j,
+        mapping::SubmapId{0, 0}, mapping::NodeId{0, j},
         OptimizationProblem::Constraint::Pose{
             AddNoise(test_data[j].ground_truth_pose, test_data[j].noise),
             Eigen::Matrix<double, 6, 6>::Identity()}});
     // We add an additional independent, but equally noisy observation.
     constraints.push_back(OptimizationProblem::Constraint{
-        mapping::SubmapId{0, 1}, j,
+        mapping::SubmapId{0, 1}, mapping::NodeId{0, j},
         OptimizationProblem::Constraint::Pose{
             AddNoise(test_data[j].ground_truth_pose,
                      RandomYawOnlyTransform(0.2, 0.3)),
@@ -146,7 +146,7 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
     // We add very noisy data with high covariance (i.e. small Lambda) to verify
     // it is mostly ignored.
     constraints.push_back(OptimizationProblem::Constraint{
-        mapping::SubmapId{0, 2}, j,
+        mapping::SubmapId{0, 2}, mapping::NodeId{0, j},
         OptimizationProblem::Constraint::Pose{
             kSubmap2Transform.inverse() * test_data[j].ground_truth_pose *
                 RandomTransform(1e3, 3.),