Perception: added secure_matrix and hungarian optimizer.

apollo.sh

@@ -117,8 +117,7 @@ function generate_build_targets() {
     `bazel query //modules/localization/msf/local_tool/local_visualization/...`
     //modules/map/...
     //modules/monitor/...
-    //modules/perception/common/...
-    //modules/perception/proto/...
+    //modules/perception/...
     //modules/planning/...
     //modules/prediction/...
     //modules/routing/...
@@ -209,17 +208,26 @@ function cibuild() {
 
   info "Building with $JOB_ARG for $MACHINE_ARCH"
   BUILD_TARGETS="
+    //modules/canbus/...
     //modules/common/...
     //modules/control/...
+    //modules/dreamview/...
+    //modules/drivers/radar/conti_radar/...
+    //modules/drivers/gnss/...
+    //modules/drivers/velodyne/...
+    //modules/drivers/usb_cam/...
+    //modules/drivers/radar/ultrasonic_radar/...
+    //modules/guardian/...
     //modules/localization/proto/...
     //modules/localization/rtk/...
     `bazel query //modules/localization/msf/... except //modules/localization/msf/local_tool/...`
     `bazel query //modules/localization/msf/local_tool/local_visualization/...`
-    //modules/perception/common/...
-    //modules/perception/proto/...
+    //modules/map/...
+    //modules/perception/...
     //modules/planning/...
     //modules/prediction/...
-    //modules/routing/..."
+    //modules/routing/...
+    //modules/third_party_perception/..."
 
   bazel build $JOB_ARG $DEFINES $@ $BUILD_TARGETS
 

modules/perception/common/graph/BUILD

@@ -50,4 +50,48 @@ cc_test(
     ],
 )
 
+cc_library(
+    name = "secure_matrix",
+    hdrs = [
+        "secure_matrix.h",
+    ],
+    deps = [
+        "@eigen",
+    ],
+)
+
+cc_test(
+    name = "secure_matrix_test",
+    size = "small",
+    srcs = [
+        "secure_matrix_test.cc",
+    ],
+    deps = [
+        ":secure_matrix",
+        "@gtest//:main",
+        "@eigen",
+    ],
+)
+cc_library(
+    name = "hungarian_optimizer",
+    hdrs = [
+        "hungarian_optimizer.h",
+    ],
+    deps = [
+        "@eigen",
+    ],
+)
+
+cc_test(
+    name = "hungarian_optimizer_test",
+    size = "small",
+    srcs = [
+        "hungarian_optimizer_test.cc",
+    ],
+    deps = [
+        ":hungarian_optimizer",
+        ":secure_matrix",
+        "@gtest//:main",
+    ],
+)
 cpplint()

modules/perception/common/graph/hungarian_optimizer_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/hungarian_optimizer.h"
+
+#include "Eigen/Core"
+#include "gtest/gtest.h"
+
+namespace apollo {
+namespace perception {
+namespace common {
+
+class HungarianOptimizerTest : public testing::Test {
+ public:
+  HungarianOptimizerTest() : optimizer_(nullptr) {}
+  ~HungarianOptimizerTest() {}
+
+ protected:
+  void SetUp() { optimizer_ = new HungarianOptimizer<float>(); }
+  void TearDown() {
+    delete optimizer_;
+    optimizer_ = nullptr;
+  }
+
+ public:
+  HungarianOptimizer<float>* optimizer_;
+};  // class HungarianOptimizerTest
+
+TEST_F(HungarianOptimizerTest, test_Minimize) {
+  std::vector<std::pair<size_t, size_t>> assignments;
+  optimizer_->costs()->Reserve(1000, 1000);
+
+  /* case 1: most basic one
+   * costs:
+   * 0.1,  1.0
+   * 1.0,  0.1
+   * matches:
+   * (0->0, 1->1) */
+  optimizer_->costs()->Resize(2, 2);
+  (*optimizer_->costs())(0, 0) = 0.1;
+  (*optimizer_->costs())(1, 0) = 1.0;
+  (*optimizer_->costs())(0, 1) = 1.0;
+  (*optimizer_->costs())(1, 1) = 0.1;
+
+  optimizer_->Minimize(&assignments);
+  optimizer_->PrintMatrix();
+
+  EXPECT_EQ(2, assignments.size());
+  EXPECT_EQ(0, assignments[0].first);
+  EXPECT_EQ(0, assignments[0].second);
+  EXPECT_EQ(1, assignments[1].first);
+  EXPECT_EQ(1, assignments[1].second);
+
+  /* case 2: special one with all 0s
+   * costs:
+   * 0.0,  0.0
+   * 0.0,  0.0
+   * matches:
+   * (0->0, 1->1) */
+  optimizer_->costs()->Resize(2, 2);
+  (*optimizer_->costs())(0, 0) = 0.0;
+  (*optimizer_->costs())(0, 1) = 0.0;
+  (*optimizer_->costs())(1, 0) = 0.0;
+  (*optimizer_->costs())(1, 1) = 0.0;
+
+  optimizer_->Minimize(&assignments);
+
+  EXPECT_EQ(2, assignments.size());
+  EXPECT_EQ(0, assignments[0].first);
+  EXPECT_EQ(0, assignments[0].second);
+  EXPECT_EQ(1, assignments[1].first);
+  EXPECT_EQ(1, assignments[1].second);
+
+  /* case 3: special one with all the same values
+   * costs:
+   * 3.0,  3.0
+   * 3.0,  3.0
+   * matches:
+   * (0->0, 1->1) */
+  optimizer_->costs()->Resize(2, 2);
+  (*optimizer_->costs())(0, 0) = 3.0;
+  (*optimizer_->costs())(0, 1) = 3.0;
+  (*optimizer_->costs())(1, 0) = 3.0;
+  (*optimizer_->costs())(1, 1) = 3.0;
+
+  optimizer_->Minimize(&assignments);
+
+  EXPECT_EQ(2, assignments.size());
+  EXPECT_EQ(0, assignments[0].first);
+  EXPECT_EQ(0, assignments[0].second);
+  EXPECT_EQ(1, assignments[1].first);
+  EXPECT_EQ(1, assignments[1].second);
+
+  /* case 4: further one with more cols
+   * costs:
+   * 4.7,  3.8,  1.0,  2.0
+   * 4.1,  3.0,  2.0,  3.0
+   * 1.0,  2.0,  4.7,  4.9
+   * matches:
+   * (0->2, 1->1, 2->0) */
+  optimizer_->costs()->Resize(3, 4);
+  (*optimizer_->costs())(0, 0) = 4.7;
+  (*optimizer_->costs())(0, 1) = 3.8;
+  (*optimizer_->costs())(0, 2) = 1.0;
+  (*optimizer_->costs())(0, 3) = 2.0;
+  (*optimizer_->costs())(1, 0) = 4.1;
+  (*optimizer_->costs())(1, 1) = 3.0;
+  (*optimizer_->costs())(1, 2) = 2.0;
+  (*optimizer_->costs())(1, 3) = 3.0;
+  (*optimizer_->costs())(2, 0) = 1.0;
+  (*optimizer_->costs())(2, 1) = 2.0;
+  (*optimizer_->costs())(2, 2) = 4.7;
+  (*optimizer_->costs())(2, 3) = 4.9;
+
+  optimizer_->Minimize(&assignments);
+
+  EXPECT_EQ(3, assignments.size());
+  EXPECT_EQ(0, assignments[0].first);
+  EXPECT_EQ(2, assignments[0].second);
+  EXPECT_EQ(1, assignments[1].first);
+  EXPECT_EQ(1, assignments[1].second);
+  EXPECT_EQ(2, assignments[2].first);
+  EXPECT_EQ(0, assignments[2].second);
+
+  /* case 5: further one with more rows
+   * costs:
+   * 4.7,  3.8,  1.0
+   * 4.1,  3.0,  2.0
+   * 1.0,  2.0,  4.7
+   * 3.2,  2.1,  0.5
+   * matches:
+   * (0->2, 2->0, 3->1) */
+  optimizer_->costs()->Resize(4, 3);
+  (*optimizer_->costs())(0, 0) = 4.7;
+  (*optimizer_->costs())(0, 1) = 3.8;
+  (*optimizer_->costs())(0, 2) = 1.0;
+  (*optimizer_->costs())(1, 0) = 4.1;
+  (*optimizer_->costs())(1, 1) = 3.0;
+  (*optimizer_->costs())(1, 2) = 2.0;
+  (*optimizer_->costs())(2, 0) = 1.0;
+  (*optimizer_->costs())(2, 1) = 2.0;
+  (*optimizer_->costs())(2, 2) = 4.7;
+  (*optimizer_->costs())(3, 0) = 3.2;
+  (*optimizer_->costs())(3, 1) = 2.1;
+  (*optimizer_->costs())(3, 2) = 0.5;
+
+  optimizer_->Minimize(&assignments);
+
+  EXPECT_EQ(3, assignments.size());
+  EXPECT_EQ(0, assignments[0].first);
+  EXPECT_EQ(2, assignments[0].second);
+  EXPECT_EQ(2, assignments[1].first);
+  EXPECT_EQ(0, assignments[1].second);
+  EXPECT_EQ(3, assignments[2].first);
+  EXPECT_EQ(1, assignments[2].second);
+
+  /* case 6: empty one */
+  optimizer_->costs()->Resize(0, 0);
+  optimizer_->Minimize(&assignments);
+  EXPECT_EQ(0, assignments.size());
+}
+
+TEST_F(HungarianOptimizerTest, test_Maximize) {
+  std::vector<std::pair<size_t, size_t>> assignments;
+  optimizer_->costs()->Reserve(1000, 1000);
+
+  /* case 1: most basic one
+   * costs:
+   * 0.1,  1.0
+   * 1.0,  0.1
+   * matches:
+   * (0->1, 1->0) */
+  optimizer_->costs()->Resize(2, 2);
+  (*optimizer_->costs())(0, 0) = 0.1;
+  (*optimizer_->costs())(1, 0) = 1.0;
+  (*optimizer_->costs())(0, 1) = 1.0;
+  (*optimizer_->costs())(1, 1) = 0.1;
+
+  optimizer_->Maximize(&assignments);
+
+  EXPECT_EQ(2, assignments.size());
+  EXPECT_EQ(0, assignments[0].first);
+  EXPECT_EQ(1, assignments[0].second);
+  EXPECT_EQ(1, assignments[1].first);
+  EXPECT_EQ(0, assignments[1].second);
+
+  /* case 2: special one with all 0s
+   * costs:
+   * 0.0,  0.0
+   * 0.0,  0.0
+   * matches:
+   * (0->0, 1->1) */
+  optimizer_->costs()->Resize(2, 2);
+  (*optimizer_->costs())(0, 0) = 0.0;
+  (*optimizer_->costs())(0, 1) = 0.0;
+  (*optimizer_->costs())(1, 0) = 0.0;
+  (*optimizer_->costs())(1, 1) = 0.0;
+
+  optimizer_->Maximize(&assignments);
+
+  EXPECT_EQ(2, assignments.size());
+  EXPECT_EQ(0, assignments[0].first);
+  EXPECT_EQ(0, assignments[0].second);
+  EXPECT_EQ(1, assignments[1].first);
+  EXPECT_EQ(1, assignments[1].second);
+
+  /* case 3: special one with all the same values
+   * costs:
+   * 3.0,  3.0
+   * 3.0,  3.0
+   * matches:
+   * (0->0, 1->1) */
+  optimizer_->costs()->Resize(2, 2);
+  (*optimizer_->costs())(0, 0) = 3.0;
+  (*optimizer_->costs())(0, 1) = 3.0;
+  (*optimizer_->costs())(1, 0) = 3.0;
+  (*optimizer_->costs())(1, 1) = 3.0;
+
+  optimizer_->Maximize(&assignments);
+
+  EXPECT_EQ(2, assignments.size());
+  EXPECT_EQ(0, assignments[0].first);
+  EXPECT_EQ(0, assignments[0].second);
+  EXPECT_EQ(1, assignments[1].first);
+  EXPECT_EQ(1, assignments[1].second);
+
+  /* case 4: further one with more cols
+   * costs:
+   * 4.7,  3.8,  1.0,  2.0
+   * 4.1,  3.0,  2.0,  3.0
+   * 1.0,  2.0,  4.7,  4.9
+   * matches:
+   * (0->1, 1->0, 2->3) */
+  optimizer_->costs()->Resize(3, 4);
+  (*optimizer_->costs())(0, 0) = 4.7;
+  (*optimizer_->costs())(0, 1) = 3.8;
+  (*optimizer_->costs())(0, 2) = 1.0;
+  (*optimizer_->costs())(0, 3) = 2.0;
+  (*optimizer_->costs())(1, 0) = 4.1;
+  (*optimizer_->costs())(1, 1) = 3.0;
+  (*optimizer_->costs())(1, 2) = 2.0;
+  (*optimizer_->costs())(1, 3) = 3.0;
+  (*optimizer_->costs())(2, 0) = 1.0;
+  (*optimizer_->costs())(2, 1) = 2.0;
+  (*optimizer_->costs())(2, 2) = 4.7;
+  (*optimizer_->costs())(2, 3) = 4.9;
+
+  optimizer_->Maximize(&assignments);
+
+  EXPECT_EQ(3, assignments.size());
+  EXPECT_EQ(0, assignments[0].first);
+  EXPECT_EQ(1, assignments[0].second);
+  EXPECT_EQ(1, assignments[1].first);
+  EXPECT_EQ(0, assignments[1].second);
+  EXPECT_EQ(2, assignments[2].first);
+  EXPECT_EQ(3, assignments[2].second);
+
+  /* case 5: further one with more rows
+   * costs:
+   * 4.7,  3.8,  1.0
+   * 4.1,  3.0,  2.0
+   * 1.0,  2.0,  4.7
+   * 3.2,  2.1,  0.5
+   * matches:
+   * (0->1, 1->0, 2->2) */
+  optimizer_->costs()->Resize(4, 3);
+  (*optimizer_->costs())(0, 0) = 4.7;
+  (*optimizer_->costs())(0, 1) = 3.8;
+  (*optimizer_->costs())(0, 2) = 1.0;
+  (*optimizer_->costs())(1, 0) = 4.1;
+  (*optimizer_->costs())(1, 1) = 3.0;
+  (*optimizer_->costs())(1, 2) = 2.0;
+  (*optimizer_->costs())(2, 0) = 1.0;
+  (*optimizer_->costs())(2, 1) = 2.0;
+  (*optimizer_->costs())(2, 2) = 4.7;
+  (*optimizer_->costs())(3, 0) = 3.2;
+  (*optimizer_->costs())(3, 1) = 2.1;
+  (*optimizer_->costs())(3, 2) = 0.5;
+
+  optimizer_->Maximize(&assignments);
+
+  EXPECT_EQ(3, assignments.size());
+  EXPECT_EQ(0, assignments[0].first);
+  EXPECT_EQ(1, assignments[0].second);
+  EXPECT_EQ(1, assignments[1].first);
+  EXPECT_EQ(0, assignments[1].second);
+  EXPECT_EQ(2, assignments[2].first);
+  EXPECT_EQ(2, assignments[2].second);
+
+  /* case 6: empty one */
+  optimizer_->costs()->Resize(0, 0);
+  optimizer_->Maximize(&assignments);
+  EXPECT_EQ(0, assignments.size());
+}
+
+}  // namespace common
+}  // namespace perception
+}  // namespace apollo

modules/perception/common/graph/secure_matrix.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_SECURE_MATRIX_H_
+#define MODULES_PERCEPTION_COMMON_GRAPH_SECURE_MATRIX_H_
+
+#include <memory>
+
+#include "Eigen/Dense"
+
+namespace apollo {
+namespace perception {
+namespace common {
+
+template <typename T>
+class SecureMat {
+ public:
+  SecureMat() : height_(0), width_(0) { Reserve(max_height_, max_width_); }
+
+  size_t height() { return height_; }
+  size_t width() { return width_; }
+
+  /* @brief: reserve memory of SecureMat
+   * @params[IN] reserve_height: height of reserve memory
+   * @params[IN] reserve_width: width of reserve memory
+   * @return nothing */
+  void Reserve(const size_t reserve_height, const size_t reserve_width) {
+    max_height_ = (reserve_height > max_height_) ? reserve_height : max_height_;
+    max_width_ = (reserve_width > max_width_) ? reserve_width : max_width_;
+    mat_.resize(max_height_, max_width_);
+  }
+
+  /* @brief: resize memory of SecureMat
+   * @params[IN] resize_height: height of resize memory
+   * @params[IN] resize_width: width of resize memory
+   * @return nothing */
+  void Resize(const size_t resize_height, const size_t resize_width) {
+    height_ = resize_height;
+    width_ = resize_width;
+    if (resize_height <= max_height_ && resize_width <= max_width_) {
+      return;
+    }
+    max_height_ = (resize_height > max_height_) ? resize_height : max_height_;
+    max_width_ = (resize_width > max_width_) ? resize_width : max_width_;
+    mat_.resize(max_height_, max_width_);
+  }
+
+  void ToString(std::ostream* out_stream) {
+    std::ostream& stream = *out_stream;
+    for (size_t row = 0; row < height_; ++row) {
+      for (size_t col = 0; col < width_; ++col) {
+        stream << mat_(row, col) << "\t";
+      }
+      stream << "\n";
+    }
+  }
+
+  inline const T& operator()(const size_t row, const size_t col) const {
+    return mat_(row, col);
+  }
+
+  inline T& operator()(const size_t row, const size_t col) {
+    return mat_(row, col);
+  }
+
+ private:
+  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> mat_;
+  size_t max_height_ = 1000;
+  size_t max_width_ = 1000;
+  size_t height_ = 0;
+  size_t width_ = 0;
+};  // class SecureMat
+
+}  // namespace common
+}  // namespace perception
+}  // namespace apollo
+
+#endif  // MODULES_PERCEPTION_COMMON_GRAPH_SECURE_MATRIX_H_

modules/perception/common/graph/secure_matrix_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/secure_matrix.h"
+
+#include "Eigen/Core"
+#include "gtest/gtest.h"
+
+namespace apollo {
+namespace perception {
+namespace common {
+
+TEST(SecureMatTest, test_reserve_mat) {
+  SecureMat<float> test_mat;
+  test_mat.Reserve(100, 100);
+  test_mat.Resize(10, 20);
+  EXPECT_EQ(10, test_mat.height());
+  EXPECT_EQ(20, test_mat.width());
+
+  test_mat.Reserve(1200, 1200);
+  EXPECT_EQ(10, test_mat.height());
+  EXPECT_EQ(20, test_mat.width());
+}
+
+TEST(SecureMatTest, test_resize_mat) {
+  SecureMat<float> test_mat;
+  test_mat.Reserve(100, 100);
+  test_mat.Resize(3, 2);
+  EXPECT_EQ(3, test_mat.height());
+  EXPECT_EQ(2, test_mat.width());
+
+  test_mat.Resize(500, 1500);
+  EXPECT_EQ(500, test_mat.height());
+  EXPECT_EQ(1500, test_mat.width());
+
+  test_mat.Resize(2000, 1500);
+  EXPECT_EQ(2000, test_mat.height());
+  EXPECT_EQ(1500, test_mat.width());
+}
+
+TEST(SecureMatTest, test_fill_mat) {
+  SecureMat<float> test_mat;
+  test_mat.Reserve(10, 10);
+  test_mat.Resize(2, 2);
+  test_mat(0, 0) = 1;
+  test_mat(0, 1) = 2;
+  test_mat(1, 0) = 3;
+  test_mat(1, 1) = 4;
+  test_mat.ToString(&std::cout);
+  EXPECT_EQ(1, test_mat(0, 0));
+  EXPECT_EQ(2, test_mat(0, 1));
+  EXPECT_EQ(3, test_mat(1, 0));
+  EXPECT_EQ(4, test_mat(1, 1));
+}
+
+}  // namespace common
+}  // namespace perception
+}  // namespace apollo