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

Remove connected_components_ from SPG. (#509) 

* Remove connected_components_ from SPG.

This PR removes connected_components_ from the SparsePoseGraph. Since
the connectivity structure is no longer updated by the
ConstraintBuilders it is no longer necessary for SPG to keep a copy of
the connected components for data consistency.
master
Christoph Schütte 2017-09-07 16:35:49 +02:00 committed by GitHub
parent fa306d03ec
commit c65f7a97b6
4 changed files with 4 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -307,6 +307,7 @@ void SparsePoseGraph::HandleWorkQueue() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end()); constraints_.insert(constraints_.end(), result.begin(), result.end());
} }
RunOptimization();
// Update the trajectory connectivity structure with the new // Update the trajectory connectivity structure with the new
// constraints. // constraints.
@ -316,7 +317,6 @@ void SparsePoseGraph::HandleWorkQueue() {
trajectory_connectivity_.Connect(constraint.node_id.trajectory_id, trajectory_connectivity_.Connect(constraint.node_id.trajectory_id,
constraint.submap_id.trajectory_id); constraint.submap_id.trajectory_id);
} }
RunOptimization();
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
num_scans_since_last_loop_closure_ = 0; num_scans_since_last_loop_closure_ = 0;
@ -465,7 +465,6 @@ void SparsePoseGraph::RunOptimization() {
} }
} }
optimized_submap_transforms_ = submap_data; optimized_submap_transforms_ = submap_data;
connected_components_ = trajectory_connectivity_.ConnectedComponents();
TrimmingHandle trimming_handle(this); TrimmingHandle trimming_handle(this);
for (auto& trimmer : trimmers_) { for (auto& trimmer : trimmers_) {
@ -504,8 +503,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
} }
std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() { std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
common::MutexLocker locker(&mutex_); return trajectory_connectivity_.ConnectedComponents();
return connected_components_;
} }
int SparsePoseGraph::num_submaps(const int trajectory_id) { int SparsePoseGraph::num_submaps(const int trajectory_id) {

3
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -194,9 +194,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
mapping::NestedVectorsById<SubmapData, mapping::SubmapId> submap_data_ mapping::NestedVectorsById<SubmapData, mapping::SubmapId> submap_data_
GUARDED_BY(mutex_); GUARDED_BY(mutex_);
// Connectivity structure of our trajectories by IDs.
std::vector<std::vector<int>> connected_components_;
// Data that are currently being shown. // Data that are currently being shown.
mapping::NestedVectorsById<mapping::TrajectoryNode, mapping::NodeId> mapping::NestedVectorsById<mapping::TrajectoryNode, mapping::NodeId>
trajectory_nodes_ GUARDED_BY(mutex_); trajectory_nodes_ GUARDED_BY(mutex_);

6
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -323,6 +323,7 @@ void SparsePoseGraph::HandleWorkQueue() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end()); constraints_.insert(constraints_.end(), result.begin(), result.end());
} }
RunOptimization();
// Update the trajectory connectivity structure with the new // Update the trajectory connectivity structure with the new
// constraints. // constraints.
@ -332,7 +333,6 @@ void SparsePoseGraph::HandleWorkQueue() {
trajectory_connectivity_.Connect(constraint.node_id.trajectory_id, trajectory_connectivity_.Connect(constraint.node_id.trajectory_id,
constraint.submap_id.trajectory_id); constraint.submap_id.trajectory_id);
} }
RunOptimization();
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
num_scans_since_last_loop_closure_ = 0; num_scans_since_last_loop_closure_ = 0;
@ -503,7 +503,6 @@ void SparsePoseGraph::RunOptimization() {
} }
} }
optimized_submap_transforms_ = optimization_problem_.submap_data(); optimized_submap_transforms_ = optimization_problem_.submap_data();
connected_components_ = trajectory_connectivity_.ConnectedComponents();
TrimmingHandle trimming_handle(this); TrimmingHandle trimming_handle(this);
for (auto& trimmer : trimmers_) { for (auto& trimmer : trimmers_) {
@ -535,8 +534,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
} }
std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() { std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
common::MutexLocker locker(&mutex_); return trajectory_connectivity_.ConnectedComponents();
return connected_components_;
} }
int SparsePoseGraph::num_submaps(const int trajectory_id) { int SparsePoseGraph::num_submaps(const int trajectory_id) {

3
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -199,9 +199,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
mapping::NestedVectorsById<SubmapData, mapping::SubmapId> submap_data_ mapping::NestedVectorsById<SubmapData, mapping::SubmapId> submap_data_
GUARDED_BY(mutex_); GUARDED_BY(mutex_);
// Connectivity structure of our trajectories by IDs.
std::vector<std::vector<int>> connected_components_;
// Data that are currently being shown. // Data that are currently being shown.
mapping::NestedVectorsById<mapping::TrajectoryNode, mapping::NodeId> mapping::NestedVectorsById<mapping::TrajectoryNode, mapping::NodeId>
trajectory_nodes_ GUARDED_BY(mutex_); trajectory_nodes_ GUARDED_BY(mutex_);