Perception: added connected_component_analysis.

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()

modules/perception/common/graph/connected_component_analysis.h

+/******************************************************************************
+ * 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 <queue>
+#include <vector>
+
+#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<std::vector<int>>& graph,
+                                std::vector<std::vector<int>>* components) {
+  CHECK_NOTNULL(components);
+  int num_item = graph.size();
+  std::vector<int> visited;
+  visited.resize(num_item, 0);
+  std::queue<int> que;
+  std::vector<int> 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_

modules/perception/common/graph/connected_component_analysis_test.cc

+/******************************************************************************
+ * 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<std::vector<int>> 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<std::vector<int>> 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