Also use vector<map<>> for node data in 3D. (#516)

This reduces the difference between 2D and 3D and moves
3D towards localization and trimming.

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -229,10 +229,9 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
   const auto& node_data = optimization_problem_.node_data();
   for (size_t trajectory_id = 0; trajectory_id != node_data.size();
        ++trajectory_id) {
-    for (size_t node_index = 0; node_index != node_data[trajectory_id].size();
+    for (const auto& index_node_data : node_data[trajectory_id]) {
-         ++node_index) {
       const mapping::NodeId node_id{static_cast<int>(trajectory_id),
-                                    static_cast<int>(node_index)};
+                                    index_node_data.first};
       if (submap_data.node_ids.count(node_id) == 0) {
         ComputeConstraint(node_id, submap_id);
       }
@@ -257,10 +256,14 @@ void SparsePoseGraph::ComputeConstraintsForScan(
   const mapping::NodeId node_id{
       matching_id.trajectory_id,
       static_cast<size_t>(matching_id.trajectory_id) <
-              optimization_problem_.node_data().size()
+                  optimization_problem_.node_data().size() &&
+              !optimization_problem_.node_data()[matching_id.trajectory_id]
+                   .empty()
           ? static_cast<int>(optimization_problem_.node_data()
                                  .at(matching_id.trajectory_id)
-                                 .size())
+                                 .rbegin()
+                                 ->first +
+                             1)
           : 0};
   const auto& scan_data = trajectory_nodes_.at(node_id).constant_data;
   optimization_problem_.AddTrajectoryNode(
@@ -484,13 +487,10 @@ void SparsePoseGraph::RunOptimization() {
   const auto& node_data = optimization_problem_.node_data();
   for (int trajectory_id = 0;
        trajectory_id != static_cast<int>(node_data.size()); ++trajectory_id) {
-    int node_index = 0;
     const int num_nodes = trajectory_nodes_.num_indices(trajectory_id);
-    for (; node_index != static_cast<int>(node_data[trajectory_id].size());
+    for (const auto& node_data_index : node_data.at(trajectory_id)) {
-         ++node_index) {
+      const mapping::NodeId node_id{trajectory_id, node_data_index.first};
-      const mapping::NodeId node_id{trajectory_id, node_index};
+      trajectory_nodes_.at(node_id).pose = node_data_index.second.pose;
-      trajectory_nodes_.at(node_id).pose =
-          node_data[trajectory_id][node_index].pose;
     }
     // Extrapolate all point cloud poses that were added later.
     const auto local_to_new_global =
@@ -499,10 +499,15 @@ void SparsePoseGraph::RunOptimization() {
         optimized_submap_transforms_, trajectory_id);
     const transform::Rigid3d old_global_to_new_global =
         local_to_new_global * local_to_old_global.inverse();
-    for (; node_index < num_nodes; ++node_index) {
+    int last_optimized_node_index =
+        node_data.at(trajectory_id).empty()
+            ? 0
+            : node_data.at(trajectory_id).rbegin()->first;
+    for (int node_index = last_optimized_node_index + 1; node_index < num_nodes;
+         ++node_index) {
       const mapping::NodeId node_id{trajectory_id, node_index};
-      trajectory_nodes_.at(node_id).pose =
+      auto& node_pose = trajectory_nodes_.at(node_id).pose;
-          old_global_to_new_global * trajectory_nodes_.at(node_id).pose;
+      node_pose = old_global_to_new_global * node_pose;
     }
   }
   optimized_submap_transforms_ = submap_data;

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc

@@ -86,7 +86,11 @@ void OptimizationProblem::AddTrajectoryNode(
   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, initial_pose, pose});
+  trajectory_data_.resize(std::max(trajectory_data_.size(), node_data_.size()));
+  auto& trajectory_data = trajectory_data_[trajectory_id];
+  node_data_[trajectory_id].emplace(trajectory_data.next_node_index,
+                                    NodeData{time, initial_pose, pose});
+  ++trajectory_data.next_node_index;
 }
 
 void OptimizationProblem::AddSubmap(const int trajectory_id,
@@ -130,7 +134,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
   CHECK(!submap_data_[0].empty());
   // TODO(hrapp): Move ceres data into SubmapData.
   std::vector<std::map<int, CeresPose>> C_submaps(submap_data_.size());
-  std::vector<std::deque<CeresPose>> C_nodes(node_data_.size());
+  std::vector<std::map<int, CeresPose>> C_nodes(node_data_.size());
   bool first_submap = true;
   for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
        ++trajectory_id) {
@@ -169,17 +173,19 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
   for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
        ++trajectory_id) {
     const bool frozen = frozen_trajectories.count(trajectory_id);
-    for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
+    for (const auto& index_node_data : node_data_[trajectory_id]) {
-         ++node_index) {
+      const int node_index = index_node_data.first;
-      C_nodes[trajectory_id].emplace_back(
+      C_nodes[trajectory_id].emplace(
-          node_data_[trajectory_id][node_index].pose,
+          std::piecewise_construct, std::forward_as_tuple(node_index),
-          translation_parameterization(),
+          std::forward_as_tuple(
-          common::make_unique<ceres::QuaternionParameterization>(), &problem);
+              index_node_data.second.pose, translation_parameterization(),
+              common::make_unique<ceres::QuaternionParameterization>(),
+              &problem));
       if (frozen) {
         problem.SetParameterBlockConstant(
-            C_nodes[trajectory_id].back().rotation());
+            C_nodes[trajectory_id].at(node_index).rotation());
         problem.SetParameterBlockConstant(
-            C_nodes[trajectory_id].back().translation());
+            C_nodes[trajectory_id].at(node_index).translation());
       }
     }
   }
@@ -212,8 +218,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
   CHECK_GE(trajectory_data_.size(), node_data_.size());
   for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
        ++trajectory_id) {
-    const auto& node_data = node_data_[trajectory_id];
+    if (node_data_[trajectory_id].empty()) {
-    if (node_data.empty()) {
       // We skip empty trajectories which might not have any IMU data.
       continue;
     }
@@ -223,29 +228,47 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
     const std::deque<sensor::ImuData>& imu_data = imu_data_.at(trajectory_id);
     CHECK(!imu_data.empty());
 
-    // Skip IMU data before the first node of this trajectory.
+    auto imu_it = imu_data.cbegin();
-    auto it = imu_data.cbegin();
+    for (auto node_it = node_data_[trajectory_id].begin();;) {
-    while ((it + 1) != imu_data.cend() && (it + 1)->time <= node_data[0].time) {
+      const int first_node_index = node_it->first;
-      ++it;
+      const NodeData& first_node_data = node_it->second;
+      ++node_it;
+      if (node_it == node_data_[trajectory_id].end()) {
+        break;
       }
 
-    for (size_t node_index = 1; node_index < node_data.size(); ++node_index) {
+      const int second_node_index = node_it->first;
-      auto it2 = it;
+      const NodeData& second_node_data = node_it->second;
-      const IntegrateImuResult<double> result =
+
-          IntegrateImu(imu_data, node_data[node_index - 1].time,
+      if (second_node_index != first_node_index + 1) {
-                       node_data[node_index].time, &it);
+        continue;
-      if (node_index + 1 < node_data.size()) {
+      }
-        const common::Time first_time = node_data[node_index - 1].time;
+
-        const common::Time second_time = node_data[node_index].time;
+      // Skip IMU data before the node.
-        const common::Time third_time = node_data[node_index + 1].time;
+      while ((imu_it + 1) != imu_data.cend() &&
+             (imu_it + 1)->time <= first_node_data.time) {
+        ++imu_it;
+      }
+
+      auto imu_it2 = imu_it;
+      const IntegrateImuResult<double> result = IntegrateImu(
+          imu_data, first_node_data.time, second_node_data.time, &imu_it);
+      const auto next_node_it = std::next(node_it);
+      if (next_node_it != node_data_[trajectory_id].end() &&
+          next_node_it->first == second_node_index + 1) {
+        const int third_node_index = next_node_it->first;
+        const NodeData& third_node_data = next_node_it->second;
+        const common::Time first_time = first_node_data.time;
+        const common::Time second_time = second_node_data.time;
+        const common::Time third_time = third_node_data.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;
         const common::Time second_center = second_time + second_duration / 2;
         const IntegrateImuResult<double> result_to_first_center =
-            IntegrateImu(imu_data, first_time, first_center, &it2);
+            IntegrateImu(imu_data, first_time, first_center, &imu_it2);
         const IntegrateImuResult<double> result_center_to_center =
-            IntegrateImu(imu_data, first_center, second_center, &it2);
+            IntegrateImu(imu_data, first_center, second_center, &imu_it2);
         // 'delta_velocity' is the change in velocity from the point in time
         // halfway between the first and second poses to halfway between second
         // and third pose. It is computed from IMU data and still contains a
@@ -262,10 +285,10 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
                     options_.acceleration_weight(), delta_velocity,
                     common::ToSeconds(first_duration),
                     common::ToSeconds(second_duration))),
-            nullptr, C_nodes[trajectory_id].at(node_index).rotation(),
+            nullptr, C_nodes[trajectory_id].at(second_node_index).rotation(),
-            C_nodes[trajectory_id].at(node_index - 1).translation(),
+            C_nodes[trajectory_id].at(first_node_index).translation(),
-            C_nodes[trajectory_id].at(node_index).translation(),
+            C_nodes[trajectory_id].at(second_node_index).translation(),
-            C_nodes[trajectory_id].at(node_index + 1).translation(),
+            C_nodes[trajectory_id].at(third_node_index).translation(),
             &trajectory_data.gravity_constant,
             trajectory_data.imu_calibration.data());
       }
@@ -273,8 +296,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
           new ceres::AutoDiffCostFunction<RotationCostFunction, 3, 4, 4, 4>(
               new RotationCostFunction(options_.rotation_weight(),
                                        result.delta_rotation)),
-          nullptr, C_nodes[trajectory_id].at(node_index - 1).rotation(),
+          nullptr, C_nodes[trajectory_id].at(first_node_index).rotation(),
-          C_nodes[trajectory_id].at(node_index).rotation(),
+          C_nodes[trajectory_id].at(second_node_index).rotation(),
           trajectory_data.imu_calibration.data());
     }
   }
@@ -289,22 +312,21 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
 
     bool fixed_frame_pose_initialized = false;
 
-    const auto& node_data = node_data_[trajectory_id];
+    for (auto& index_node_data : node_data_[trajectory_id]) {
-    for (size_t node_index = 0; node_index < node_data.size(); ++node_index) {
+      const int node_index = index_node_data.first;
-      if (!fixed_frame_pose_data_.at(trajectory_id)
+      const NodeData& node_data = index_node_data.second;
-               .Has(node_data[node_index].time)) {
+      if (!fixed_frame_pose_data_.at(trajectory_id).Has(node_data.time)) {
         continue;
       }
 
       const mapping::SparsePoseGraph::Constraint::Pose constraint_pose{
-          fixed_frame_pose_data_.at(trajectory_id)
+          fixed_frame_pose_data_.at(trajectory_id).Lookup(node_data.time),
-              .Lookup(node_data[node_index].time),
           options_.fixed_frame_pose_translation_weight(),
           options_.fixed_frame_pose_rotation_weight()};
 
       if (!fixed_frame_pose_initialized) {
         const transform::Rigid3d fixed_frame_pose_in_map =
-            node_data[node_index].pose * constraint_pose.zbar_ij.inverse();
+            node_data.pose * constraint_pose.zbar_ij.inverse();
         C_fixed_frames.emplace_back(
             transform::Rigid3d(
                 fixed_frame_pose_in_map.translation(),
@@ -362,15 +384,14 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
   }
   for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
        ++trajectory_id) {
-    for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
+    for (auto& index_node_data : node_data_[trajectory_id]) {
-         ++node_index) {
+      index_node_data.second.pose =
-      node_data_[trajectory_id][node_index].pose =
+          C_nodes[trajectory_id].at(index_node_data.first).ToRigid();
-          C_nodes[trajectory_id][node_index].ToRigid();
     }
   }
 }
 
-const std::vector<std::vector<NodeData>>& OptimizationProblem::node_data()
+const std::vector<std::map<int, NodeData>>& OptimizationProblem::node_data()
     const {
   return node_data_;
 }

cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h

@@ -81,7 +81,7 @@ class OptimizationProblem {
   void Solve(const std::vector<Constraint>& constraints,
              const std::set<int>& frozen_trajectories);
 
-  const std::vector<std::vector<NodeData>>& node_data() const;
+  const std::vector<std::map<int, NodeData>>& node_data() const;
   const std::vector<std::map<int, SubmapData>>& submap_data() const;
 
  private:
@@ -89,12 +89,13 @@ class OptimizationProblem {
     double gravity_constant = 9.8;
     std::array<double, 4> imu_calibration{{1., 0., 0., 0.}};
     int next_submap_index = 0;
+    int next_node_index = 0;
   };
 
   mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
   FixZ fix_z_;
   std::vector<std::deque<sensor::ImuData>> imu_data_;
-  std::vector<std::vector<NodeData>> node_data_;
+  std::vector<std::map<int, NodeData>> node_data_;
   std::vector<transform::TransformInterpolationBuffer> odometry_data_;
   std::vector<std::map<int, SubmapData>> submap_data_;
   std::vector<TrajectoryData> trajectory_data_;

cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc

@@ -159,10 +159,10 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
   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].pose.translation())
+                                 node_data.at(j).pose.translation())
                                     .norm();
     rotation_error_before += transform::GetAngle(
-        test_data[j].ground_truth_pose.inverse() * node_data[j].pose);
+        test_data[j].ground_truth_pose.inverse() * node_data.at(j).pose);
   }
 
   optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform);
@@ -175,10 +175,10 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
   double rotation_error_after = 0.;
   for (int j = 0; j != kNumNodes; ++j) {
     translation_error_after += (test_data[j].ground_truth_pose.translation() -
-                                node_data[j].pose.translation())
+                                node_data.at(j).pose.translation())
                                    .norm();
     rotation_error_after += transform::GetAngle(
-        test_data[j].ground_truth_pose.inverse() * node_data[j].pose);
+        test_data[j].ground_truth_pose.inverse() * node_data.at(j).pose);
   }
 
   EXPECT_GT(0.8 * translation_error_before, translation_error_after);