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

Add FinishTrajectory() in SparsePosegraph. (#562)

master
Jihoon Lee 2017-11-09 14:29:26 +01:00 committed by Wolfgang Hess
parent 92f81aec8a
commit 3bdee588bd
6 changed files with 16 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
cartographer/mapping/map_builder.cc
View File

@ -125,6 +125,7 @@ TrajectoryBuilder* MapBuilder::GetTrajectoryBuilder(
void MapBuilder::FinishTrajectory(const int trajectory_id) { void MapBuilder::FinishTrajectory(const int trajectory_id) {
sensor_collator_.FinishTrajectory(trajectory_id); sensor_collator_.FinishTrajectory(trajectory_id);
sparse_pose_graph_->FinishTrajectory(trajectory_id);
} }
int MapBuilder::GetBlockingTrajectoryId() const { int MapBuilder::GetBlockingTrajectoryId() const {

3
cartographer/mapping/sparse_pose_graph.h
View File

@ -80,6 +80,9 @@ class SparsePoseGraph {
SparsePoseGraph(const SparsePoseGraph&) = delete; SparsePoseGraph(const SparsePoseGraph&) = delete;
SparsePoseGraph& operator=(const SparsePoseGraph&) = delete; SparsePoseGraph& operator=(const SparsePoseGraph&) = delete;
// Finishes the given trajectory.
virtual void FinishTrajectory(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;

5
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -372,6 +372,11 @@ void SparsePoseGraph::WaitForAllComputations() {
locker.Await([&notification]() { return notification; }); locker.Await([&notification]() { return notification; });
} }
void SparsePoseGraph::FinishTrajectory(const int trajectory_id) {
// TODO(jihoonl): Add a logic to notify trimmers to finish the given
// trajectory.
}
void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) { void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
trajectory_connectivity_state_.Add(trajectory_id); trajectory_connectivity_state_.Add(trajectory_id);

1
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -83,6 +83,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
int trajectory_id, int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data); const sensor::FixedFramePoseData& fixed_frame_pose_data);
void FinishTrajectory(int trajectory_id) override;
void FreezeTrajectory(int trajectory_id) override; void FreezeTrajectory(int trajectory_id) override;
void AddSubmapFromProto(const transform::Rigid3d& global_pose, void AddSubmapFromProto(const transform::Rigid3d& global_pose,
const mapping::proto::Submap& submap) override; const mapping::proto::Submap& submap) override;

5
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -388,6 +388,11 @@ void SparsePoseGraph::WaitForAllComputations() {
locker.Await([&notification]() { return notification; }); locker.Await([&notification]() { return notification; });
} }
void SparsePoseGraph::FinishTrajectory(const int trajectory_id) {
// TODO(jihoonl): Add a logic to notify trimmers to finish the given
// trajectory.
}
void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) { void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
trajectory_connectivity_state_.Add(trajectory_id); trajectory_connectivity_state_.Add(trajectory_id);

1
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -83,6 +83,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
int trajectory_id, int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data); const sensor::FixedFramePoseData& fixed_frame_pose_data);
void FinishTrajectory(int trajectory_id) override;
void FreezeTrajectory(int trajectory_id) override; void FreezeTrajectory(int trajectory_id) override;
void AddSubmapFromProto(const transform::Rigid3d& global_pose, void AddSubmapFromProto(const transform::Rigid3d& global_pose,
const mapping::proto::Submap& submap) override; const mapping::proto::Submap& submap) override;