DeleteTrajectory (#1205)

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

cartographer/cloud/internal/client/pose_graph_stub.cc

@@ -135,6 +135,10 @@ void PoseGraphStub::SetLandmarkPose(const std::string& landmark_id,
   CHECK(client.Write(request));
 }
 
+void PoseGraphStub::DeleteTrajectory(int trajectory_id) {
+  LOG(FATAL) << "not implemented";
+}
+
 bool PoseGraphStub::IsTrajectoryFinished(int trajectory_id) const {
   proto::IsTrajectoryFinishedRequest request;
   request.set_trajectory_id(trajectory_id);

cartographer/cloud/internal/client/pose_graph_stub.h

@@ -45,6 +45,7 @@ class PoseGraphStub : public ::cartographer::mapping::PoseGraphInterface {
   std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override;
   void SetLandmarkPose(const std::string& landmark_id,
                        const transform::Rigid3d& global_pose) override;
+  void DeleteTrajectory(int trajectory_id) override;
   bool IsTrajectoryFinished(int trajectory_id) const override;
   bool IsTrajectoryFrozen(int trajectory_id) const override;
   std::map<int, mapping::PoseGraphInterface::TrajectoryData> GetTrajectoryData()

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

@@ -113,6 +113,9 @@ NodeId PoseGraph2D::AddNode(
 
   common::MutexLocker locker(&mutex_);
   AddTrajectoryIfNeeded(trajectory_id);
+  if (!CanAddWorkItemModifying(trajectory_id)) {
+    LOG(WARNING) << "AddNode was called for finished or deleted trajectory.";
+  }
   const NodeId node_id = data_.trajectory_nodes.Append(
       trajectory_id, TrajectoryNode{constant_data, optimized_pose});
   ++data_.num_trajectory_nodes;
@@ -152,6 +155,8 @@ void PoseGraph2D::AddTrajectoryIfNeeded(const int trajectory_id) {
         TrajectoryState::FINISHED);
   CHECK(data_.trajectories_state.at(trajectory_id).state !=
         TrajectoryState::DELETED);
+  CHECK(data_.trajectories_state.at(trajectory_id).deletion_state ==
+        InternalTrajectoryState::DeletionState::NORMAL);
   data_.trajectory_connectivity_state.Add(trajectory_id);
   // Make sure we have a sampler for this trajectory.
   if (!global_localization_samplers_[trajectory_id]) {
@@ -164,6 +169,7 @@ void PoseGraph2D::AddTrajectoryIfNeeded(const int trajectory_id) {
 void PoseGraph2D::AddImuData(const int trajectory_id,
                              const sensor::ImuData& imu_data) {
   common::MutexLocker locker(&mutex_);
+  if (!CanAddWorkItemModifying(trajectory_id)) return;
   AddWorkItem([=]() REQUIRES(mutex_) {
     optimization_problem_->AddImuData(trajectory_id, imu_data);
   });
@@ -172,6 +178,7 @@ void PoseGraph2D::AddImuData(const int trajectory_id,
 void PoseGraph2D::AddOdometryData(const int trajectory_id,
                                   const sensor::OdometryData& odometry_data) {
   common::MutexLocker locker(&mutex_);
+  if (!CanAddWorkItemModifying(trajectory_id)) return;
   AddWorkItem([=]() REQUIRES(mutex_) {
     optimization_problem_->AddOdometryData(trajectory_id, odometry_data);
   });
@@ -184,9 +191,9 @@ void PoseGraph2D::AddFixedFramePoseData(
 }
 
 void PoseGraph2D::AddLandmarkData(int trajectory_id,
-                                  const sensor::LandmarkData& landmark_data)
+                                  const sensor::LandmarkData& landmark_data) {
-    EXCLUDES(mutex_) {
   common::MutexLocker locker(&mutex_);
+  if (!CanAddWorkItemModifying(trajectory_id)) return;
   AddWorkItem([=]() REQUIRES(mutex_) {
     for (const auto& observation : landmark_data.landmark_observations) {
       data_.landmark_nodes[observation.id].landmark_observations.emplace_back(
@@ -284,6 +291,8 @@ void PoseGraph2D::ComputeConstraintsForNode(
                    Constraint::INTRA_SUBMAP});
   }
 
+  // TODO(gaschler): Consider not searching for constraints against trajectories
+  // scheduled for deletion.
   for (const auto& submap_id_data : data_.submap_data) {
     if (submap_id_data.data.state == SubmapState::kFinished) {
       CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0);
@@ -342,6 +351,23 @@ void PoseGraph2D::UpdateTrajectoryConnectivity(const Constraint& constraint) {
       time);
 }
 
+void PoseGraph2D::DeleteTrajectoriesIfNeeded() {
+  TrimmingHandle trimming_handle(this);
+  for (auto& it : data_.trajectories_state) {
+    if (it.second.deletion_state ==
+        InternalTrajectoryState::DeletionState::WAIT_FOR_DELETION) {
+      // TODO(gaschler): Consider directly deleting from data_, which may be
+      // more complete.
+      auto submap_ids = trimming_handle.GetSubmapIds(it.first);
+      for (auto& submap_id : submap_ids) {
+        trimming_handle.TrimSubmap(submap_id);
+      }
+      it.second.state = TrajectoryState::DELETED;
+      it.second.deletion_state = InternalTrajectoryState::DeletionState::NORMAL;
+    }
+  }
+}
+
 void PoseGraph2D::HandleWorkQueue(
     const constraints::ConstraintBuilder2D::Result& result) {
   {
@@ -374,6 +400,7 @@ void PoseGraph2D::HandleWorkQueue(
   for (const Constraint& constraint : result) {
     UpdateTrajectoryConnectivity(constraint);
   }
+  DeleteTrajectoriesIfNeeded();
   TrimmingHandle trimming_handle(this);
   for (auto& trimmer : trimmers_) {
     trimmer->Trim(&trimming_handle);
@@ -438,6 +465,22 @@ void PoseGraph2D::WaitForAllComputations() {
   locker.Await([&notification]() { return notification; });
 }
 
+void PoseGraph2D::DeleteTrajectory(const int trajectory_id) {
+  common::MutexLocker locker(&mutex_);
+  data_.trajectories_state.at(trajectory_id).deletion_state =
+      InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION;
+  AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
+    CHECK(data_.trajectories_state.at(trajectory_id).state !=
+          TrajectoryState::ACTIVE);
+    CHECK(data_.trajectories_state.at(trajectory_id).state !=
+          TrajectoryState::DELETED);
+    CHECK(data_.trajectories_state.at(trajectory_id).deletion_state ==
+          InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION);
+    data_.trajectories_state.at(trajectory_id).deletion_state =
+        InternalTrajectoryState::DeletionState::WAIT_FOR_DELETION;
+  });
+}
+
 void PoseGraph2D::FinishTrajectory(const int trajectory_id) {
   common::MutexLocker locker(&mutex_);
   AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
@@ -488,6 +531,7 @@ void PoseGraph2D::AddSubmapFromProto(
 
   common::MutexLocker locker(&mutex_);
   AddTrajectoryIfNeeded(submap_id.trajectory_id);
+  if (!CanAddWorkItemModifying(submap_id.trajectory_id)) return;
   data_.submap_data.Insert(submap_id, InternalSubmapData());
   data_.submap_data.at(submap_id).submap = submap_ptr;
   // Immediately show the submap at the 'global_submap_pose'.
@@ -508,6 +552,7 @@ void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose,
 
   common::MutexLocker locker(&mutex_);
   AddTrajectoryIfNeeded(node_id.trajectory_id);
+  if (!CanAddWorkItemModifying(node_id.trajectory_id)) return;
   data_.trajectory_nodes.Insert(node_id,
                                 TrajectoryNode{constant_data, global_pose});
 
@@ -535,6 +580,7 @@ void PoseGraph2D::SetTrajectoryDataFromProto(
 void PoseGraph2D::AddNodeToSubmap(const NodeId& node_id,
                                   const SubmapId& submap_id) {
   common::MutexLocker locker(&mutex_);
+  if (!CanAddWorkItemModifying(submap_id.trajectory_id)) return;
   AddWorkItem([this, node_id, submap_id]() REQUIRES(mutex_) {
     data_.submap_data.at(submap_id).node_ids.insert(node_id);
   });
@@ -649,6 +695,37 @@ void PoseGraph2D::RunOptimization() {
   data_.global_submap_poses_2d = submap_data;
 }
 
+bool PoseGraph2D::CanAddWorkItemModifying(int trajectory_id) {
+  auto it = data_.trajectories_state.find(trajectory_id);
+  if (it == data_.trajectories_state.end()) {
+    LOG(WARNING) << "trajectory_id:" << trajectory_id
+                 << " has not been added "
+                    "but modification is requested.";
+    return true;
+  }
+  if (it->second.state == TrajectoryState::FINISHED) {
+    // TODO(gaschler): Replace all FATAL to WARNING after some testing.
+    LOG(FATAL) << "trajectory_id " << trajectory_id
+               << " has finished "
+                  "but modification is requested, skipping.";
+    return false;
+  }
+  if (it->second.deletion_state !=
+      InternalTrajectoryState::DeletionState::NORMAL) {
+    LOG(FATAL) << "trajectory_id " << trajectory_id
+               << " has been scheduled for deletion "
+                  "but modification is requested, skipping.";
+    return false;
+  }
+  if (it->second.state == TrajectoryState::DELETED) {
+    LOG(FATAL) << "trajectory_id " << trajectory_id
+               << " has been deleted "
+                  "but modification is requested, skipping.";
+    return false;
+  }
+  return true;
+}
+
 MapById<NodeId, TrajectoryNode> PoseGraph2D::GetTrajectoryNodes() const {
   common::MutexLocker locker(&mutex_);
   return data_.trajectory_nodes;

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

@@ -93,6 +93,7 @@ class PoseGraph2D : public PoseGraph {
                        const sensor::LandmarkData& landmark_data) override
       EXCLUDES(mutex_);
 
+  void DeleteTrajectory(int trajectory_id) override;
   void FinishTrajectory(int trajectory_id) override;
   bool IsTrajectoryFinished(int trajectory_id) const override REQUIRES(mutex_);
   void FreezeTrajectory(int trajectory_id) override;
@@ -179,6 +180,10 @@ class PoseGraph2D : public PoseGraph {
   void ComputeConstraintsForOldNodes(const SubmapId& submap_id)
       REQUIRES(mutex_);
 
+  // Deletes trajectories waiting for deletion. Must not be called during
+  // constraint search.
+  void DeleteTrajectoriesIfNeeded() REQUIRES(mutex_);
+
   // Runs the optimization, executes the trimmers and processes the work queue.
   void HandleWorkQueue(const constraints::ConstraintBuilder2D::Result& result)
       REQUIRES(mutex_);
@@ -191,6 +196,8 @@ class PoseGraph2D : public PoseGraph {
   // optimization being run at a time.
   void RunOptimization() EXCLUDES(mutex_);
 
+  bool CanAddWorkItemModifying(int trajectory_id) REQUIRES(mutex_);
+
   // Computes the local to global map frame transform based on the given
   // 'global_submap_poses'.
   transform::Rigid3d ComputeLocalToGlobalTransform(

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

@@ -111,6 +111,9 @@ NodeId PoseGraph3D::AddNode(
 
   common::MutexLocker locker(&mutex_);
   AddTrajectoryIfNeeded(trajectory_id);
+  if (!CanAddWorkItemModifying(trajectory_id)) {
+    LOG(WARNING) << "AddNode was called for finished or deleted trajectory.";
+  }
   const NodeId node_id = data_.trajectory_nodes.Append(
       trajectory_id, TrajectoryNode{constant_data, optimized_pose});
   ++data_.num_trajectory_nodes;
@@ -150,6 +153,8 @@ void PoseGraph3D::AddTrajectoryIfNeeded(const int trajectory_id) {
         TrajectoryState::FINISHED);
   CHECK(data_.trajectories_state.at(trajectory_id).state !=
         TrajectoryState::DELETED);
+  CHECK(data_.trajectories_state.at(trajectory_id).deletion_state ==
+        InternalTrajectoryState::DeletionState::NORMAL);
   data_.trajectory_connectivity_state.Add(trajectory_id);
   // Make sure we have a sampler for this trajectory.
   if (!global_localization_samplers_[trajectory_id]) {
@@ -162,6 +167,7 @@ void PoseGraph3D::AddTrajectoryIfNeeded(const int trajectory_id) {
 void PoseGraph3D::AddImuData(const int trajectory_id,
                              const sensor::ImuData& imu_data) {
   common::MutexLocker locker(&mutex_);
+  if (!CanAddWorkItemModifying(trajectory_id)) return;
   AddWorkItem([=]() REQUIRES(mutex_) {
     optimization_problem_->AddImuData(trajectory_id, imu_data);
   });
@@ -170,6 +176,7 @@ void PoseGraph3D::AddImuData(const int trajectory_id,
 void PoseGraph3D::AddOdometryData(const int trajectory_id,
                                   const sensor::OdometryData& odometry_data) {
   common::MutexLocker locker(&mutex_);
+  if (!CanAddWorkItemModifying(trajectory_id)) return;
   AddWorkItem([=]() REQUIRES(mutex_) {
     optimization_problem_->AddOdometryData(trajectory_id, odometry_data);
   });
@@ -179,6 +186,7 @@ void PoseGraph3D::AddFixedFramePoseData(
     const int trajectory_id,
     const sensor::FixedFramePoseData& fixed_frame_pose_data) {
   common::MutexLocker locker(&mutex_);
+  if (!CanAddWorkItemModifying(trajectory_id)) return;
   AddWorkItem([=]() REQUIRES(mutex_) {
     optimization_problem_->AddFixedFramePoseData(trajectory_id,
                                                  fixed_frame_pose_data);
@@ -186,9 +194,9 @@ void PoseGraph3D::AddFixedFramePoseData(
 }
 
 void PoseGraph3D::AddLandmarkData(int trajectory_id,
-                                  const sensor::LandmarkData& landmark_data)
+                                  const sensor::LandmarkData& landmark_data) {
-    EXCLUDES(mutex_) {
   common::MutexLocker locker(&mutex_);
+  if (!CanAddWorkItemModifying(trajectory_id)) return;
   AddWorkItem([=]() REQUIRES(mutex_) {
     for (const auto& observation : landmark_data.landmark_observations) {
       data_.landmark_nodes[observation.id].landmark_observations.emplace_back(
@@ -299,6 +307,8 @@ void PoseGraph3D::ComputeConstraintsForNode(
                    Constraint::INTRA_SUBMAP});
   }
 
+  // TODO(gaschler): Consider not searching for constraints against trajectories
+  // scheduled for deletion.
   for (const auto& submap_id_data : data_.submap_data) {
     if (submap_id_data.data.state == SubmapState::kFinished) {
       CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0);
@@ -358,6 +368,23 @@ void PoseGraph3D::UpdateTrajectoryConnectivity(const Constraint& constraint) {
       time);
 }
 
+void PoseGraph3D::DeleteTrajectoriesIfNeeded() {
+  TrimmingHandle trimming_handle(this);
+  for (auto& it : data_.trajectories_state) {
+    if (it.second.deletion_state ==
+        InternalTrajectoryState::DeletionState::WAIT_FOR_DELETION) {
+      // TODO(gaschler): Consider directly deleting from data_, which may be
+      // more complete.
+      auto submap_ids = trimming_handle.GetSubmapIds(it.first);
+      for (auto& submap_id : submap_ids) {
+        trimming_handle.TrimSubmap(submap_id);
+      }
+      it.second.state = TrajectoryState::DELETED;
+      it.second.deletion_state = InternalTrajectoryState::DeletionState::NORMAL;
+    }
+  }
+}
+
 void PoseGraph3D::HandleWorkQueue(
     const constraints::ConstraintBuilder3D::Result& result) {
   {
@@ -390,6 +417,7 @@ void PoseGraph3D::HandleWorkQueue(
   for (const Constraint& constraint : result) {
     UpdateTrajectoryConnectivity(constraint);
   }
+  DeleteTrajectoriesIfNeeded();
   TrimmingHandle trimming_handle(this);
   for (auto& trimmer : trimmers_) {
     trimmer->Trim(&trimming_handle);
@@ -454,6 +482,22 @@ void PoseGraph3D::WaitForAllComputations() {
   locker.Await([&notification]() { return notification; });
 }
 
+void PoseGraph3D::DeleteTrajectory(const int trajectory_id) {
+  common::MutexLocker locker(&mutex_);
+  data_.trajectories_state.at(trajectory_id).deletion_state =
+      InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION;
+  AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
+    CHECK(data_.trajectories_state.at(trajectory_id).state !=
+          TrajectoryState::ACTIVE);
+    CHECK(data_.trajectories_state.at(trajectory_id).state !=
+          TrajectoryState::DELETED);
+    CHECK(data_.trajectories_state.at(trajectory_id).deletion_state ==
+          InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION);
+    data_.trajectories_state.at(trajectory_id).deletion_state =
+        InternalTrajectoryState::DeletionState::WAIT_FOR_DELETION;
+  });
+}
+
 void PoseGraph3D::FinishTrajectory(const int trajectory_id) {
   common::MutexLocker locker(&mutex_);
   AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
@@ -502,6 +546,7 @@ void PoseGraph3D::AddSubmapFromProto(
 
   common::MutexLocker locker(&mutex_);
   AddTrajectoryIfNeeded(submap_id.trajectory_id);
+  if (!CanAddWorkItemModifying(submap_id.trajectory_id)) return;
   data_.submap_data.Insert(submap_id, InternalSubmapData());
   data_.submap_data.at(submap_id).submap = submap_ptr;
   // Immediately show the submap at the 'global_submap_pose'.
@@ -522,6 +567,7 @@ void PoseGraph3D::AddNodeFromProto(const transform::Rigid3d& global_pose,
 
   common::MutexLocker locker(&mutex_);
   AddTrajectoryIfNeeded(node_id.trajectory_id);
+  if (!CanAddWorkItemModifying(node_id.trajectory_id)) return;
   data_.trajectory_nodes.Insert(node_id,
                                 TrajectoryNode{constant_data, global_pose});
 
@@ -549,6 +595,7 @@ void PoseGraph3D::SetTrajectoryDataFromProto(
 
   const int trajectory_id = data.trajectory_id();
   common::MutexLocker locker(&mutex_);
+  if (!CanAddWorkItemModifying(trajectory_id)) return;
   AddWorkItem([this, trajectory_id, trajectory_data]() REQUIRES(mutex_) {
     optimization_problem_->SetTrajectoryData(trajectory_id, trajectory_data);
   });
@@ -557,6 +604,7 @@ void PoseGraph3D::SetTrajectoryDataFromProto(
 void PoseGraph3D::AddNodeToSubmap(const NodeId& node_id,
                                   const SubmapId& submap_id) {
   common::MutexLocker locker(&mutex_);
+  if (!CanAddWorkItemModifying(submap_id.trajectory_id)) return;
   AddWorkItem([this, node_id, submap_id]() REQUIRES(mutex_) {
     data_.submap_data.at(submap_id).node_ids.insert(node_id);
   });
@@ -691,6 +739,37 @@ void PoseGraph3D::RunOptimization() {
   }
 }
 
+bool PoseGraph3D::CanAddWorkItemModifying(int trajectory_id) {
+  auto it = data_.trajectories_state.find(trajectory_id);
+  if (it == data_.trajectories_state.end()) {
+    LOG(WARNING) << "trajectory_id:" << trajectory_id
+                 << " has not been added "
+                    "but modification is requested.";
+    return true;
+  }
+  if (it->second.state == TrajectoryState::FINISHED) {
+    // TODO(gaschler): Replace all FATAL to WARNING after some testing.
+    LOG(FATAL) << "trajectory_id " << trajectory_id
+               << " has finished "
+                  "but modification is requested, skipping.";
+    return false;
+  }
+  if (it->second.deletion_state !=
+      InternalTrajectoryState::DeletionState::NORMAL) {
+    LOG(FATAL) << "trajectory_id " << trajectory_id
+               << " has been scheduled for deletion "
+                  "but modification is requested, skipping.";
+    return false;
+  }
+  if (it->second.state == TrajectoryState::DELETED) {
+    LOG(FATAL) << "trajectory_id " << trajectory_id
+               << " has been deleted "
+                  "but modification is requested, skipping.";
+    return false;
+  }
+  return true;
+}
+
 MapById<NodeId, TrajectoryNode> PoseGraph3D::GetTrajectoryNodes() const {
   common::MutexLocker locker(&mutex_);
   return data_.trajectory_nodes;

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

@@ -92,6 +92,7 @@ class PoseGraph3D : public PoseGraph {
                        const sensor::LandmarkData& landmark_data) override
       EXCLUDES(mutex_);
 
+  void DeleteTrajectory(int trajectory_id) override;
   void FinishTrajectory(int trajectory_id) override;
   bool IsTrajectoryFinished(int trajectory_id) const override REQUIRES(mutex_);
   void FreezeTrajectory(int trajectory_id) override;
@@ -182,6 +183,10 @@ class PoseGraph3D : public PoseGraph {
   void ComputeConstraintsForOldNodes(const SubmapId& submap_id)
       REQUIRES(mutex_);
 
+  // Deletes trajectories waiting for deletion. Must not be called during
+  // constraint search.
+  void DeleteTrajectoriesIfNeeded() REQUIRES(mutex_);
+
   // Runs the optimization, executes the trimmers and processes the work queue.
   void HandleWorkQueue(const constraints::ConstraintBuilder3D::Result& result)
       REQUIRES(mutex_);
@@ -190,6 +195,8 @@ class PoseGraph3D : public PoseGraph {
   // optimization being run at a time.
   void RunOptimization() EXCLUDES(mutex_);
 
+  bool CanAddWorkItemModifying(int trajectory_id) REQUIRES(mutex_);
+
   // Computes the local to global map frame transform based on the given
   // 'global_submap_poses'.
   transform::Rigid3d ComputeLocalToGlobalTransform(

cartographer/mapping/internal/testing/mock_pose_graph.h

@@ -50,6 +50,7 @@ class MockPoseGraph : public mapping::PoseGraphInterface {
                      std::map<std::string, transform::Rigid3d>());
   MOCK_METHOD2(SetLandmarkPose,
                void(const std::string&, const transform::Rigid3d&));
+  MOCK_METHOD1(DeleteTrajectory, void(int));
   MOCK_CONST_METHOD1(IsTrajectoryFinished, bool(int));
   MOCK_CONST_METHOD1(IsTrajectoryFrozen, bool(int));
   MOCK_CONST_METHOD0(

cartographer/mapping/map_builder_test.cc

@@ -89,7 +89,7 @@ class MapBuilderTest : public ::testing::Test {
     };
   }
 
-  int CreateTrajectoryWithFakeData() {
+  int CreateTrajectoryWithFakeData(double timestamps_add_duration = 0.) {
     int trajectory_id = map_builder_->AddTrajectoryBuilder(
         {kRangeSensorId}, trajectory_builder_options_,
         GetLocalSlamResultCallback());
@@ -98,6 +98,7 @@ class MapBuilderTest : public ::testing::Test {
     auto measurements = testing::GenerateFakeRangeMeasurements(
         kTravelDistance, kDuration, kTimeStep);
     for (auto& measurement : measurements) {
+      measurement.time += common::FromSeconds(timestamps_add_duration);
       trajectory_builder->AddSensorData(kRangeSensorId.id, measurement);
     }
     map_builder_->FinishTrajectory(trajectory_id);
@@ -267,6 +268,47 @@ TEST_F(MapBuilderTest, GlobalSlam3D) {
               0.1 * kTravelDistance);
 }
 
+TEST_F(MapBuilderTest, DeleteFinishedTrajectory2D) {
+  SetOptionsEnableGlobalOptimization();
+  BuildMapBuilder();
+  int trajectory_id = CreateTrajectoryWithFakeData();
+  map_builder_->pose_graph()->RunFinalOptimization();
+  EXPECT_TRUE(map_builder_->pose_graph()->IsTrajectoryFinished(trajectory_id));
+  EXPECT_GE(map_builder_->pose_graph()->constraints().size(), 50);
+  EXPECT_GE(
+      map_builder_->pose_graph()->GetTrajectoryNodes().SizeOfTrajectoryOrZero(
+          trajectory_id),
+      20);
+  EXPECT_GE(
+      map_builder_->pose_graph()->GetAllSubmapData().SizeOfTrajectoryOrZero(
+          trajectory_id),
+      5);
+  map_builder_->pose_graph()->DeleteTrajectory(trajectory_id);
+  int another_trajectory_id = CreateTrajectoryWithFakeData(100.);
+  map_builder_->pose_graph()->RunFinalOptimization();
+  EXPECT_TRUE(
+      map_builder_->pose_graph()->IsTrajectoryFinished(another_trajectory_id));
+  EXPECT_EQ(
+      map_builder_->pose_graph()->GetTrajectoryNodes().SizeOfTrajectoryOrZero(
+          trajectory_id),
+      0);
+  EXPECT_EQ(
+      map_builder_->pose_graph()->GetAllSubmapData().SizeOfTrajectoryOrZero(
+          trajectory_id),
+      0);
+  map_builder_->pose_graph()->DeleteTrajectory(another_trajectory_id);
+  map_builder_->pose_graph()->RunFinalOptimization();
+  EXPECT_EQ(map_builder_->pose_graph()->constraints().size(), 0);
+  EXPECT_EQ(
+      map_builder_->pose_graph()->GetTrajectoryNodes().SizeOfTrajectoryOrZero(
+          another_trajectory_id),
+      0);
+  EXPECT_EQ(
+      map_builder_->pose_graph()->GetAllSubmapData().SizeOfTrajectoryOrZero(
+          another_trajectory_id),
+      0);
+}
+
 TEST_F(MapBuilderTest, SaveLoadState) {
   for (int dimensions : {2, 3}) {
     if (dimensions == 3) {

cartographer/mapping/pose_graph_data.h

@@ -37,7 +37,6 @@ namespace mapping {
 enum class SubmapState { kActive, kFinished };
 
 struct InternalTrajectoryState {
-  // TODO(gaschler): Implement PoseGraphInterface::DeleteTrajectory.
   enum class DeletionState {
     NORMAL,
     SCHEDULED_FOR_DELETION,

cartographer/mapping/pose_graph_interface.h

@@ -124,6 +124,9 @@ class PoseGraphInterface {
   virtual void SetLandmarkPose(const std::string& landmark_id,
                                const transform::Rigid3d& global_pose) = 0;
 
+  // Deletes a trajectory asynchronously.
+  virtual void DeleteTrajectory(int trajectory_id) = 0;
+
   // Checks if the given trajectory is finished.
   virtual bool IsTrajectoryFinished(int trajectory_id) const = 0;