12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

DeleteTrajectory (#1205) 

[RFC=0023](https://github.com/googlecartographer/rfcs/blob/master/text/0023-delete-load.md)
master
gaschler 2018-06-19 13:41:55 +02:00 committed by Wally B. Feed
parent ecaa95f3b0
commit 54041d76eb
10 changed files with 226 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cartographer/cloud/internal/client/pose_graph_stub.cc
View File

@ -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);

1
cartographer/cloud/internal/client/pose_graph_stub.h
View File

@ -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()

81
cartographer/mapping/internal/2d/pose_graph_2d.cc
View File

@ -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([&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;

7
cartographer/mapping/internal/2d/pose_graph_2d.h
View File

@ -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(

83
cartographer/mapping/internal/3d/pose_graph_3d.cc
View File

@ -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([&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;

7
cartographer/mapping/internal/3d/pose_graph_3d.h
View File

@ -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(

1
cartographer/mapping/internal/testing/mock_pose_graph.h
View File

@ -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(

44
cartographer/mapping/map_builder_test.cc
View File

@ -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) {

1
cartographer/mapping/pose_graph_data.h
View File

@ -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,

3
cartographer/mapping/pose_graph_interface.h
View File

@ -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;