From 286d16238eabb47861ebdc305adcdfb83c76e1ab Mon Sep 17 00:00:00 2001
From: Susanne Pielawa <32822068+spielawa@users.noreply.github.com>
Date: Wed, 10 Jan 2018 14:30:51 +0100
Subject: [PATCH] In optimization_problem, use a map instead of vector for
 trajectory data. (#805)

PAIR=wohe
---
 .../pose_graph/optimization_problem.cc        | 29 +++++++------------
 .../pose_graph/optimization_problem.h         |  3 +-
 2 files changed, 11 insertions(+), 21 deletions(-)

diff --git a/cartographer/mapping_3d/pose_graph/optimization_problem.cc b/cartographer/mapping_3d/pose_graph/optimization_problem.cc
index 925868c..6c6de43 100644
--- a/cartographer/mapping_3d/pose_graph/optimization_problem.cc
+++ b/cartographer/mapping_3d/pose_graph/optimization_problem.cc
@@ -126,20 +126,16 @@ void OptimizationProblem::AddTrajectoryNode(
     const int trajectory_id, const common::Time time,
     const transform::Rigid3d& local_pose,
     const transform::Rigid3d& global_pose) {
-  CHECK_GE(trajectory_id, 0);
-  trajectory_data_.resize(std::max(trajectory_data_.size(),
-                                   static_cast<size_t>(trajectory_id) + 1));
   node_data_.Append(trajectory_id, NodeData{time, local_pose, global_pose});
+  trajectory_data_[trajectory_id];
 }
 
 void OptimizationProblem::InsertTrajectoryNode(
     const mapping::NodeId& node_id, const common::Time time,
     const transform::Rigid3d& local_pose,
     const transform::Rigid3d& global_pose) {
-  CHECK_GE(node_id.trajectory_id, 0);
-  trajectory_data_.resize(std::max(
-      trajectory_data_.size(), static_cast<size_t>(node_id.trajectory_id) + 1));
   node_data_.Insert(node_id, NodeData{time, local_pose, global_pose});
+  trajectory_data_[node_id.trajectory_id];
 }
 
 void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
@@ -147,23 +143,19 @@ void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
   odometry_data_.Trim(node_data_, node_id);
   fixed_frame_pose_data_.Trim(node_data_, node_id);
   node_data_.Trim(node_id);
+  if (node_data_.SizeOfTrajectoryOrZero(node_id.trajectory_id) == 0) {
+    trajectory_data_.erase(node_id.trajectory_id);
+  }
 }
 
 void OptimizationProblem::AddSubmap(
     const int trajectory_id, const transform::Rigid3d& global_submap_pose) {
-  CHECK_GE(trajectory_id, 0);
-  trajectory_data_.resize(std::max(trajectory_data_.size(),
-                                   static_cast<size_t>(trajectory_id) + 1));
   submap_data_.Append(trajectory_id, SubmapData{global_submap_pose});
 }
 
 void OptimizationProblem::InsertSubmap(
     const mapping::SubmapId& submap_id,
     const transform::Rigid3d& global_submap_pose) {
-  CHECK_GE(submap_id.trajectory_id, 0);
-  trajectory_data_.resize(
-      std::max(trajectory_data_.size(),
-               static_cast<size_t>(submap_id.trajectory_id) + 1));
   submap_data_.Insert(submap_id, SubmapData{global_submap_pose});
 }
 
@@ -441,15 +433,14 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
       &problem, &summary);
   if (options_.log_solver_summary()) {
     LOG(INFO) << summary.FullReport();
-    for (size_t trajectory_id = 0; trajectory_id != trajectory_data_.size();
-         ++trajectory_id) {
+    for (const auto& trajectory_id_and_data : trajectory_data_) {
+      const int trajectory_id = trajectory_id_and_data.first;
+      const TrajectoryData& trajectory_data = trajectory_id_and_data.second;
       if (trajectory_id != 0) {
         LOG(INFO) << "Trajectory " << trajectory_id << ":";
       }
-      LOG(INFO) << "Gravity was: "
-                << trajectory_data_[trajectory_id].gravity_constant;
-      const auto& imu_calibration =
-          trajectory_data_[trajectory_id].imu_calibration;
+      LOG(INFO) << "Gravity was: " << trajectory_data.gravity_constant;
+      const auto& imu_calibration = trajectory_data.imu_calibration;
       LOG(INFO) << "IMU correction was: "
                 << common::RadToDeg(2. * std::acos(imu_calibration[0]))
                 << " deg (" << imu_calibration[0] << ", " << imu_calibration[1]
diff --git a/cartographer/mapping_3d/pose_graph/optimization_problem.h b/cartographer/mapping_3d/pose_graph/optimization_problem.h
index 580d691..aa42132 100644
--- a/cartographer/mapping_3d/pose_graph/optimization_problem.h
+++ b/cartographer/mapping_3d/pose_graph/optimization_problem.h
@@ -18,7 +18,6 @@
 #define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_
 
 #include <array>
-#include <deque>
 #include <map>
 #include <set>
 #include <vector>
@@ -115,7 +114,7 @@ class OptimizationProblem {
   sensor::MapByTime<sensor::ImuData> imu_data_;
   sensor::MapByTime<sensor::OdometryData> odometry_data_;
   sensor::MapByTime<sensor::FixedFramePoseData> fixed_frame_pose_data_;
-  std::vector<TrajectoryData> trajectory_data_;
+  std::map<int, TrajectoryData> trajectory_data_;
 };
 
 }  // namespace pose_graph