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

Rename TrajectoryConnectivity ConnectedComponents. (#510) 

* Rename TrajectoryConnectivity ConnectedComponents.
master
Christoph Schütte 2017-09-07 17:11:06 +02:00 committed by GitHub
parent c65f7a97b6
commit 1a367f0549
8 changed files with 74 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
cartographer/mapping/trajectory_connectivity.cc → cartographer/mapping/connected_components.cc
View File

@ -14,26 +14,26 @@
* limitations under the License.
*/
#include "cartographer/mapping/trajectory_connectivity.h"
#include "cartographer/mapping/connected_components.h"
#include <algorithm>
#include <unordered_set>
#include "cartographer/mapping/proto/trajectory_connectivity.pb.h"
#include "cartographer/mapping/proto/connected_components.pb.h"
#include "glog/logging.h"
namespace cartographer {
namespace mapping {
TrajectoryConnectivity::TrajectoryConnectivity()
ConnectedComponents::ConnectedComponents()
: lock_(), forest_(), connection_map_() {}
void TrajectoryConnectivity::Add(const int trajectory_id) {
void ConnectedComponents::Add(const int trajectory_id) {
common::MutexLocker locker(&lock_);
forest_.emplace(trajectory_id, trajectory_id);
}
void TrajectoryConnectivity::Connect(const int trajectory_id_a,
void ConnectedComponents::Connect(const int trajectory_id_a,
const int trajectory_id_b) {
common::MutexLocker locker(&lock_);
Union(trajectory_id_a, trajectory_id_b);
@ -41,7 +41,7 @@ void TrajectoryConnectivity::Connect(const int trajectory_id_a,
++connection_map_[sorted_pair];
}
void TrajectoryConnectivity::Union(const int trajectory_id_a,
void ConnectedComponents::Union(const int trajectory_id_a,
const int trajectory_id_b) {
forest_.emplace(trajectory_id_a, trajectory_id_a);
forest_.emplace(trajectory_id_b, trajectory_id_b);
@ -50,7 +50,7 @@ void TrajectoryConnectivity::Union(const int trajectory_id_a,
forest_[representative_a] = representative_b;
}
int TrajectoryConnectivity::FindSet(const int trajectory_id) {
int ConnectedComponents::FindSet(const int trajectory_id) {
auto it = forest_.find(trajectory_id);
CHECK(it != forest_.end());
if (it->first != it->second) {
@ -59,7 +59,7 @@ int TrajectoryConnectivity::FindSet(const int trajectory_id) {
return it->second;
}
bool TrajectoryConnectivity::TransitivelyConnected(const int trajectory_id_a,
bool ConnectedComponents::TransitivelyConnected(const int trajectory_id_a,
const int trajectory_id_b) {
if (trajectory_id_a == trajectory_id_b) {
return true;
@ -74,7 +74,7 @@ bool TrajectoryConnectivity::TransitivelyConnected(const int trajectory_id_a,
return FindSet(trajectory_id_a) == FindSet(trajectory_id_b);
}
std::vector<std::vector<int>> TrajectoryConnectivity::ConnectedComponents() {
std::vector<std::vector<int>> ConnectedComponents::Components() {
// Map from cluster exemplar -> growing cluster.
std::unordered_map<int, std::vector<int>> map;
common::MutexLocker locker(&lock_);
@ -91,7 +91,7 @@ std::vector<std::vector<int>> TrajectoryConnectivity::ConnectedComponents() {
return result;
}
int TrajectoryConnectivity::ConnectionCount(const int trajectory_id_a,
int ConnectedComponents::ConnectionCount(const int trajectory_id_a,
const int trajectory_id_b) {
common::MutexLocker locker(&lock_);
const auto it =
@ -99,9 +99,9 @@ int TrajectoryConnectivity::ConnectionCount(const int trajectory_id_a,
return it != connection_map_.end() ? it->second : 0;
}
proto::TrajectoryConnectivity ToProto(
proto::ConnectedComponents ToProto(
std::vector<std::vector<int>> connected_components) {
proto::TrajectoryConnectivity proto;
proto::ConnectedComponents proto;
for (auto& connected_component : connected_components) {
std::sort(connected_component.begin(), connected_component.end());
}
@ -115,11 +115,11 @@ proto::TrajectoryConnectivity ToProto(
return proto;
}
proto::TrajectoryConnectivity::ConnectedComponent FindConnectedComponent(
const proto::TrajectoryConnectivity& trajectory_connectivity,
proto::ConnectedComponents::ConnectedComponent FindConnectedComponent(
const proto::ConnectedComponents& connected_components,
const int trajectory_id) {
for (const auto& connected_component :
trajectory_connectivity.connected_component()) {
connected_components.connected_component()) {
if (std::find(connected_component.trajectory_id().begin(),
connected_component.trajectory_id().end(),
trajectory_id) != connected_component.trajectory_id().end()) {
@ -127,7 +127,7 @@ proto::TrajectoryConnectivity::ConnectedComponent FindConnectedComponent(
}
}
proto::TrajectoryConnectivity::ConnectedComponent connected_component;
proto::ConnectedComponents::ConnectedComponent connected_component;
connected_component.add_trajectory_id(trajectory_id);
return connected_component;
}

26
cartographer/mapping/trajectory_connectivity.h → cartographer/mapping/connected_components.h
View File

@ -14,14 +14,14 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_TRAJECTORY_CONNECTIVITY_H_
#define CARTOGRAPHER_MAPPING_TRAJECTORY_CONNECTIVITY_H_
#ifndef CARTOGRAPHER_MAPPING_CONNECTED_COMPONENTS_H_
#define CARTOGRAPHER_MAPPING_CONNECTED_COMPONENTS_H_
#include <map>
#include <unordered_map>
#include "cartographer/common/mutex.h"
#include "cartographer/mapping/proto/trajectory_connectivity.pb.h"
#include "cartographer/mapping/proto/connected_components.pb.h"
#include "cartographer/mapping/submaps.h"
namespace cartographer {
@ -33,12 +33,12 @@ namespace mapping {
// connected trajectories i and j?") and the transitive connectivity.
//
// This class is thread-safe.
class TrajectoryConnectivity {
class ConnectedComponents {
public:
TrajectoryConnectivity();
ConnectedComponents();
TrajectoryConnectivity(const TrajectoryConnectivity&) = delete;
TrajectoryConnectivity& operator=(const TrajectoryConnectivity&) = delete;
ConnectedComponents(const ConnectedComponents&) = delete;
ConnectedComponents& operator=(const ConnectedComponents&) = delete;
// Add a trajectory which is initially connected to only itself.
void Add(int trajectory_id) EXCLUDES(lock_);
@ -61,7 +61,7 @@ class TrajectoryConnectivity {
int ConnectionCount(int trajectory_id_a, int trajectory_id_b) EXCLUDES(lock_);
// The trajectory IDs, grouped by connectivity.
std::vector<std::vector<int>> ConnectedComponents() EXCLUDES(lock_);
std::vector<std::vector<int>> Components() EXCLUDES(lock_);
private:
// Find the representative and compresses the path to it.
@ -77,16 +77,16 @@ class TrajectoryConnectivity {
};
// Returns a proto encoding connected components.
proto::TrajectoryConnectivity ToProto(
proto::ConnectedComponents ToProto(
std::vector<std::vector<int>> connected_components);
// Returns the connected component containing 'trajectory_id'.
proto::TrajectoryConnectivity::ConnectedComponent FindConnectedComponent(
const cartographer::mapping::proto::TrajectoryConnectivity&
trajectory_connectivity,
proto::ConnectedComponents::ConnectedComponent FindConnectedComponent(
const cartographer::mapping::proto::ConnectedComponents&
connected_components,
int trajectory_id);
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_TRAJECTORY_CONNECTIVITY_H_
#endif // CARTOGRAPHER_MAPPING_CONNECTED_COMPONENTS_H_

62
cartographer/mapping/trajectory_connectivity_test.cc → cartographer/mapping/connected_components_test.cc
View File

@ -14,7 +14,7 @@
* limitations under the License.
*/
#include "cartographer/mapping/trajectory_connectivity.h"
#include "cartographer/mapping/connected_components.h"
#include <algorithm>
#include <memory>
@ -28,49 +28,49 @@ namespace {
constexpr int kNumTrajectories = 10;
TEST(TrajectoryConnectivityTest, TransitivelyConnected) {
TrajectoryConnectivity trajectory_connectivity;
TEST(ConnectedComponentsTest, TransitivelyConnected) {
ConnectedComponents connected_components;
// Make sure nothing's connected until we connect some things.
for (int trajectory_a = 0; trajectory_a < kNumTrajectories; ++trajectory_a) {
for (int trajectory_b = 0; trajectory_b < kNumTrajectories;
++trajectory_b) {
EXPECT_EQ(trajectory_a == trajectory_b,
trajectory_connectivity.TransitivelyConnected(trajectory_a,
connected_components.TransitivelyConnected(trajectory_a,
trajectory_b));
}
}
// Connect some stuff up.
trajectory_connectivity.Connect(0, 1);
EXPECT_TRUE(trajectory_connectivity.TransitivelyConnected(0, 1));
trajectory_connectivity.Connect(8, 9);
EXPECT_TRUE(trajectory_connectivity.TransitivelyConnected(8, 9));
EXPECT_FALSE(trajectory_connectivity.TransitivelyConnected(0, 9));
connected_components.Connect(0, 1);
EXPECT_TRUE(connected_components.TransitivelyConnected(0, 1));
connected_components.Connect(8, 9);
EXPECT_TRUE(connected_components.TransitivelyConnected(8, 9));
EXPECT_FALSE(connected_components.TransitivelyConnected(0, 9));
trajectory_connectivity.Connect(1, 8);
connected_components.Connect(1, 8);
for (int i : {0, 1}) {
for (int j : {8, 9}) {
EXPECT_TRUE(trajectory_connectivity.TransitivelyConnected(i, j));
EXPECT_TRUE(connected_components.TransitivelyConnected(i, j));
}
}
}
TEST(TrajectoryConnectivityTest, EmptyConnectedComponents) {
TrajectoryConnectivity trajectory_connectivity;
auto connections = trajectory_connectivity.ConnectedComponents();
TEST(ConnectedComponentsTest, EmptyConnectedComponents) {
ConnectedComponents connected_components;
auto connections = connected_components.Components();
EXPECT_EQ(0, connections.size());
}
TEST(TrajectoryConnectivityTest, ConnectedComponents) {
TrajectoryConnectivity trajectory_connectivity;
TEST(ConnectedComponentsTest, ConnectedComponents) {
ConnectedComponents connected_components;
for (int i = 0; i <= 4; ++i) {
trajectory_connectivity.Connect(0, i);
connected_components.Connect(0, i);
}
for (int i = 5; i <= 9; ++i) {
trajectory_connectivity.Connect(5, i);
connected_components.Connect(5, i);
}
auto connections = trajectory_connectivity.ConnectedComponents();
auto connections = connected_components.Components();
ASSERT_EQ(2, connections.size());
// The clustering is arbitrary; we need to figure out which one is which.
const std::vector<int>* zero_cluster = nullptr;
@ -91,25 +91,25 @@ TEST(TrajectoryConnectivityTest, ConnectedComponents) {
}
}
TEST(TrajectoryConnectivityTest, ConnectionCount) {
TrajectoryConnectivity trajectory_connectivity;
TEST(ConnectedComponentsTest, ConnectionCount) {
ConnectedComponents connected_components;
for (int i = 0; i < kNumTrajectories; ++i) {
trajectory_connectivity.Connect(0, 1);
connected_components.Connect(0, 1);
// Permute the arguments to check invariance.
EXPECT_EQ(i + 1, trajectory_connectivity.ConnectionCount(1, 0));
EXPECT_EQ(i + 1, connected_components.ConnectionCount(1, 0));
}
for (int i = 1; i < 9; ++i) {
EXPECT_EQ(0, trajectory_connectivity.ConnectionCount(i, i + 1));
EXPECT_EQ(0, connected_components.ConnectionCount(i, i + 1));
}
}
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));
TEST(ConnectedComponentsTest, ReflexiveConnectivity) {
ConnectedComponents connected_components;
EXPECT_TRUE(connected_components.TransitivelyConnected(0, 0));
EXPECT_EQ(0, connected_components.ConnectionCount(0, 0));
connected_components.Add(0);
EXPECT_TRUE(connected_components.TransitivelyConnected(0, 0));
EXPECT_EQ(0, connected_components.ConnectionCount(0, 0));
}
} // namespace

4
cartographer/mapping/proto/trajectory_connectivity.proto → cartographer/mapping/proto/connected_components.proto
View File

@ -18,10 +18,10 @@ package cartographer.mapping.proto;
// This is how proto2 calls the outer class since there is already a message
// with the same name in the file.
option java_outer_classname = "TrajectoryConnectivityOuterClass";
option java_outer_classname = "ConnectedComponentsOuterClass";
// Connectivity structure between trajectories.
message TrajectoryConnectivity {
message ConnectedComponents {
message ConnectedComponent {
repeated int32 trajectory_id = 1;
}

8
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -104,7 +104,7 @@ void SparsePoseGraph::AddScan(
trajectory_nodes_.Append(
trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose});
++num_trajectory_nodes_;
trajectory_connectivity_.Add(trajectory_id);
connected_components_.Add(trajectory_id);
// Test if the 'insertion_submap.back()' is one we never saw before.
if (trajectory_id >= submap_data_.num_trajectories() ||
@ -180,7 +180,7 @@ 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 {
if (trajectory_connectivity_.TransitivelyConnected(
if (connected_components_.TransitivelyConnected(
node_id.trajectory_id, submap_id.trajectory_id)) {
const transform::Rigid2d initial_relative_pose =
optimization_problem_.submap_data()
@ -314,7 +314,7 @@ void SparsePoseGraph::HandleWorkQueue() {
for (const Constraint& constraint : result) {
CHECK_EQ(constraint.tag,
mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
trajectory_connectivity_.Connect(constraint.node_id.trajectory_id,
connected_components_.Connect(constraint.node_id.trajectory_id,
constraint.submap_id.trajectory_id);
}
@ -503,7 +503,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
}
std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
return trajectory_connectivity_.ConnectedComponents();
return connected_components_.Components();
}
int SparsePoseGraph::num_submaps(const int trajectory_id) {

4
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -32,9 +32,9 @@
#include "cartographer/common/mutex.h"
#include "cartographer/common/thread_pool.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/connected_components.h"
#include "cartographer/mapping/pose_graph_trimmer.h"
#include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/trajectory_connectivity.h"
#include "cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h"
#include "cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h"
#include "cartographer/mapping_2d/submaps.h"
@ -172,7 +172,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
GUARDED_BY(mutex_);
// How our various trajectories are related.
mapping::TrajectoryConnectivity trajectory_connectivity_;
mapping::ConnectedComponents connected_components_;
// We globally localize a fraction of the scans from each trajectory.
std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>>

8
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -102,7 +102,7 @@ void SparsePoseGraph::AddScan(
trajectory_nodes_.Append(
trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose});
++num_trajectory_nodes_;
trajectory_connectivity_.Add(trajectory_id);
connected_components_.Add(trajectory_id);
// Test if the 'insertion_submap.back()' is one we never saw before.
if (trajectory_id >= submap_data_.num_trajectories() ||
@ -212,7 +212,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
initial_relative_pose.rotation());
} else {
if (trajectory_connectivity_.TransitivelyConnected(
if (connected_components_.TransitivelyConnected(
node_id.trajectory_id, submap_id.trajectory_id)) {
constraint_builder_.MaybeAddConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
@ -330,7 +330,7 @@ void SparsePoseGraph::HandleWorkQueue() {
for (const Constraint& constraint : result) {
CHECK_EQ(constraint.tag,
mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
trajectory_connectivity_.Connect(constraint.node_id.trajectory_id,
connected_components_.Connect(constraint.node_id.trajectory_id,
constraint.submap_id.trajectory_id);
}
@ -534,7 +534,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
}
std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() {
return trajectory_connectivity_.ConnectedComponents();
return connected_components_.Components();
}
int SparsePoseGraph::num_submaps(const int trajectory_id) {

4
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -32,9 +32,9 @@
#include "cartographer/common/mutex.h"
#include "cartographer/common/thread_pool.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/connected_components.h"
#include "cartographer/mapping/pose_graph_trimmer.h"
#include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/trajectory_connectivity.h"
#include "cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h"
#include "cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h"
#include "cartographer/mapping_3d/submaps.h"
@ -177,7 +177,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
GUARDED_BY(mutex_);
// How our various trajectories are related.
mapping::TrajectoryConnectivity trajectory_connectivity_;
mapping::ConnectedComponents connected_components_;
// We globally localize a fraction of the scans from each trajectory.
std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>>