trajectory_connectivity.cc
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
18 
19 #include <algorithm>
20 #include <unordered_set>
21 
22 #include "cartographer/mapping/proto/trajectory_connectivity.pb.h"
23 #include "glog/logging.h"
24 
25 namespace cartographer {
26 namespace mapping {
27 
29  : lock_(), forest_(), connection_map_() {}
30 
31 void TrajectoryConnectivity::Add(const int trajectory_id) {
32  common::MutexLocker locker(&lock_);
33  forest_.emplace(trajectory_id, trajectory_id);
34 }
35 
36 void TrajectoryConnectivity::Connect(const int trajectory_id_a,
37  const int trajectory_id_b) {
38  common::MutexLocker locker(&lock_);
39  Union(trajectory_id_a, trajectory_id_b);
40  auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b);
41  ++connection_map_[sorted_pair];
42 }
43 
44 void TrajectoryConnectivity::Union(const int trajectory_id_a,
45  const int trajectory_id_b) {
46  forest_.emplace(trajectory_id_a, trajectory_id_a);
47  forest_.emplace(trajectory_id_b, trajectory_id_b);
48  const int representative_a = FindSet(trajectory_id_a);
49  const int representative_b = FindSet(trajectory_id_b);
50  forest_[representative_a] = representative_b;
51 }
52 
53 int TrajectoryConnectivity::FindSet(const int trajectory_id) {
54  auto it = forest_.find(trajectory_id);
55  CHECK(it != forest_.end());
56  if (it->first != it->second) {
57  it->second = FindSet(it->second);
58  }
59  return it->second;
60 }
61 
62 bool TrajectoryConnectivity::TransitivelyConnected(const int trajectory_id_a,
63  const int trajectory_id_b) {
64  common::MutexLocker locker(&lock_);
65 
66  if (forest_.count(trajectory_id_a) == 0 ||
67  forest_.count(trajectory_id_b) == 0) {
68  return false;
69  }
70  return FindSet(trajectory_id_a) == FindSet(trajectory_id_b);
71 }
72 
73 std::vector<std::vector<int>> TrajectoryConnectivity::ConnectedComponents() {
74  // Map from cluster exemplar -> growing cluster.
75  std::unordered_map<int, std::vector<int>> map;
76  common::MutexLocker locker(&lock_);
77  for (const auto& trajectory_id_entry : forest_) {
78  map[FindSet(trajectory_id_entry.first)].push_back(
79  trajectory_id_entry.first);
80  }
81 
82  std::vector<std::vector<int>> result;
83  result.reserve(map.size());
84  for (auto& pair : map) {
85  result.emplace_back(std::move(pair.second));
86  }
87  return result;
88 }
89 
90 int TrajectoryConnectivity::ConnectionCount(const int trajectory_id_a,
91  const int trajectory_id_b) {
92  common::MutexLocker locker(&lock_);
93  const auto it =
94  connection_map_.find(std::minmax(trajectory_id_a, trajectory_id_b));
95  return it != connection_map_.end() ? it->second : 0;
96 }
97 
98 proto::TrajectoryConnectivity ToProto(
99  std::vector<std::vector<int>> connected_components) {
100  proto::TrajectoryConnectivity proto;
101  for (auto& connected_component : connected_components) {
102  std::sort(connected_component.begin(), connected_component.end());
103  }
104  std::sort(connected_components.begin(), connected_components.end());
105  for (const auto& connected_component : connected_components) {
106  auto* proto_connected_component = proto.add_connected_component();
107  for (const int trajectory_id : connected_component) {
108  proto_connected_component->add_trajectory_id(trajectory_id);
109  }
110  }
111  return proto;
112 }
113 
114 proto::TrajectoryConnectivity::ConnectedComponent FindConnectedComponent(
115  const proto::TrajectoryConnectivity& trajectory_connectivity,
116  const int trajectory_id) {
117  for (const auto& connected_component :
118  trajectory_connectivity.connected_component()) {
119  if (std::find(connected_component.trajectory_id().begin(),
120  connected_component.trajectory_id().end(),
121  trajectory_id) != connected_component.trajectory_id().end()) {
122  return connected_component;
123  }
124  }
125 
126  proto::TrajectoryConnectivity::ConnectedComponent connected_component;
127  connected_component.add_trajectory_id(trajectory_id);
128  return connected_component;
129 }
130 
131 } // namespace mapping
132 } // namespace cartographer
void Union(int trajectory_id_a, int trajectory_id_b) REQUIRES(lock_)
void Add(int trajectory_id) EXCLUDES(lock_)
int FindSet(int trajectory_id) REQUIRES(lock_)
bool TransitivelyConnected(int trajectory_id_a, int trajectory_id_b) EXCLUDES(lock_)
proto::TrajectoryConnectivity::ConnectedComponent FindConnectedComponent(const proto::TrajectoryConnectivity &trajectory_connectivity, const int trajectory_id)
void Connect(int trajectory_id_a, int trajectory_id_b) EXCLUDES(lock_)
std::vector< std::vector< int > > ConnectedComponents() EXCLUDES(lock_)
proto::SparsePoseGraph::Constraint::Tag ToProto(const SparsePoseGraph::Constraint::Tag &tag)
Mutex::Locker MutexLocker
Definition: mutex.h:95
int ConnectionCount(int trajectory_id_a, int trajectory_id_b) EXCLUDES(lock_)


cartographer
Author(s):
autogenerated on Wed Jun 5 2019 21:57:59