Trim all submaps when pure localization trajectory is finished. (#563)
Fixes #560 based on #562master
parent
bebe021b04
commit
88805a301d
|
@ -100,6 +100,9 @@ class PoseGraph {
|
||||||
// Finishes the given trajectory.
|
// Finishes the given trajectory.
|
||||||
virtual void FinishTrajectory(int trajectory_id) = 0;
|
virtual void FinishTrajectory(int trajectory_id) = 0;
|
||||||
|
|
||||||
|
// Checks if the given trajectory is finished.
|
||||||
|
virtual bool IsTrajectoryFinished(int trajectory_id) = 0;
|
||||||
|
|
||||||
// Freezes a trajectory. Poses in this trajectory will not be optimized.
|
// Freezes a trajectory. Poses in this trajectory will not be optimized.
|
||||||
virtual void FreezeTrajectory(int trajectory_id) = 0;
|
virtual void FreezeTrajectory(int trajectory_id) = 0;
|
||||||
|
|
||||||
|
|
|
@ -28,13 +28,23 @@ PureLocalizationTrimmer::PureLocalizationTrimmer(const int trajectory_id,
|
||||||
}
|
}
|
||||||
|
|
||||||
void PureLocalizationTrimmer::Trim(Trimmable* const pose_graph) {
|
void PureLocalizationTrimmer::Trim(Trimmable* const pose_graph) {
|
||||||
|
if (pose_graph->IsFinished(trajectory_id_)) {
|
||||||
|
num_submaps_to_keep_ = 0;
|
||||||
|
}
|
||||||
|
|
||||||
while (pose_graph->num_submaps(trajectory_id_) > num_submaps_to_keep_) {
|
while (pose_graph->num_submaps(trajectory_id_) > num_submaps_to_keep_) {
|
||||||
const int submap_index_to_trim_next = num_submaps_trimmed_;
|
const int submap_index_to_trim_next = num_submaps_trimmed_;
|
||||||
pose_graph->MarkSubmapAsTrimmed(
|
pose_graph->MarkSubmapAsTrimmed(
|
||||||
SubmapId{trajectory_id_, submap_index_to_trim_next});
|
SubmapId{trajectory_id_, submap_index_to_trim_next});
|
||||||
++num_submaps_trimmed_;
|
++num_submaps_trimmed_;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (num_submaps_to_keep_ == 0) {
|
||||||
|
finished_ = true;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool PureLocalizationTrimmer::IsFinished() { return finished_; }
|
||||||
|
|
||||||
} // namespace mapping
|
} // namespace mapping
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
|
@ -36,6 +36,9 @@ class Trimmable {
|
||||||
// will no longer take part in scan matching, loop closure, visualization.
|
// will no longer take part in scan matching, loop closure, visualization.
|
||||||
// Submaps and nodes are only marked, the numbering remains unchanged.
|
// Submaps and nodes are only marked, the numbering remains unchanged.
|
||||||
virtual void MarkSubmapAsTrimmed(const SubmapId& submap_id) = 0;
|
virtual void MarkSubmapAsTrimmed(const SubmapId& submap_id) = 0;
|
||||||
|
|
||||||
|
// Checks if the given trajectory is finished or not.
|
||||||
|
virtual bool IsFinished(int trajectory_id) const = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
// An interface to implement algorithms that choose how to trim the pose graph.
|
// An interface to implement algorithms that choose how to trim the pose graph.
|
||||||
|
@ -45,6 +48,9 @@ class PoseGraphTrimmer {
|
||||||
|
|
||||||
// Called once after each pose graph optimization.
|
// Called once after each pose graph optimization.
|
||||||
virtual void Trim(Trimmable* pose_graph) = 0;
|
virtual void Trim(Trimmable* pose_graph) = 0;
|
||||||
|
|
||||||
|
// Checks if this trimmer is in a terminatable state.
|
||||||
|
virtual bool IsFinished() = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
// Keeps the last 'num_submaps_to_keep' of the trajectory with 'trajectory_id'
|
// Keeps the last 'num_submaps_to_keep' of the trajectory with 'trajectory_id'
|
||||||
|
@ -55,11 +61,13 @@ class PureLocalizationTrimmer : public PoseGraphTrimmer {
|
||||||
~PureLocalizationTrimmer() override {}
|
~PureLocalizationTrimmer() override {}
|
||||||
|
|
||||||
void Trim(Trimmable* pose_graph) override;
|
void Trim(Trimmable* pose_graph) override;
|
||||||
|
bool IsFinished() override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
const int trajectory_id_;
|
const int trajectory_id_;
|
||||||
const int num_submaps_to_keep_;
|
int num_submaps_to_keep_;
|
||||||
int num_submaps_trimmed_ = 0;
|
int num_submaps_trimmed_ = 0;
|
||||||
|
bool finished_ = false;
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace mapping
|
} // namespace mapping
|
||||||
|
|
|
@ -29,7 +29,7 @@ class FakePoseGraph : public Trimmable {
|
||||||
public:
|
public:
|
||||||
~FakePoseGraph() override {}
|
~FakePoseGraph() override {}
|
||||||
|
|
||||||
int num_submaps(int trajectory_id) const override {
|
int num_submaps(const int trajectory_id) const override {
|
||||||
return 17 - trimmed_submaps_.size();
|
return 17 - trimmed_submaps_.size();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -37,6 +37,8 @@ class FakePoseGraph : public Trimmable {
|
||||||
trimmed_submaps_.push_back(submap_id);
|
trimmed_submaps_.push_back(submap_id);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool IsFinished(const int trajectory_id) const override { return false; }
|
||||||
|
|
||||||
std::vector<SubmapId> trimmed_submaps() { return trimmed_submaps_; }
|
std::vector<SubmapId> trimmed_submaps() { return trimmed_submaps_; }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
|
@ -320,6 +320,13 @@ void PoseGraph::HandleWorkQueue() {
|
||||||
for (auto& trimmer : trimmers_) {
|
for (auto& trimmer : trimmers_) {
|
||||||
trimmer->Trim(&trimming_handle);
|
trimmer->Trim(&trimming_handle);
|
||||||
}
|
}
|
||||||
|
trimmers_.erase(
|
||||||
|
std::remove_if(
|
||||||
|
trimmers_.begin(), trimmers_.end(),
|
||||||
|
[](std::unique_ptr<mapping::PoseGraphTrimmer>& trimmer) {
|
||||||
|
return trimmer->IsFinished();
|
||||||
|
}),
|
||||||
|
trimmers_.end());
|
||||||
|
|
||||||
num_nodes_since_last_loop_closure_ = 0;
|
num_nodes_since_last_loop_closure_ = 0;
|
||||||
run_loop_closure_ = false;
|
run_loop_closure_ = false;
|
||||||
|
@ -369,8 +376,21 @@ void PoseGraph::WaitForAllComputations() {
|
||||||
}
|
}
|
||||||
|
|
||||||
void PoseGraph::FinishTrajectory(const int trajectory_id) {
|
void PoseGraph::FinishTrajectory(const int trajectory_id) {
|
||||||
// TODO(jihoonl): Add a logic to notify trimmers to finish the given
|
AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
|
||||||
// trajectory.
|
CHECK_EQ(finished_trajectories_.count(trajectory_id), 0);
|
||||||
|
finished_trajectories_.insert(trajectory_id);
|
||||||
|
|
||||||
|
auto submap_data = optimization_problem_.submap_data();
|
||||||
|
for (auto submap_id_data : submap_data) {
|
||||||
|
submap_data_.at(submap_id_data.id).state = SubmapState::kFinished;
|
||||||
|
}
|
||||||
|
// TODO(jihoonl): Refactor HandleWorkQueue() logic from
|
||||||
|
// ComputeConstraintsForNode and call from here
|
||||||
|
});
|
||||||
|
}
|
||||||
|
|
||||||
|
bool PoseGraph::IsTrajectoryFinished(const int trajectory_id) {
|
||||||
|
return finished_trajectories_.count(trajectory_id) > 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
void PoseGraph::FreezeTrajectory(const int trajectory_id) {
|
void PoseGraph::FreezeTrajectory(const int trajectory_id) {
|
||||||
|
@ -684,6 +704,10 @@ int PoseGraph::TrimmingHandle::num_submaps(const int trajectory_id) const {
|
||||||
return submap_data.SizeOfTrajectoryOrZero(trajectory_id);
|
return submap_data.SizeOfTrajectoryOrZero(trajectory_id);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool PoseGraph::TrimmingHandle::IsFinished(const int trajectory_id) const {
|
||||||
|
return parent_->IsTrajectoryFinished(trajectory_id);
|
||||||
|
}
|
||||||
|
|
||||||
void PoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
|
void PoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
|
||||||
const mapping::SubmapId& submap_id) {
|
const mapping::SubmapId& submap_id) {
|
||||||
// TODO(hrapp): We have to make sure that the trajectory has been finished
|
// TODO(hrapp): We have to make sure that the trajectory has been finished
|
||||||
|
|
|
@ -87,6 +87,7 @@ class PoseGraph : public mapping::PoseGraph {
|
||||||
EXCLUDES(mutex_);
|
EXCLUDES(mutex_);
|
||||||
|
|
||||||
void FinishTrajectory(int trajectory_id) override;
|
void FinishTrajectory(int trajectory_id) override;
|
||||||
|
bool IsTrajectoryFinished(int trajectory_id) override;
|
||||||
void FreezeTrajectory(int trajectory_id) override;
|
void FreezeTrajectory(int trajectory_id) override;
|
||||||
void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
|
void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
|
||||||
const mapping::proto::Submap& submap) override;
|
const mapping::proto::Submap& submap) override;
|
||||||
|
@ -240,6 +241,9 @@ class PoseGraph : public mapping::PoseGraph {
|
||||||
// Set of all frozen trajectories not being optimized.
|
// Set of all frozen trajectories not being optimized.
|
||||||
std::set<int> frozen_trajectories_ GUARDED_BY(mutex_);
|
std::set<int> frozen_trajectories_ GUARDED_BY(mutex_);
|
||||||
|
|
||||||
|
// Set of all finished trajectories.
|
||||||
|
std::set<int> finished_trajectories_ GUARDED_BY(mutex_);
|
||||||
|
|
||||||
// Set of all initial trajectory poses.
|
// Set of all initial trajectory poses.
|
||||||
std::map<int, InitialTrajectoryPose> initial_trajectory_poses_
|
std::map<int, InitialTrajectoryPose> initial_trajectory_poses_
|
||||||
GUARDED_BY(mutex_);
|
GUARDED_BY(mutex_);
|
||||||
|
@ -254,6 +258,7 @@ class PoseGraph : public mapping::PoseGraph {
|
||||||
int num_submaps(int trajectory_id) const override;
|
int num_submaps(int trajectory_id) const override;
|
||||||
void MarkSubmapAsTrimmed(const mapping::SubmapId& submap_id)
|
void MarkSubmapAsTrimmed(const mapping::SubmapId& submap_id)
|
||||||
REQUIRES(parent_->mutex_) override;
|
REQUIRES(parent_->mutex_) override;
|
||||||
|
bool IsFinished(int trajectory_id) const override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
PoseGraph* const parent_;
|
PoseGraph* const parent_;
|
||||||
|
|
|
@ -337,6 +337,13 @@ void PoseGraph::HandleWorkQueue() {
|
||||||
for (auto& trimmer : trimmers_) {
|
for (auto& trimmer : trimmers_) {
|
||||||
trimmer->Trim(&trimming_handle);
|
trimmer->Trim(&trimming_handle);
|
||||||
}
|
}
|
||||||
|
trimmers_.erase(
|
||||||
|
std::remove_if(
|
||||||
|
trimmers_.begin(), trimmers_.end(),
|
||||||
|
[](std::unique_ptr<mapping::PoseGraphTrimmer>& trimmer) {
|
||||||
|
return trimmer->IsFinished();
|
||||||
|
}),
|
||||||
|
trimmers_.end());
|
||||||
|
|
||||||
num_nodes_since_last_loop_closure_ = 0;
|
num_nodes_since_last_loop_closure_ = 0;
|
||||||
run_loop_closure_ = false;
|
run_loop_closure_ = false;
|
||||||
|
@ -386,8 +393,21 @@ void PoseGraph::WaitForAllComputations() {
|
||||||
}
|
}
|
||||||
|
|
||||||
void PoseGraph::FinishTrajectory(const int trajectory_id) {
|
void PoseGraph::FinishTrajectory(const int trajectory_id) {
|
||||||
// TODO(jihoonl): Add a logic to notify trimmers to finish the given
|
AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
|
||||||
// trajectory.
|
CHECK_EQ(finished_trajectories_.count(trajectory_id), 0);
|
||||||
|
finished_trajectories_.insert(trajectory_id);
|
||||||
|
|
||||||
|
auto submap_data = optimization_problem_.submap_data();
|
||||||
|
for (auto submap_id_data : submap_data) {
|
||||||
|
submap_data_.at(submap_id_data.id).state = SubmapState::kFinished;
|
||||||
|
}
|
||||||
|
// TODO(jihoonl): Refactor HandleWorkQueue() logic from
|
||||||
|
// ComputeConstraintsForNode and call from here
|
||||||
|
});
|
||||||
|
}
|
||||||
|
|
||||||
|
bool PoseGraph::IsTrajectoryFinished(const int trajectory_id) {
|
||||||
|
return finished_trajectories_.count(trajectory_id) > 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
void PoseGraph::FreezeTrajectory(const int trajectory_id) {
|
void PoseGraph::FreezeTrajectory(const int trajectory_id) {
|
||||||
|
@ -700,6 +720,10 @@ int PoseGraph::TrimmingHandle::num_submaps(const int trajectory_id) const {
|
||||||
return submap_data.SizeOfTrajectoryOrZero(trajectory_id);
|
return submap_data.SizeOfTrajectoryOrZero(trajectory_id);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool PoseGraph::TrimmingHandle::IsFinished(const int trajectory_id) const {
|
||||||
|
return parent_->IsTrajectoryFinished(trajectory_id);
|
||||||
|
}
|
||||||
|
|
||||||
void PoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
|
void PoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
|
||||||
const mapping::SubmapId& submap_id) {
|
const mapping::SubmapId& submap_id) {
|
||||||
// TODO(hrapp): We have to make sure that the trajectory has been finished
|
// TODO(hrapp): We have to make sure that the trajectory has been finished
|
||||||
|
|
|
@ -87,6 +87,7 @@ class PoseGraph : public mapping::PoseGraph {
|
||||||
EXCLUDES(mutex_);
|
EXCLUDES(mutex_);
|
||||||
|
|
||||||
void FinishTrajectory(int trajectory_id) override;
|
void FinishTrajectory(int trajectory_id) override;
|
||||||
|
bool IsTrajectoryFinished(int trajectory_id) override;
|
||||||
void FreezeTrajectory(int trajectory_id) override;
|
void FreezeTrajectory(int trajectory_id) override;
|
||||||
void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
|
void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
|
||||||
const mapping::proto::Submap& submap) override;
|
const mapping::proto::Submap& submap) override;
|
||||||
|
@ -244,6 +245,9 @@ class PoseGraph : public mapping::PoseGraph {
|
||||||
// Set of all frozen trajectories not being optimized.
|
// Set of all frozen trajectories not being optimized.
|
||||||
std::set<int> frozen_trajectories_ GUARDED_BY(mutex_);
|
std::set<int> frozen_trajectories_ GUARDED_BY(mutex_);
|
||||||
|
|
||||||
|
// Set of all finished trajectories.
|
||||||
|
std::set<int> finished_trajectories_ GUARDED_BY(mutex_);
|
||||||
|
|
||||||
// Set of all initial trajectory poses.
|
// Set of all initial trajectory poses.
|
||||||
std::map<int, InitialTrajectoryPose> initial_trajectory_poses_
|
std::map<int, InitialTrajectoryPose> initial_trajectory_poses_
|
||||||
GUARDED_BY(mutex_);
|
GUARDED_BY(mutex_);
|
||||||
|
@ -258,6 +262,7 @@ class PoseGraph : public mapping::PoseGraph {
|
||||||
int num_submaps(int trajectory_id) const override;
|
int num_submaps(int trajectory_id) const override;
|
||||||
void MarkSubmapAsTrimmed(const mapping::SubmapId& submap_id)
|
void MarkSubmapAsTrimmed(const mapping::SubmapId& submap_id)
|
||||||
REQUIRES(parent_->mutex_) override;
|
REQUIRES(parent_->mutex_) override;
|
||||||
|
bool IsFinished(int trajectory_id) const override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
PoseGraph* const parent_;
|
PoseGraph* const parent_;
|
||||||
|
|
Loading…
Reference in New Issue