From 54041d76eb388ef65e86070f63f438211e678ea2 Mon Sep 17 00:00:00 2001 From: gaschler Date: Tue, 19 Jun 2018 13:41:55 +0200 Subject: [PATCH] DeleteTrajectory (#1205) [RFC=0023](https://github.com/googlecartographer/rfcs/blob/master/text/0023-delete-load.md) --- .../cloud/internal/client/pose_graph_stub.cc | 4 + .../cloud/internal/client/pose_graph_stub.h | 1 + .../mapping/internal/2d/pose_graph_2d.cc | 81 +++++++++++++++++- .../mapping/internal/2d/pose_graph_2d.h | 7 ++ .../mapping/internal/3d/pose_graph_3d.cc | 83 ++++++++++++++++++- .../mapping/internal/3d/pose_graph_3d.h | 7 ++ .../internal/testing/mock_pose_graph.h | 1 + cartographer/mapping/map_builder_test.cc | 44 +++++++++- cartographer/mapping/pose_graph_data.h | 1 - cartographer/mapping/pose_graph_interface.h | 3 + 10 files changed, 226 insertions(+), 6 deletions(-) diff --git a/cartographer/cloud/internal/client/pose_graph_stub.cc b/cartographer/cloud/internal/client/pose_graph_stub.cc index f37e541..f1b9450 100644 --- a/cartographer/cloud/internal/client/pose_graph_stub.cc +++ b/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); diff --git a/cartographer/cloud/internal/client/pose_graph_stub.h b/cartographer/cloud/internal/client/pose_graph_stub.h index 0edc450..e46e92d 100644 --- a/cartographer/cloud/internal/client/pose_graph_stub.h +++ b/cartographer/cloud/internal/client/pose_graph_stub.h @@ -45,6 +45,7 @@ class PoseGraphStub : public ::cartographer::mapping::PoseGraphInterface { std::map 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 GetTrajectoryData() diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index a35e507..65e9195 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/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) - EXCLUDES(mutex_) { + const sensor::LandmarkData& landmark_data) { 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([¬ification]() { 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 PoseGraph2D::GetTrajectoryNodes() const { common::MutexLocker locker(&mutex_); return data_.trajectory_nodes; diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h index a6eadcf..b81770b 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.h +++ b/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( diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index 367e985..31bfca7 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/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) - EXCLUDES(mutex_) { + const sensor::LandmarkData& landmark_data) { 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([¬ification]() { 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 PoseGraph3D::GetTrajectoryNodes() const { common::MutexLocker locker(&mutex_); return data_.trajectory_nodes; diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h index 6afb130..51b396e 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.h +++ b/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( diff --git a/cartographer/mapping/internal/testing/mock_pose_graph.h b/cartographer/mapping/internal/testing/mock_pose_graph.h index 32ecd3e..348ddf8 100644 --- a/cartographer/mapping/internal/testing/mock_pose_graph.h +++ b/cartographer/mapping/internal/testing/mock_pose_graph.h @@ -50,6 +50,7 @@ class MockPoseGraph : public mapping::PoseGraphInterface { std::map()); 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( diff --git a/cartographer/mapping/map_builder_test.cc b/cartographer/mapping/map_builder_test.cc index e7ea9b6..de02af6 100644 --- a/cartographer/mapping/map_builder_test.cc +++ b/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) { diff --git a/cartographer/mapping/pose_graph_data.h b/cartographer/mapping/pose_graph_data.h index 237b102..6d2ffbd 100644 --- a/cartographer/mapping/pose_graph_data.h +++ b/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, diff --git a/cartographer/mapping/pose_graph_interface.h b/cartographer/mapping/pose_graph_interface.h index 1912b9d..5e7bdcc 100644 --- a/cartographer/mapping/pose_graph_interface.h +++ b/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;