diff --git a/modules/perception/common/graph/BUILD b/modules/perception/common/graph/BUILD index 8d0bcaca108aa6b26db7590c48bc7c4f53c70d5c..5a487eaf65772a122f8111121803511110dd9f69 100644 --- a/modules/perception/common/graph/BUILD +++ b/modules/perception/common/graph/BUILD @@ -72,6 +72,7 @@ cc_test( "@eigen", ], ) + cc_library( name = "hungarian_optimizer", hdrs = [ @@ -94,4 +95,28 @@ cc_test( "@gtest//:main", ], ) + +cc_library( + name = "connected_component_analysis", + hdrs = [ + "connected_component_analysis.h", + ], + deps = [ + "//framework:cybertron", + ], +) + +cc_test( + name = "connected_component_analysis_test", + size = "small", + srcs = [ + "connected_component_analysis_test.cc", + ], + deps = [ + ":connected_component_analysis", + "@eigen", + "@gtest//:main", + ], +) + cpplint() diff --git a/modules/perception/common/graph/connected_component_analysis.h b/modules/perception/common/graph/connected_component_analysis.h new file mode 100644 index 0000000000000000000000000000000000000000..f551a4bb55d120dc4794feafb1c6f38e93326f97 --- /dev/null +++ b/modules/perception/common/graph/connected_component_analysis.h @@ -0,0 +1,75 @@ +/****************************************************************************** + * Copyright 2018 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#ifndef MODULES_PERCEPTION_COMMON_GRAPH_CONNECTED_COMPONENT_ANALYSIS_H_ +#define MODULES_PERCEPTION_COMMON_GRAPH_CONNECTED_COMPONENT_ANALYSIS_H_ + +#include +#include + +#include "cybertron/common/log.h" + +namespace apollo { +namespace perception { +namespace common { + +/* +* @brief: bfs based connected component analysis +* @params[IN] graph: input graph for connected component analysis +* @params[OUT] components: connected components of input graph +* @return nothing +* */ +void ConnectedComponentAnalysis(const std::vector>& graph, + std::vector>* components) { + CHECK_NOTNULL(components); + int num_item = graph.size(); + std::vector visited; + visited.resize(num_item, 0); + std::queue que; + std::vector component; + component.reserve(num_item); + components->clear(); + + for (int index = 0; index < num_item; ++index) { + if (visited[index]) { + continue; + } + component.push_back(index); + que.push(index); + visited[index] = 1; + while (!que.empty()) { + int current_id = que.front(); + que.pop(); + for (size_t sub_index = 0; sub_index < graph[current_id].size(); + ++sub_index) { + int neighbor_id = graph[current_id][sub_index]; + if (visited[neighbor_id] == 0) { + component.push_back(neighbor_id); + que.push(neighbor_id); + visited[neighbor_id] = 1; + } + } + } + components->push_back(component); + component.clear(); + } +} + +} // namespace common +} // namespace perception +} // namespace apollo + +#endif // MODULES_PERCEPTION_COMMON_GRAPH_CONNECTED_COMPONENT_ANALYSIS_H_ diff --git a/modules/perception/common/graph/connected_component_analysis_test.cc b/modules/perception/common/graph/connected_component_analysis_test.cc new file mode 100644 index 0000000000000000000000000000000000000000..9cb476ebfaaa6c8899370fa88419c1a65bc2222d --- /dev/null +++ b/modules/perception/common/graph/connected_component_analysis_test.cc @@ -0,0 +1,68 @@ +/****************************************************************************** + * Copyright 2018 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/perception/common/graph/connected_component_analysis.h" + +#include "Eigen/Core" +#include "gtest/gtest.h" + +namespace apollo { +namespace perception { +namespace common { + +class ConnectedComponentAnalysisTest : public testing::Test { + protected: + ConnectedComponentAnalysisTest() = default; + virtual ~ConnectedComponentAnalysisTest() = default; + void SetUp() {} + void TearDown() {} +}; // class ConnectedComponentAnalysisTest + +TEST_F(ConnectedComponentAnalysisTest, test_ConnectedComponenetAnalysis) { + Eigen::MatrixXf association_mat(3, 4); + association_mat << 0.3, 1.2, 4.0, 3.0, 0.9, 2.0, 3.0, 8.0, 4.0, 3.0, 0.3, 0.1; + const float connected_threshold = 2.1; + // Compute connected components within given threshold + int no_track = association_mat.rows(); + int no_obj = association_mat.cols(); + std::vector> nb_graph; + nb_graph.resize(no_track + no_obj); + for (int i = 0; i < no_track; i++) { + for (int j = 0; j < no_obj; j++) { + if (association_mat(i, j) <= connected_threshold) { + nb_graph[i].push_back(no_track + j); + nb_graph[j + no_track].push_back(i); + } + } + } + + std::vector> components; + ConnectedComponentAnalysis(nb_graph, &components); + EXPECT_EQ(2, components.size()); + EXPECT_EQ(4, components[0].size()); + EXPECT_EQ(0, components[0][0]); + EXPECT_EQ(3, components[0][1]); + EXPECT_EQ(4, components[0][2]); + EXPECT_EQ(1, components[0][3]); + EXPECT_EQ(3, components[1].size()); + EXPECT_EQ(2, components[1][0]); + EXPECT_EQ(5, components[1][1]); + EXPECT_EQ(6, components[1][2]); +} + +} // namespace common +} // namespace perception +} // namespace apollo