Keep node data per trajectory in the optimization problem. (#282)

Related to #256.

PAIR=SirVer

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -102,9 +102,6 @@ void SparsePoseGraph::AddScan(
   const int trajectory_id = trajectory_ids_.at(trajectory);
   const int flat_scan_index = trajectory_nodes_.size();
   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,
@@ -167,12 +164,15 @@ void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory,
 
 void SparsePoseGraph::ComputeConstraint(const int scan_index,
                                         const mapping::SubmapId& submap_id) {
-  const transform::Rigid2d relative_pose =
+  const mapping::NodeId node_id = scan_index_to_node_id_.at(scan_index);
-      optimization_problem_.submap_data()
+  const transform::Rigid2d relative_pose = optimization_problem_.submap_data()
                                                .at(submap_id.trajectory_id)
                                                .at(submap_id.submap_index)
                                                .pose.inverse() *
-      optimization_problem_.node_data().at(scan_index).point_cloud_pose;
+                                           optimization_problem_.node_data()
+                                               .at(node_id.trajectory_id)
+                                               .at(node_id.node_index)
+                                               .point_cloud_pose;
 
   const mapping::Submaps* const scan_trajectory =
       trajectory_nodes_[scan_index].constant_data->trajectory;
@@ -186,8 +186,7 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
         submap_states_.at(submap_id.trajectory_id)
             .at(submap_id.submap_index)
             .submap,
-        scan_index_to_node_id_.at(scan_index), scan_index,
+        node_id, scan_index, &trajectory_connectivity_,
-        &trajectory_connectivity_,
         &trajectory_nodes_[scan_index].constant_data->range_data_2d.returns);
   } else {
     const bool scan_and_submap_trajectories_connected =
@@ -202,7 +201,7 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
           submap_states_.at(submap_id.trajectory_id)
               .at(submap_id.submap_index)
               .submap,
-          scan_index_to_node_id_.at(scan_index), scan_index,
+          node_id, scan_index,
           &trajectory_nodes_[scan_index].constant_data->range_data_2d.returns,
           relative_pose);
     }
@@ -212,14 +211,11 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
 void SparsePoseGraph::ComputeConstraintsForOldScans(
     const mapping::Submap* submap) {
   const auto submap_id = GetSubmapId(submap);
-  const auto& node_data = optimization_problem_.node_data();
+  const auto& submap_state =
-  CHECK_GT(node_data.size(), 0);
+      submap_states_.at(submap_id.trajectory_id).at(submap_id.submap_index);
-  CHECK_LT(node_data.size(), std::numeric_limits<int>::max());
+  const int num_nodes = scan_index_to_node_id_.size();
-  const int num_nodes = node_data.size();
   for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
-    if (submap_states_.at(submap_id.trajectory_id)
+    if (submap_state.scan_indices.count(scan_index) == 0) {
-            .at(submap_id.submap_index)
-            .scan_indices.count(scan_index) == 0) {
       ComputeConstraint(scan_index, submap_id);
     }
   }
@@ -238,12 +234,17 @@ void SparsePoseGraph::ComputeConstraintsForScan(
           .at(matching_id.submap_index)
           .pose *
       sparse_pose_graph::ComputeSubmapPose(*matching_submap).inverse() * pose;
-  CHECK_EQ(scan_index, optimization_problem_.node_data().size());
+  CHECK_EQ(scan_index, scan_index_to_node_id_.size());
+  scan_index_to_node_id_.push_back(
+      mapping::NodeId{matching_id.trajectory_id,
+                      num_nodes_in_trajectory_[matching_id.trajectory_id]});
+  ++num_nodes_in_trajectory_[matching_id.trajectory_id];
   const mapping::TrajectoryNode::ConstantData* const scan_data =
       trajectory_nodes_[scan_index].constant_data;
+  CHECK_EQ(trajectory_ids_.at(scan_data->trajectory),
+           matching_id.trajectory_id);
   optimization_problem_.AddTrajectoryNode(
-      trajectory_ids_.at(scan_data->trajectory), scan_data->time, pose,
+      matching_id.trajectory_id, scan_data->time, pose, optimized_pose);
-      optimized_pose);
   for (const mapping::Submap* submap : insertion_submaps) {
     const mapping::SubmapId submap_id = GetSubmapId(submap);
     CHECK(!submap_states_.at(submap_id.trajectory_id)
@@ -385,10 +386,12 @@ void SparsePoseGraph::RunOptimization() {
   common::MutexLocker locker(&mutex_);
 
   const auto& node_data = optimization_problem_.node_data();
-  const size_t num_optimized_poses = node_data.size();
+  const size_t num_optimized_poses = scan_index_to_node_id_.size();
   for (size_t i = 0; i != num_optimized_poses; ++i) {
     trajectory_nodes_[i].pose =
-        transform::Rigid3d(transform::Embed3D(node_data[i].point_cloud_pose));
+        transform::Embed3D(node_data.at(scan_index_to_node_id_[i].trajectory_id)
+                               .at(scan_index_to_node_id_[i].node_index)
+                               .point_cloud_pose);
   }
   // Extrapolate all point cloud poses that were added later.
   std::unordered_map<const mapping::Submaps*, transform::Rigid3d>
@@ -478,8 +481,8 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
     } else if (result.empty()) {
       result.push_back(transform::Rigid3d::Identity());
     } else {
-      // Extrapolate to the remaining submaps. Accessing local_pose() in
+      // Extrapolate to the remaining submaps. Accessing local_pose() in Submaps
-      // Submaps is okay, since the member is const.
+      // is okay, since the member is const.
       result.push_back(result.back() *
                        submap_states_.at(trajectory_id)
                            .at(result.size() - 1)

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc

@@ -66,7 +66,7 @@ void OptimizationProblem::AddImuData(const int trajectory_id,
                                      const Eigen::Vector3d& angular_velocity) {
   CHECK_GE(trajectory_id, 0);
   imu_data_.resize(
-      std::max(submap_data_.size(), static_cast<size_t>(trajectory_id) + 1));
+      std::max(imu_data_.size(), static_cast<size_t>(trajectory_id) + 1));
   imu_data_[trajectory_id].push_back(
       mapping_3d::ImuData{time, linear_acceleration, angular_velocity});
 }
@@ -75,8 +75,11 @@ void OptimizationProblem::AddTrajectoryNode(
     const int trajectory_id, const common::Time time,
     const transform::Rigid2d& initial_point_cloud_pose,
     const transform::Rigid2d& point_cloud_pose) {
-  node_data_.push_back(NodeData{trajectory_id, time, initial_point_cloud_pose,
+  CHECK_GE(trajectory_id, 0);
-                                point_cloud_pose});
+  node_data_.resize(
+      std::max(node_data_.size(), static_cast<size_t>(trajectory_id) + 1));
+  node_data_[trajectory_id].push_back(
+      NodeData{time, initial_point_cloud_pose, point_cloud_pose});
 }
 
 void OptimizationProblem::AddSubmap(const int trajectory_id,
@@ -98,20 +101,13 @@ 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::deque<std::array<double, 3>>> C_nodes(
+  std::vector<std::deque<std::array<double, 3>>> C_nodes(node_data_.size());
-      nodes_per_trajectory.size());
   for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
        ++trajectory_id) {
     for (size_t submap_index = 0;
@@ -130,15 +126,13 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
       }
     }
   }
-  for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size();
+  for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
        ++trajectory_id) {
-    C_nodes[trajectory_id].resize(nodes_per_trajectory[trajectory_id].size());
+    C_nodes[trajectory_id].resize(node_data_[trajectory_id].size());
-    for (size_t node_index = 0;
+    for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
-         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]]
+          FromPose(node_data_[trajectory_id][node_index].point_cloud_pose);
-                       .point_cloud_pose);
       problem.AddParameterBlock(C_nodes[trajectory_id][node_index].data(), 3);
     }
   }
@@ -166,24 +160,18 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
       options_.consecutive_scan_translation_penalty_factor(),
       options_.consecutive_scan_rotation_penalty_factor());
 
-  for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size();
+  for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
        ++trajectory_id) {
-    if (nodes_per_trajectory[trajectory_id].empty()) {
+    for (size_t node_index = 1; node_index < node_data_[trajectory_id].size();
-      continue;
-    }
-    for (size_t node_index = 1;
-         node_index != nodes_per_trajectory[trajectory_id].size();
          ++node_index) {
       constexpr double kUnusedPositionPenalty = 1.;
       constexpr double kUnusedOrientationPenalty = 1.;
       problem.AddResidualBlock(
-          new ceres::AutoDiffCostFunction<
+          new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
-              SpaCostFunction, 3, 3, 3>(new SpaCostFunction(Constraint::Pose{
+              new SpaCostFunction(Constraint::Pose{
-              transform::Embed3D(
+                  transform::Embed3D(node_data_[trajectory_id][node_index - 1]
-                  node_data_[nodes_per_trajectory[trajectory_id]
-                                                 [node_index - 1]]
                                          .initial_point_cloud_pose.inverse() *
-                  node_data_[nodes_per_trajectory[trajectory_id][node_index]]
+                                     node_data_[trajectory_id][node_index]
                                          .initial_point_cloud_pose),
                   kalman_filter::Embed3D(consecutive_pose_change_penalty_matrix,
                                          kUnusedPositionPenalty,
@@ -213,18 +201,18 @@ 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 trajectory_id = 0; trajectory_id != node_data_.size();
        ++trajectory_id) {
-    for (size_t node_index = 0;
+    for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
-         node_index != nodes_per_trajectory[trajectory_id].size();
          ++node_index) {
-      node_data_[nodes_per_trajectory[trajectory_id][node_index]]
+      node_data_[trajectory_id][node_index].point_cloud_pose =
-          .point_cloud_pose = ToPose(C_nodes[trajectory_id][node_index]);
+          ToPose(C_nodes[trajectory_id][node_index]);
     }
   }
 }
 
-const std::vector<NodeData>& OptimizationProblem::node_data() const {
+const std::vector<std::vector<NodeData>>& OptimizationProblem::node_data()
+    const {
   return node_data_;
 }
 

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h

@@ -18,6 +18,7 @@
 #define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_
 
 #include <array>
+#include <deque>
 #include <map>
 #include <vector>
 
@@ -27,7 +28,6 @@
 #include "cartographer/common/time.h"
 #include "cartographer/mapping/sparse_pose_graph.h"
 #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h"
-#include "cartographer/mapping_2d/submaps.h"
 #include "cartographer/mapping_3d/imu_integration.h"
 
 namespace cartographer {
@@ -35,8 +35,6 @@ namespace mapping_2d {
 namespace sparse_pose_graph {
 
 struct NodeData {
-  // TODO(whess): Keep nodes per trajectory instead.
-  int trajectory_id;
   common::Time time;
   transform::Rigid2d initial_point_cloud_pose;
   transform::Rigid2d point_cloud_pose;
@@ -72,13 +70,13 @@ class OptimizationProblem {
   // Computes the optimized poses.
   void Solve(const std::vector<Constraint>& constraints);
 
-  const std::vector<NodeData>& node_data() const;
+  const std::vector<std::vector<NodeData>>& node_data() const;
   const std::vector<std::vector<SubmapData>>& submap_data() const;
 
  private:
   mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
   std::vector<std::deque<mapping_3d::ImuData>> imu_data_;
-  std::vector<NodeData> node_data_;
+  std::vector<std::vector<NodeData>> node_data_;
   std::vector<std::vector<SubmapData>> submap_data_;
 };
 

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -100,9 +100,6 @@ void SparsePoseGraph::AddScan(
   const int trajectory_id = trajectory_ids_.at(trajectory);
   const int flat_scan_index = trajectory_nodes_.size();
   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(), {}, {}},
@@ -167,12 +164,15 @@ void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory,
 
 void SparsePoseGraph::ComputeConstraint(const int scan_index,
                                         const mapping::SubmapId& submap_id) {
-  const transform::Rigid3d relative_pose =
+  const mapping::NodeId node_id = scan_index_to_node_id_.at(scan_index);
-      optimization_problem_.submap_data()
+  const transform::Rigid3d relative_pose = optimization_problem_.submap_data()
                                                .at(submap_id.trajectory_id)
                                                .at(submap_id.submap_index)
                                                .pose.inverse() *
-      optimization_problem_.node_data().at(scan_index).point_cloud_pose;
+                                           optimization_problem_.node_data()
+                                               .at(node_id.trajectory_id)
+                                               .at(node_id.node_index)
+                                               .point_cloud_pose;
 
   const mapping::Submaps* const scan_trajectory =
       trajectory_nodes_[scan_index].constant_data->trajectory;
@@ -186,8 +186,7 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
         submap_states_.at(submap_id.trajectory_id)
             .at(submap_id.submap_index)
             .submap,
-        scan_index_to_node_id_.at(scan_index), scan_index,
+        node_id, scan_index, &trajectory_connectivity_, trajectory_nodes_);
-        &trajectory_connectivity_, trajectory_nodes_);
   } else {
     const bool scan_and_submap_trajectories_connected =
         reverse_connected_components_.count(scan_trajectory_id) > 0 &&
@@ -201,22 +200,18 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
           submap_states_.at(submap_id.trajectory_id)
               .at(submap_id.submap_index)
               .submap,
-          scan_index_to_node_id_.at(scan_index), scan_index, trajectory_nodes_,
+          node_id, scan_index, trajectory_nodes_, relative_pose);
-          relative_pose);
     }
   }
 }
 
 void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) {
   const auto submap_id = GetSubmapId(submap);
-  const auto& node_data = optimization_problem_.node_data();
+  const auto& submap_state =
-  CHECK_GT(node_data.size(), 0);
+      submap_states_.at(submap_id.trajectory_id).at(submap_id.submap_index);
-  CHECK_LT(node_data.size(), std::numeric_limits<int>::max());
+  const int num_nodes = scan_index_to_node_id_.size();
-  const int num_nodes = node_data.size();
   for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
-    if (submap_states_.at(submap_id.trajectory_id)
+    if (submap_state.scan_indices.count(scan_index) == 0) {
-            .at(submap_id.submap_index)
-            .scan_indices.count(scan_index) == 0) {
       ComputeConstraint(scan_index, submap_id);
     }
   }
@@ -235,12 +230,17 @@ void SparsePoseGraph::ComputeConstraintsForScan(
           .at(matching_id.submap_index)
           .pose *
       matching_submap->local_pose().inverse() * pose;
-  CHECK_EQ(scan_index, optimization_problem_.node_data().size());
+  CHECK_EQ(scan_index, scan_index_to_node_id_.size());
+  scan_index_to_node_id_.push_back(
+      mapping::NodeId{matching_id.trajectory_id,
+                      num_nodes_in_trajectory_[matching_id.trajectory_id]});
+  ++num_nodes_in_trajectory_[matching_id.trajectory_id];
   const mapping::TrajectoryNode::ConstantData* const scan_data =
       trajectory_nodes_[scan_index].constant_data;
-  optimization_problem_.AddTrajectoryNode(
+  CHECK_EQ(trajectory_ids_.at(scan_data->trajectory),
-      trajectory_ids_.at(scan_data->trajectory), scan_data->time,
+           matching_id.trajectory_id);
-      optimized_pose);
+  optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id,
+                                          scan_data->time, optimized_pose);
   for (const Submap* submap : insertion_submaps) {
     const mapping::SubmapId submap_id = GetSubmapId(submap);
     CHECK(!submap_states_.at(submap_id.trajectory_id)
@@ -378,9 +378,12 @@ void SparsePoseGraph::RunOptimization() {
   common::MutexLocker locker(&mutex_);
 
   const auto& node_data = optimization_problem_.node_data();
-  const size_t num_optimized_poses = node_data.size();
+  const size_t num_optimized_poses = scan_index_to_node_id_.size();
   for (size_t i = 0; i != num_optimized_poses; ++i) {
-    trajectory_nodes_[i].pose = node_data[i].point_cloud_pose;
+    trajectory_nodes_[i].pose =
+        node_data.at(scan_index_to_node_id_[i].trajectory_id)
+            .at(scan_index_to_node_id_[i].node_index)
+            .point_cloud_pose;
   }
   // Extrapolate all point cloud poses that were added later.
   std::unordered_map<const mapping::Submaps*, transform::Rigid3d>
@@ -459,6 +462,7 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
     return {transform::Rigid3d::Identity()};
   }
 
+  // Submaps for which we have optimized poses.
   std::vector<transform::Rigid3d> result;
   for (const auto& state : submap_states_.at(trajectory_id)) {
     if (trajectory_id < submap_transforms.size() &&

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc

@@ -84,7 +84,7 @@ void OptimizationProblem::AddImuData(const int trajectory_id,
                                      const Eigen::Vector3d& angular_velocity) {
   CHECK_GE(trajectory_id, 0);
   imu_data_.resize(
-      std::max(submap_data_.size(), static_cast<size_t>(trajectory_id) + 1));
+      std::max(imu_data_.size(), static_cast<size_t>(trajectory_id) + 1));
   imu_data_[trajectory_id].push_back(
       ImuData{time, linear_acceleration, angular_velocity});
 }
@@ -92,7 +92,10 @@ void OptimizationProblem::AddImuData(const int trajectory_id,
 void OptimizationProblem::AddTrajectoryNode(
     const int trajectory_id, const common::Time time,
     const transform::Rigid3d& point_cloud_pose) {
-  node_data_.push_back(NodeData{trajectory_id, time, point_cloud_pose});
+  CHECK_GE(trajectory_id, 0);
+  node_data_.resize(
+      std::max(node_data_.size(), static_cast<size_t>(trajectory_id) + 1));
+  node_data_[trajectory_id].push_back(NodeData{time, point_cloud_pose});
 }
 
 void OptimizationProblem::AddSubmap(const int trajectory_id,
@@ -114,12 +117,6 @@ 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);
 
@@ -136,7 +133,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::vector<std::deque<CeresPose>> C_nodes(nodes_per_trajectory.size());
+  std::vector<std::deque<CeresPose>> C_nodes(node_data_.size());
   for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
        ++trajectory_id) {
     for (size_t submap_index = 0;
@@ -158,14 +155,12 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
       }
     }
   }
-  for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size();
+  for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
        ++trajectory_id) {
-    for (size_t node_index = 0;
+    for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
-         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]]
+          node_data_[trajectory_id][node_index].point_cloud_pose,
-              .point_cloud_pose,
           translation_parameterization(),
           common::make_unique<ceres::QuaternionParameterization>(), &problem);
     }
@@ -196,34 +191,29 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
 
   // Add constraints based on IMU observations of angular velocities and
   // linear acceleration.
-  for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size();
+  for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
        ++trajectory_id) {
-    const std::vector<size_t>& flat_indices =
-        nodes_per_trajectory[trajectory_id];
     const std::deque<ImuData>& imu_data = imu_data_.at(trajectory_id);
-    CHECK(!flat_indices.empty());
     CHECK(!imu_data.empty());
+    // TODO(whess): Add support for empty trajectories.
+    const auto& node_data = node_data_[trajectory_id];
+    CHECK(!node_data.empty());
 
     // Skip IMU data before the first node of this trajectory.
     auto it = imu_data.cbegin();
-    while ((it + 1) != imu_data.cend() &&
+    while ((it + 1) != imu_data.cend() && (it + 1)->time <= node_data[0].time) {
-           (it + 1)->time <= node_data_[flat_indices[0]].time) {
       ++it;
     }
 
-    for (size_t node_index = 1; node_index < flat_indices.size();
+    for (size_t node_index = 1; node_index < node_data.size(); ++node_index) {
-         ++node_index) {
       auto it2 = it;
       const IntegrateImuResult<double> result =
-          IntegrateImu(imu_data, node_data_[flat_indices[node_index - 1]].time,
+          IntegrateImu(imu_data, node_data[node_index - 1].time,
-                       node_data_[flat_indices[node_index]].time, &it);
+                       node_data[node_index].time, &it);
-      if (node_index + 1 < flat_indices.size()) {
+      if (node_index + 1 < node_data.size()) {
-        const common::Time first_time =
+        const common::Time first_time = node_data[node_index - 1].time;
-            node_data_[flat_indices[node_index - 1]].time;
+        const common::Time second_time = node_data[node_index].time;
-        const common::Time second_time =
+        const common::Time third_time = node_data[node_index + 1].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;
@@ -283,18 +273,18 @@ 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 trajectory_id = 0; trajectory_id != node_data_.size();
        ++trajectory_id) {
-    for (size_t node_index = 0;
+    for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
-         node_index != nodes_per_trajectory[trajectory_id].size();
          ++node_index) {
-      node_data_[nodes_per_trajectory[trajectory_id][node_index]]
+      node_data_[trajectory_id][node_index].point_cloud_pose =
-          .point_cloud_pose = C_nodes[trajectory_id][node_index].ToRigid();
+          C_nodes[trajectory_id][node_index].ToRigid();
     }
   }
 }
 
-const std::vector<NodeData>& OptimizationProblem::node_data() const {
+const std::vector<std::vector<NodeData>>& OptimizationProblem::node_data()
+    const {
   return node_data_;
 }
 

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h

@@ -35,8 +35,6 @@ namespace mapping_3d {
 namespace sparse_pose_graph {
 
 struct NodeData {
-  // TODO(whess): Keep nodes per trajectory instead.
-  int trajectory_id;
   common::Time time;
   transform::Rigid3d point_cloud_pose;
 };
@@ -73,14 +71,14 @@ class OptimizationProblem {
   // Computes the optimized poses.
   void Solve(const std::vector<Constraint>& constraints);
 
-  const std::vector<NodeData>& node_data() const;
+  const std::vector<std::vector<NodeData>>& node_data() const;
   const std::vector<std::vector<SubmapData>>& submap_data() const;
 
  private:
   mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
   FixZ fix_z_;
   std::vector<std::deque<ImuData>> imu_data_;
-  std::vector<NodeData> node_data_;
+  std::vector<std::vector<NodeData>> node_data_;
   std::vector<std::vector<SubmapData>> submap_data_;
   double gravity_constant_ = 9.8;
 };

cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc

@@ -155,7 +155,7 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
 
   double translation_error_before = 0.;
   double rotation_error_before = 0.;
-  const auto& node_data = optimization_problem_.node_data();
+  const auto& node_data = optimization_problem_.node_data().at(0);
   for (int j = 0; j != kNumNodes; ++j) {
     translation_error_before += (test_data[j].ground_truth_pose.translation() -
                                  node_data[j].point_cloud_pose.translation())