From c65f7a97b639369d0a2e430ae867a76206c06137 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= <cschuet@google.com>
Date: Thu, 7 Sep 2017 16:35:49 +0200
Subject: [PATCH] Remove connected_components_ from SPG. (#509)

* Remove connected_components_ from SPG.

This PR removes connected_components_ from the SparsePoseGraph. Since
the connectivity structure is no longer updated by the
ConstraintBuilders it is no longer necessary for SPG to keep a copy of
the connected components for data consistency.
---
 cartographer/mapping_2d/sparse_pose_graph.cc | 6 ++----
 cartographer/mapping_2d/sparse_pose_graph.h  | 3 ---
 cartographer/mapping_3d/sparse_pose_graph.cc | 6 ++----
 cartographer/mapping_3d/sparse_pose_graph.h  | 3 ---
 4 files changed, 4 insertions(+), 14 deletions(-)

diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc
index 4c7f465..4830b77 100644
--- a/cartographer/mapping_2d/sparse_pose_graph.cc
+++ b/cartographer/mapping_2d/sparse_pose_graph.cc
@@ -307,6 +307,7 @@ void SparsePoseGraph::HandleWorkQueue() {
           common::MutexLocker locker(&mutex_);
           constraints_.insert(constraints_.end(), result.begin(), result.end());
         }
+        RunOptimization();
 
         // Update the trajectory connectivity structure with the new
         // constraints.
@@ -316,7 +317,6 @@ void SparsePoseGraph::HandleWorkQueue() {
           trajectory_connectivity_.Connect(constraint.node_id.trajectory_id,
                                            constraint.submap_id.trajectory_id);
         }
-        RunOptimization();
 
         common::MutexLocker locker(&mutex_);
         num_scans_since_last_loop_closure_ = 0;
@@ -465,7 +465,6 @@ void SparsePoseGraph::RunOptimization() {
     }
   }
   optimized_submap_transforms_ = submap_data;
-  connected_components_ = trajectory_connectivity_.ConnectedComponents();
 
   TrimmingHandle trimming_handle(this);
   for (auto& trimmer : trimmers_) {
@@ -504,8 +503,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
 }
 
 std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
-  common::MutexLocker locker(&mutex_);
-  return connected_components_;
+  return trajectory_connectivity_.ConnectedComponents();
 }
 
 int SparsePoseGraph::num_submaps(const int trajectory_id) {
diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h
index bc94b28..79a95a6 100644
--- a/cartographer/mapping_2d/sparse_pose_graph.h
+++ b/cartographer/mapping_2d/sparse_pose_graph.h
@@ -194,9 +194,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   mapping::NestedVectorsById<SubmapData, mapping::SubmapId> submap_data_
       GUARDED_BY(mutex_);
 
-  // Connectivity structure of our trajectories by IDs.
-  std::vector<std::vector<int>> connected_components_;
-
   // Data that are currently being shown.
   mapping::NestedVectorsById<mapping::TrajectoryNode, mapping::NodeId>
       trajectory_nodes_ GUARDED_BY(mutex_);
diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc
index 3a680d4..9643971 100644
--- a/cartographer/mapping_3d/sparse_pose_graph.cc
+++ b/cartographer/mapping_3d/sparse_pose_graph.cc
@@ -323,6 +323,7 @@ void SparsePoseGraph::HandleWorkQueue() {
           common::MutexLocker locker(&mutex_);
           constraints_.insert(constraints_.end(), result.begin(), result.end());
         }
+        RunOptimization();
 
         // Update the trajectory connectivity structure with the new
         // constraints.
@@ -332,7 +333,6 @@ void SparsePoseGraph::HandleWorkQueue() {
           trajectory_connectivity_.Connect(constraint.node_id.trajectory_id,
                                            constraint.submap_id.trajectory_id);
         }
-        RunOptimization();
 
         common::MutexLocker locker(&mutex_);
         num_scans_since_last_loop_closure_ = 0;
@@ -503,7 +503,6 @@ void SparsePoseGraph::RunOptimization() {
     }
   }
   optimized_submap_transforms_ = optimization_problem_.submap_data();
-  connected_components_ = trajectory_connectivity_.ConnectedComponents();
 
   TrimmingHandle trimming_handle(this);
   for (auto& trimmer : trimmers_) {
@@ -535,8 +534,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
 }
 
 std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
-  common::MutexLocker locker(&mutex_);
-  return connected_components_;
+  return trajectory_connectivity_.ConnectedComponents();
 }
 
 int SparsePoseGraph::num_submaps(const int trajectory_id) {
diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h
index ef81186..e081479 100644
--- a/cartographer/mapping_3d/sparse_pose_graph.h
+++ b/cartographer/mapping_3d/sparse_pose_graph.h
@@ -199,9 +199,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   mapping::NestedVectorsById<SubmapData, mapping::SubmapId> submap_data_
       GUARDED_BY(mutex_);
 
-  // Connectivity structure of our trajectories by IDs.
-  std::vector<std::vector<int>> connected_components_;
-
   // Data that are currently being shown.
   mapping::NestedVectorsById<mapping::TrajectoryNode, mapping::NodeId>
       trajectory_nodes_ GUARDED_BY(mutex_);