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

Remove reverse_connected_components. (#507) 

Remove reverse_connected_components from SparsePoseGraphs and update
TrajectoryConnectivity to return "connected" for the reflexive case even
if trajectories are unknown.
master
Christoph Schütte 2017-09-07 15:58:30 +02:00 committed by GitHub
parent 476d156f66
commit fa306d03ec
7 changed files with 24 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cartographer/mapping/trajectory_connectivity.cc
View File

@ -61,6 +61,10 @@ int TrajectoryConnectivity::FindSet(const int trajectory_id) {
bool TrajectoryConnectivity::TransitivelyConnected(const int trajectory_id_a,
const int trajectory_id_b) {
if (trajectory_id_a == trajectory_id_b) {
return true;
}
common::MutexLocker locker(&lock_);
if (forest_.count(trajectory_id_a) == 0 ||

7
cartographer/mapping/trajectory_connectivity.h
View File

@ -40,7 +40,7 @@ class TrajectoryConnectivity {
TrajectoryConnectivity(const TrajectoryConnectivity&) = delete;
TrajectoryConnectivity& operator=(const TrajectoryConnectivity&) = delete;
// Add a trajectory which is initially connected to nothing.
// Add a trajectory which is initially connected to only itself.
void Add(int trajectory_id) EXCLUDES(lock_);
// Connect two trajectories. If either trajectory is untracked, it will be
@ -49,8 +49,9 @@ class TrajectoryConnectivity {
void Connect(int trajectory_id_a, int trajectory_id_b) EXCLUDES(lock_);
// Determines if two trajectories have been (transitively) connected. If
// either trajectory is not being tracked, returns false. This function is
// invariant to the order of its arguments.
// either trajectory is not being tracked, returns false, except when it is
// the same trajectory, where it returns true. This function is invariant to
// the order of its arguments.
bool TransitivelyConnected(int trajectory_id_a, int trajectory_id_b)
EXCLUDES(lock_);

14
cartographer/mapping/trajectory_connectivity_test.cc
View File

@ -35,8 +35,9 @@ TEST(TrajectoryConnectivityTest, TransitivelyConnected) {
for (int trajectory_a = 0; trajectory_a < kNumTrajectories; ++trajectory_a) {
for (int trajectory_b = 0; trajectory_b < kNumTrajectories;
++trajectory_b) {
EXPECT_FALSE(trajectory_connectivity.TransitivelyConnected(trajectory_a,
trajectory_b));
EXPECT_EQ(trajectory_a == trajectory_b,
trajectory_connectivity.TransitivelyConnected(trajectory_a,
trajectory_b));
}
}
@ -102,6 +103,15 @@ TEST(TrajectoryConnectivityTest, ConnectionCount) {
}
}
TEST(TrajectoryConnectivityTest, ReflexiveConnectivity) {
TrajectoryConnectivity trajectory_connectivity;
EXPECT_TRUE(trajectory_connectivity.TransitivelyConnected(0, 0));
EXPECT_EQ(0, trajectory_connectivity.ConnectionCount(0, 0));
trajectory_connectivity.Add(0);
EXPECT_TRUE(trajectory_connectivity.TransitivelyConnected(0, 0));
EXPECT_EQ(0, trajectory_connectivity.ConnectionCount(0, 0));
}
} // namespace
} // namespace mapping
} // namespace cartographer

15
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -180,13 +180,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
trajectory_nodes_.at(node_id).constant_data.get());
} else {
const bool scan_and_submap_trajectories_connected =
reverse_connected_components_.count(node_id.trajectory_id) > 0 &&
reverse_connected_components_.count(submap_id.trajectory_id) > 0 &&
reverse_connected_components_.at(node_id.trajectory_id) ==
reverse_connected_components_.at(submap_id.trajectory_id);
if (node_id.trajectory_id == submap_id.trajectory_id ||
scan_and_submap_trajectories_connected) {
if (trajectory_connectivity_.TransitivelyConnected(
node_id.trajectory_id, submap_id.trajectory_id)) {
const transform::Rigid2d initial_relative_pose =
optimization_problem_.submap_data()
.at(submap_id.trajectory_id)
@ -471,12 +466,6 @@ void SparsePoseGraph::RunOptimization() {
}
optimized_submap_transforms_ = submap_data;
connected_components_ = trajectory_connectivity_.ConnectedComponents();
reverse_connected_components_.clear();
for (size_t i = 0; i != connected_components_.size(); ++i) {
for (const int trajectory_id : connected_components_[i]) {
reverse_connected_components_.emplace(trajectory_id, i);
}
}
TrimmingHandle trimming_handle(this);
for (auto& trimmer : trimmers_) {

2
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -196,8 +196,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Connectivity structure of our trajectories by IDs.
std::vector<std::vector<int>> connected_components_;
// Trajectory ID to connected component ID.
std::map<int, size_t> reverse_connected_components_;
// Data that are currently being shown.
mapping::NestedVectorsById<mapping::TrajectoryNode, mapping::NodeId>

15
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -212,13 +212,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
initial_relative_pose.rotation());
} else {
const bool scan_and_submap_trajectories_connected =
reverse_connected_components_.count(node_id.trajectory_id) > 0 &&
reverse_connected_components_.count(submap_id.trajectory_id) > 0 &&
reverse_connected_components_.at(node_id.trajectory_id) ==
reverse_connected_components_.at(submap_id.trajectory_id);
if (node_id.trajectory_id == submap_id.trajectory_id ||
scan_and_submap_trajectories_connected) {
if (trajectory_connectivity_.TransitivelyConnected(
node_id.trajectory_id, submap_id.trajectory_id)) {
constraint_builder_.MaybeAddConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
@ -509,12 +504,6 @@ void SparsePoseGraph::RunOptimization() {
}
optimized_submap_transforms_ = optimization_problem_.submap_data();
connected_components_ = trajectory_connectivity_.ConnectedComponents();
reverse_connected_components_.clear();
for (size_t i = 0; i != connected_components_.size(); ++i) {
for (const int trajectory_id : connected_components_[i]) {
reverse_connected_components_.emplace(trajectory_id, i);
}
}
TrimmingHandle trimming_handle(this);
for (auto& trimmer : trimmers_) {

2
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -201,8 +201,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Connectivity structure of our trajectories by IDs.
std::vector<std::vector<int>> connected_components_;
// Trajectory ID to connected component ID.
std::map<int, size_t> reverse_connected_components_;
// Data that are currently being shown.
mapping::NestedVectorsById<mapping::TrajectoryNode, mapping::NodeId>