Introduce TrajectoryState (#1201)

[RFC=0023](https://github.com/googlecartographer/rfcs/blob/master/text/0023-delete-load.md)

cartographer/cloud/internal/client/pose_graph_stub.cc

@@ -105,6 +105,11 @@ PoseGraphStub::GetTrajectoryNodePoses() const {
   return node_poses;
 }
 
+std::map<int, mapping::PoseGraphInterface::TrajectoryState>
+PoseGraphStub::GetTrajectoryStates() const {
+  LOG(FATAL) << "not implemented";
+}
+
 std::map<std::string, transform::Rigid3d> PoseGraphStub::GetLandmarkPoses()
     const {
   google::protobuf::Empty request;

cartographer/cloud/internal/client/pose_graph_stub.h

@@ -41,6 +41,7 @@ class PoseGraphStub : public ::cartographer::mapping::PoseGraphInterface {
   GetTrajectoryNodes() const override;
   mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>
   GetTrajectoryNodePoses() const override;
+  std::map<int, TrajectoryState> GetTrajectoryStates() const override;
   std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override;
   void SetLandmarkPose(const std::string& landmark_id,
                        const transform::Rigid3d& global_pose) override;

cartographer/mapping/internal/2d/pose_graph_2d.cc

@@ -147,6 +147,11 @@ void PoseGraph2D::AddWorkItem(const std::function<void()>& work_item) {
 }
 
 void PoseGraph2D::AddTrajectoryIfNeeded(const int trajectory_id) {
+  data_.trajectories_state[trajectory_id];
+  CHECK(data_.trajectories_state.at(trajectory_id).state !=
+        TrajectoryState::FINISHED);
+  CHECK(data_.trajectories_state.at(trajectory_id).state !=
+        TrajectoryState::DELETED);
   data_.trajectory_connectivity_state.Add(trajectory_id);
   // Make sure we have a sampler for this trajectory.
   if (!global_localization_samplers_[trajectory_id]) {
@@ -436,8 +441,8 @@ void PoseGraph2D::WaitForAllComputations() {
 void PoseGraph2D::FinishTrajectory(const int trajectory_id) {
   common::MutexLocker locker(&mutex_);
   AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
-    CHECK_EQ(data_.finished_trajectories.count(trajectory_id), 0);
-    data_.finished_trajectories.insert(trajectory_id);
+    CHECK(!IsTrajectoryFinished(trajectory_id));
+    data_.trajectories_state[trajectory_id].state = TrajectoryState::FINISHED;
 
     for (const auto& submap : data_.submap_data.trajectory(trajectory_id)) {
       data_.submap_data.at(submap.id).state = SubmapState::kFinished;
@@ -448,20 +453,24 @@ void PoseGraph2D::FinishTrajectory(const int trajectory_id) {
 }
 
 bool PoseGraph2D::IsTrajectoryFinished(const int trajectory_id) const {
-  return data_.finished_trajectories.count(trajectory_id) > 0;
+  return data_.trajectories_state.count(trajectory_id) != 0 &&
+         data_.trajectories_state.at(trajectory_id).state ==
+             TrajectoryState::FINISHED;
 }
 
 void PoseGraph2D::FreezeTrajectory(const int trajectory_id) {
   common::MutexLocker locker(&mutex_);
   data_.trajectory_connectivity_state.Add(trajectory_id);
   AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
-    CHECK_EQ(data_.frozen_trajectories.count(trajectory_id), 0);
-    data_.frozen_trajectories.insert(trajectory_id);
+    CHECK(!IsTrajectoryFrozen(trajectory_id));
+    data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN;
   });
 }
 
 bool PoseGraph2D::IsTrajectoryFrozen(const int trajectory_id) const {
-  return data_.frozen_trajectories.count(trajectory_id) > 0;
+  return data_.trajectories_state.count(trajectory_id) != 0 &&
+         data_.trajectories_state.at(trajectory_id).state ==
+             TrajectoryState::FROZEN;
 }
 
 void PoseGraph2D::AddSubmapFromProto(
@@ -599,7 +608,7 @@ void PoseGraph2D::RunOptimization() {
   // data_.constraints, data_.frozen_trajectories and data_.landmark_nodes
   // when executing the Solve. Solve is time consuming, so not taking the mutex
   // before Solve to avoid blocking foreground processing.
-  optimization_problem_->Solve(data_.constraints, data_.frozen_trajectories,
+  optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),
                                data_.landmark_nodes);
   common::MutexLocker locker(&mutex_);
 
@@ -663,6 +672,16 @@ MapById<NodeId, TrajectoryNodePose> PoseGraph2D::GetTrajectoryNodePoses()
   return node_poses;
 }
 
+std::map<int, PoseGraphInterface::TrajectoryState>
+PoseGraph2D::GetTrajectoryStates() const {
+  std::map<int, PoseGraphInterface::TrajectoryState> trajectories_state;
+  common::MutexLocker locker(&mutex_);
+  for (const auto& it : data_.trajectories_state) {
+    trajectories_state[it.first] = it.second.state;
+  }
+  return trajectories_state;
+}
+
 std::map<std::string, transform::Rigid3d> PoseGraph2D::GetLandmarkPoses()
     const {
   std::map<std::string, transform::Rigid3d> landmark_poses;

cartographer/mapping/internal/2d/pose_graph_2d.h

@@ -121,6 +121,8 @@ class PoseGraph2D : public PoseGraph {
       EXCLUDES(mutex_);
   MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() const override
       EXCLUDES(mutex_);
+  std::map<int, TrajectoryState> GetTrajectoryStates() const override
+      EXCLUDES(mutex_);
   std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override
       EXCLUDES(mutex_);
   void SetLandmarkPose(const std::string& landmark_id,

cartographer/mapping/internal/3d/pose_graph_3d.cc

@@ -145,6 +145,11 @@ void PoseGraph3D::AddWorkItem(const std::function<void()>& work_item) {
 }
 
 void PoseGraph3D::AddTrajectoryIfNeeded(const int trajectory_id) {
+  data_.trajectories_state[trajectory_id];
+  CHECK(data_.trajectories_state.at(trajectory_id).state !=
+        TrajectoryState::FINISHED);
+  CHECK(data_.trajectories_state.at(trajectory_id).state !=
+        TrajectoryState::DELETED);
   data_.trajectory_connectivity_state.Add(trajectory_id);
   // Make sure we have a sampler for this trajectory.
   if (!global_localization_samplers_[trajectory_id]) {
@@ -452,8 +457,8 @@ void PoseGraph3D::WaitForAllComputations() {
 void PoseGraph3D::FinishTrajectory(const int trajectory_id) {
   common::MutexLocker locker(&mutex_);
   AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
-    CHECK_EQ(data_.finished_trajectories.count(trajectory_id), 0);
-    data_.finished_trajectories.insert(trajectory_id);
+    CHECK(!IsTrajectoryFinished(trajectory_id));
+    data_.trajectories_state[trajectory_id].state = TrajectoryState::FINISHED;
 
     for (const auto& submap : data_.submap_data.trajectory(trajectory_id)) {
       data_.submap_data.at(submap.id).state = SubmapState::kFinished;
@@ -464,20 +469,24 @@ void PoseGraph3D::FinishTrajectory(const int trajectory_id) {
 }
 
 bool PoseGraph3D::IsTrajectoryFinished(const int trajectory_id) const {
-  return data_.finished_trajectories.count(trajectory_id) > 0;
+  return data_.trajectories_state.count(trajectory_id) != 0 &&
+         data_.trajectories_state.at(trajectory_id).state ==
+             TrajectoryState::FINISHED;
 }
 
 void PoseGraph3D::FreezeTrajectory(const int trajectory_id) {
   common::MutexLocker locker(&mutex_);
   data_.trajectory_connectivity_state.Add(trajectory_id);
   AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
-    CHECK_EQ(data_.frozen_trajectories.count(trajectory_id), 0);
-    data_.frozen_trajectories.insert(trajectory_id);
+    CHECK(!IsTrajectoryFrozen(trajectory_id));
+    data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN;
   });
 }
 
 bool PoseGraph3D::IsTrajectoryFrozen(const int trajectory_id) const {
-  return data_.frozen_trajectories.count(trajectory_id) > 0;
+  return data_.trajectories_state.count(trajectory_id) != 0 &&
+         data_.trajectories_state.at(trajectory_id).state ==
+             TrajectoryState::FROZEN;
 }
 
 void PoseGraph3D::AddSubmapFromProto(
@@ -640,7 +649,7 @@ void PoseGraph3D::RunOptimization() {
   // data_.frozen_trajectories and data_.landmark_nodes when executing the
   // Solve. Solve is time consuming, so not taking the mutex before Solve to
   // avoid blocking foreground processing.
-  optimization_problem_->Solve(data_.constraints, data_.frozen_trajectories,
+  optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),
                                data_.landmark_nodes);
   common::MutexLocker locker(&mutex_);
 
@@ -705,6 +714,16 @@ MapById<NodeId, TrajectoryNodePose> PoseGraph3D::GetTrajectoryNodePoses()
   return node_poses;
 }
 
+std::map<int, PoseGraphInterface::TrajectoryState>
+PoseGraph3D::GetTrajectoryStates() const {
+  std::map<int, PoseGraphInterface::TrajectoryState> trajectories_state;
+  common::MutexLocker locker(&mutex_);
+  for (const auto& it : data_.trajectories_state) {
+    trajectories_state[it.first] = it.second.state;
+  }
+  return trajectories_state;
+}
+
 std::map<std::string, transform::Rigid3d> PoseGraph3D::GetLandmarkPoses()
     const {
   std::map<std::string, transform::Rigid3d> landmark_poses;

cartographer/mapping/internal/3d/pose_graph_3d.h

@@ -120,6 +120,8 @@ class PoseGraph3D : public PoseGraph {
       EXCLUDES(mutex_);
   MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() const override
       EXCLUDES(mutex_);
+  std::map<int, TrajectoryState> GetTrajectoryStates() const override
+      EXCLUDES(mutex_);
   std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override
       EXCLUDES(mutex_);
   void SetLandmarkPose(const std::string& landmark_id,

cartographer/mapping/internal/3d/pose_graph_3d_test.cc

@@ -39,7 +39,8 @@ class MockOptimizationProblem3D : public OptimizationProblem3D {
   ~MockOptimizationProblem3D() override = default;
 
   MOCK_METHOD3(Solve,
-               void(const std::vector<Constraint> &, const std::set<int> &,
+               void(const std::vector<Constraint> &,
+                    const std::map<int, PoseGraphInterface::TrajectoryState> &,
                     const std::map<std::string, LandmarkNode> &));
 };
 

cartographer/mapping/internal/optimization/optimization_problem_2d.cc

@@ -195,13 +195,21 @@ void OptimizationProblem2D::SetMaxNumIterations(
 
 void OptimizationProblem2D::Solve(
     const std::vector<Constraint>& constraints,
-    const std::set<int>& frozen_trajectories,
+    const std::map<int, PoseGraphInterface::TrajectoryState>&
+        trajectories_state,
     const std::map<std::string, LandmarkNode>& landmark_nodes) {
   if (node_data_.empty()) {
     // Nothing to optimize.
     return;
   }
 
+  std::set<int> frozen_trajectories;
+  for (const auto& it : trajectories_state) {
+    if (it.second == PoseGraphInterface::TrajectoryState::FROZEN) {
+      frozen_trajectories.insert(it.first);
+    }
+  }
+
   ceres::Problem::Options problem_options;
   ceres::Problem problem(problem_options);
 

cartographer/mapping/internal/optimization/optimization_problem_2d.h

@@ -79,7 +79,8 @@ class OptimizationProblem2D
 
   void Solve(
       const std::vector<Constraint>& constraints,
-      const std::set<int>& frozen_trajectories,
+      const std::map<int, PoseGraphInterface::TrajectoryState>&
+          trajectories_state,
       const std::map<std::string, LandmarkNode>& landmark_nodes) override;
 
   const MapById<NodeId, NodeSpec2D>& node_data() const override {

cartographer/mapping/internal/optimization/optimization_problem_3d.cc

@@ -258,13 +258,21 @@ void OptimizationProblem3D::SetMaxNumIterations(
 
 void OptimizationProblem3D::Solve(
     const std::vector<Constraint>& constraints,
-    const std::set<int>& frozen_trajectories,
+    const std::map<int, PoseGraphInterface::TrajectoryState>&
+        trajectories_state,
     const std::map<std::string, LandmarkNode>& landmark_nodes) {
   if (node_data_.empty()) {
     // Nothing to optimize.
     return;
   }
 
+  std::set<int> frozen_trajectories;
+  for (const auto& it : trajectories_state) {
+    if (it.second == PoseGraphInterface::TrajectoryState::FROZEN) {
+      frozen_trajectories.insert(it.first);
+    }
+  }
+
   ceres::Problem::Options problem_options;
   ceres::Problem problem(problem_options);
 

cartographer/mapping/internal/optimization/optimization_problem_3d.h

@@ -79,7 +79,8 @@ class OptimizationProblem3D
 
   void Solve(
       const std::vector<Constraint>& constraints,
-      const std::set<int>& frozen_trajectories,
+      const std::map<int, PoseGraphInterface::TrajectoryState>&
+          trajectories_state,
       const std::map<std::string, LandmarkNode>& landmark_nodes) override;
 
   const MapById<NodeId, NodeSpec3D>& node_data() const override {

cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc

@@ -171,8 +171,9 @@ TEST_F(OptimizationProblem3DTest, ReducesNoise) {
   optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform);
   optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform);
   optimization_problem_.AddSubmap(kTrajectoryId, kSubmap2Transform);
-  const std::set<int> kFrozen = {};
-  optimization_problem_.Solve(constraints, kFrozen, {});
+  const std::map<int, PoseGraphInterface::TrajectoryState> kTrajectoriesState =
+      {{kTrajectoryId, PoseGraphInterface::TrajectoryState::ACTIVE}};
+  optimization_problem_.Solve(constraints, kTrajectoriesState, {});
 
   double translation_error_after = 0.;
   double rotation_error_after = 0.;

cartographer/mapping/internal/optimization/optimization_problem_interface.h

@@ -69,7 +69,8 @@ class OptimizationProblemInterface {
   // Optimizes the global poses.
   virtual void Solve(
       const std::vector<Constraint>& constraints,
-      const std::set<int>& frozen_trajectories,
+      const std::map<int, PoseGraphInterface::TrajectoryState>&
+          trajectories_state,
       const std::map<std::string, LandmarkNode>& landmark_nodes) = 0;
 
   virtual const MapById<NodeId, NodeDataType>& node_data() const = 0;

cartographer/mapping/internal/testing/mock_pose_graph.h

@@ -43,6 +43,9 @@ class MockPoseGraph : public mapping::PoseGraphInterface {
   MOCK_CONST_METHOD0(
       GetTrajectoryNodePoses,
       mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose>());
+  MOCK_CONST_METHOD0(
+      GetTrajectoryStates,
+      std::map<int, mapping::PoseGraphInterface::TrajectoryState>());
   MOCK_CONST_METHOD0(GetLandmarkPoses,
                      std::map<std::string, transform::Rigid3d>());
   MOCK_METHOD2(SetLandmarkPose,

cartographer/mapping/pose_graph_data.h

@@ -35,6 +35,20 @@ namespace mapping {
 // transitions to kFinished, all nodes are tried to match against this submap.
 // Likewise, all new nodes are matched against submaps which are finished.
 enum class SubmapState { kActive, kFinished };
+
+struct InternalTrajectoryState {
+  // TODO(gaschler): Implement PoseGraphInterface::DeleteTrajectory.
+  enum class DeletionState {
+    NORMAL,
+    SCHEDULED_FOR_DELETION,
+    WAIT_FOR_DELETION
+  };
+
+  PoseGraphInterface::TrajectoryState state =
+      PoseGraphInterface::TrajectoryState::ACTIVE;
+  DeletionState deletion_state = DeletionState::NORMAL;
+};
+
 struct InternalSubmapData {
   std::shared_ptr<const Submap> submap;
   SubmapState state = SubmapState::kActive;
@@ -64,8 +78,7 @@ struct PoseGraphData {
   // How our various trajectories are related.
   TrajectoryConnectivityState trajectory_connectivity_state;
   int num_trajectory_nodes = 0;
-  std::set<int> finished_trajectories;
-  std::set<int> frozen_trajectories;
+  std::map<int, InternalTrajectoryState> trajectories_state;
 
   // Set of all initial trajectory poses.
   std::map<int, PoseGraph::InitialTrajectoryPose> initial_trajectory_poses;

cartographer/mapping/pose_graph_interface.h

@@ -79,6 +79,8 @@ class PoseGraphInterface {
     common::optional<transform::Rigid3d> fixed_frame_origin_in_map;
   };
 
+  enum class TrajectoryState { ACTIVE, FINISHED, FROZEN, DELETED };
+
   using GlobalSlamOptimizationCallback =
       std::function<void(const std::map<int /* trajectory_id */, SubmapId>&,
                          const std::map<int /* trajectory_id */, NodeId>&)>;
@@ -111,6 +113,9 @@ class PoseGraphInterface {
   virtual MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses()
       const = 0;
 
+  // Returns the states of trajectories.
+  virtual std::map<int, TrajectoryState> GetTrajectoryStates() const = 0;
+
   // Returns the current optimized landmark poses.
   virtual std::map<std::string, transform::Rigid3d> GetLandmarkPoses()
       const = 0;