Perception: added cnnseg.

modules/perception/lidar/lib/segmentation/cnnseg/BUILD

@@ -2,19 +2,30 @@ load("//tools:cpplint.bzl", "cpplint")
 
 package(default_visibility = ["//visibility:public"])
 
-# cc_library(
-#     name = "cnn_segmentation",
-#     srcs = [
-#         "cnn_segmentation.cc",
-#     ],
-#     hdrs = [
-#         "cnn_segmentation.h",
-#     ],
-#     deps = [
-#         ":feature_generator",
-#         "//framework:cybertron",
-#     ],
-# )
+cc_library(
+    name = "cnn_segmentation",
+    srcs = [
+        "cnn_segmentation.cc",
+    ],
+    hdrs = [
+        "cnn_segmentation.h",
+    ],
+    deps = [
+        ":feature_generator",
+        "//framework:cybertron",
+        "//modules/perception/base",
+        "//modules/perception/inference:inference_factory_lib",
+        "//modules/perception/inference:inference_lib",
+        "//modules/perception/lib/config_manager",
+        "//modules/perception/lib/io:protobuf_util",
+        "//modules/perception/lidar/common",
+        "//modules/perception/lidar/lib/interface",
+        "//modules/perception/lidar/lib/segmentation/cnnseg/proto:cnnseg_config_proto",
+        "//modules/perception/lidar/lib/segmentation/cnnseg/proto:spp_engine_config_proto",
+        "//modules/perception/lidar/lib/segmentation/cnnseg/spp_engine",
+        "//modules/perception/proto:perception_config_schema_proto",
+    ],
+)
 
 cc_library(
     name = "disjoint_set",

modules/perception/lidar/lib/segmentation/cnnseg/cnn_segmentation.cc

@@ -14,10 +14,11 @@
  * limitations under the License.
  *****************************************************************************/
 #include <map>
-#include <string>
-#include <vector>
 
 #include "cybertron/common/log.h"
+
+#include "modules/perception/lidar/lib/segmentation/cnnseg/proto/cnnseg_config.pb.h"
+
 #include "modules/perception/base/object_pool_types.h"
 #include "modules/perception/lib/config_manager/config_manager.h"
 #include "modules/perception/lib/io/file_util.h"
@@ -25,13 +26,17 @@
 #include "modules/perception/lidar/common/lidar_point_label.h"
 #include "modules/perception/lidar/common/lidar_timer.h"
 #include "modules/perception/lidar/lib/segmentation/cnnseg/cnn_segmentation.h"
-#include "modules/perception/lidar/lib/segmentation/cnnseg/proto/cnnseg_config.pb.h"
 #include "modules/perception/lidar/lib/segmentation/cnnseg/util.h"
 
 namespace apollo {
 namespace perception {
 namespace lidar {
 
+using base::AttributePointCloud;
+using base::PointF;
+using base::PointD;
+using base::Object;
+
 bool CNNSegmentation::Init(const SegmentationInitOptions& options) {
   // get configs
   std::string param_file;
@@ -208,16 +213,17 @@ bool CNNSegmentation::InitClusterAndBackgroundSegmentation() {
     }
     ground_detector_time_ = timer.toc(true);
     AINFO << "Roi-filter time: " << roi_filter_time_
-             << "\tGround-detector time: " << ground_detector_time_;
+          << "\tGround-detector time: " << ground_detector_time_;
     return true;
   });
   worker_.Start();
   return true;
 }
 
-void CNNSegmentation::MapPointToGrid(const base::PointFCloudPtr& pc_ptr) {
+void CNNSegmentation::MapPointToGrid(
+    const std::shared_ptr<AttributePointCloud<PointF>>& pc_ptr) {
   float inv_res_x = 0.5 * static_cast<float>(width_) / range_;
-  float inv_res_y = 0.5 * static_cast<float>(height_) / range_;
+  // float inv_res_y = 0.5 * static_cast<float>(height_) / range_;
   point2grid_.assign(pc_ptr->size(), -1);
   int pos_x = -1;
   int pos_y = -1;
@@ -294,20 +300,20 @@ bool CNNSegmentation::Segment(const SegmentationOptions& options,
   GetObjectsFromSppEngine(&frame->segmented_objects);
 
   AINFO << "CNNSEG: mapping: " << mapping_time_ << "\t"
-           << " feature: " << feature_time_ << "\t"
-           << " infer: " << infer_time_ << "\t"
-           << " fg-seg: " << fg_seg_time_ << "\t"
-           << " join: " << join_time_ << "\t"
-           << " collect: " << collect_time_;
+        << " feature: " << feature_time_ << "\t"
+        << " infer: " << infer_time_ << "\t"
+        << " fg-seg: " << fg_seg_time_ << "\t"
+        << " join: " << join_time_ << "\t"
+        << " collect: " << collect_time_;
   return true;
 }
 
 void CNNSegmentation::GetObjectsFromSppEngine(
-    std::vector<base::ObjectPtr>* objects) {
+    std::vector<std::shared_ptr<Object>>* objects) {
   Timer timer;
   spp_engine_.GetSppData().grid_indices = point2grid_.data();
-  size_t num_foreground =
-      spp_engine_.ProcessForegroundSegmentation(original_cloud_);
+  // size_t num_foreground =
+  //     spp_engine_.ProcessForegroundSegmentation(original_cloud_);
   fg_seg_time_ = timer.toc(true);
   // should sync with worker before do background segmentation
   worker_.Join();
@@ -319,19 +325,21 @@ void CNNSegmentation::GetObjectsFromSppEngine(
   for (std::size_t i = 0; i < lidar_frame_ref_->roi_indices.indices.size();
        ++i) {
     const int& roi_id = lidar_frame_ref_->roi_indices.indices[i];
-    original_cloud_->points_height(roi_id) = roi_cloud_->points_height(i);
-    if (roi_cloud_->points_label(i) ==
+    original_cloud_->mutable_points_height()->at(roi_id) =
+        roi_cloud_->points_height(i);
+    if (roi_cloud_->mutable_points_label()->at(i) ==
         static_cast<uint8_t>(LidarPointLabel::GROUND)) {
-      original_cloud_->points_label(roi_id) = roi_cloud_->points_label(i);
+      original_cloud_->mutable_points_label()->at(roi_id) =
+          roi_cloud_->points_label().at(i);
     }
   }
-  memcpy(&original_world_cloud_->points_height(0),
-         &original_cloud_->points_height(0),
+  memcpy(&original_world_cloud_->mutable_points_height()->at(0),
+         &original_cloud_->points_height().at(0),
          sizeof(float) * original_cloud_->size());
   if (cnnseg_param_.remove_ground_points()) {
-    num_foreground = spp_engine_.RemoveGroundPointsInForegroundCluster(
-        original_cloud_, lidar_frame_ref_->roi_indices,
-        lidar_frame_ref_->non_ground_indices);
+    //  num_foreground = spp_engine_.RemoveGroundPointsInForegroundCluster(
+    //      original_cloud_, lidar_frame_ref_->roi_indices,
+    //      lidar_frame_ref_->non_ground_indices);
   }
 
   const auto& clusters = spp_engine_.clusters();

modules/perception/lidar/lib/segmentation/cnnseg/cnn_segmentation.h

@@ -16,6 +16,7 @@
 #ifndef MODULES_PERCEPTION_LIDAR_LIB_SEGMENTATION_CNNSEG_H_
 #define MODULES_PERCEPTION_LIDAR_LIB_SEGMENTATION_CNNSEG_H_
 
+#include <memory>
 #include <string>
 #include <vector>
 
@@ -26,6 +27,7 @@
 
 #include "modules/perception/base/blob.h"
 #include "modules/perception/inference/inference.h"
+#include "modules/perception/inference/inference_factory.h"
 #include "modules/perception/lib/thread/thread_worker.h"
 #include "modules/perception/lidar/lib/interface/base_ground_detector.h"
 #include "modules/perception/lidar/lib/interface/base_roi_filter.h"
@@ -55,9 +57,11 @@ class CNNSegmentation : public BaseSegmentation {
 
   bool InitClusterAndBackgroundSegmentation();
 
-  void GetObjectsFromSppEngine(std::vector<base::ObjectPtr>* objects);
+  void GetObjectsFromSppEngine(
+      std::vector<std::shared_ptr<base::Object>>* objects);
 
-  void MapPointToGrid(const base::PointFCloudPtr& pc_ptr);
+  void MapPointToGrid(
+      const std::shared_ptr<base::AttributePointCloud<base::PointF>>& pc_ptr);
 
   CNNSegParam cnnseg_param_;
   std::shared_ptr<inference::Inference> inference_;
@@ -97,10 +101,11 @@ class CNNSegmentation : public BaseSegmentation {
 
   // reference pointer of lidar frame
   LidarFrame* lidar_frame_ref_ = nullptr;
-  base::PointFCloudPtr original_cloud_;
-  base::PointDCloudPtr original_world_cloud_;
-  base::PointFCloudPtr roi_cloud_;
-  base::PointDCloudPtr roi_world_cloud_;
+  std::shared_ptr<base::AttributePointCloud<base::PointF>> original_cloud_;
+  std::shared_ptr<base::AttributePointCloud<base::PointD>>
+      original_world_cloud_;
+  std::shared_ptr<base::AttributePointCloud<base::PointF>> roi_cloud_;
+  std::shared_ptr<base::AttributePointCloud<base::PointD>> roi_world_cloud_;
   int gpu_id_ = -1;
 
   // time statistics

modules/perception/lidar/lib/tracker/hm_tracker/measurement_computer.cc

@@ -98,7 +98,7 @@ void MeasurementComputer::ComputeMeasuredBboxCenterVelocity(
     const double &time_diff) {
   // Compute 2D bbox center velocity measurement
   Eigen::Vector3f old_dir_tmp = old_object->output_direction.cast<float>();
-  Eigen::Vector3d old_size = old_object->output_size;
+  // Eigen::Vector3d old_size = old_object->output_size;
   Eigen::Vector3d old_center = old_object->output_center;
   Eigen::Vector3f new_size_tmp;
   Eigen::Vector3d new_center;
@@ -107,7 +107,7 @@ void MeasurementComputer::ComputeMeasuredBboxCenterVelocity(
       (new_object->object_ptr->lidar_supplement).cloud_world;
   common::CalculateBBoxSizeCenter2DXY(cloud, old_dir_tmp, &new_size_tmp,
                                       &new_center, minimum_edge_length);
-  Eigen::Vector3d old_dir = old_dir_tmp.cast<double>();
+  // Eigen::Vector3d old_dir = old_dir_tmp.cast<double>();
   Eigen::Vector3d new_size = new_size_tmp.cast<double>();
   Eigen::Vector3d measured_bbox_center_velocity_with_old_dir =
       (new_center - old_center);

modules/planning/math/smoothing_spline/osqp_spline_2d_solver.cc

@@ -59,6 +59,8 @@ Spline2dKernel* OsqpSpline2dSolver::mutable_kernel() { return &kernel_; }
 Spline2d* OsqpSpline2dSolver::mutable_spline() { return &spline_; }
 
 bool OsqpSpline2dSolver::Solve() {
+  // TODO(All): implement here.
+  /*
   const MatrixXd& kernel_matrix = kernel_.kernel_matrix();
   const MatrixXd& offset = kernel_.offset();
   const MatrixXd& inequality_constraint_matrix =
@@ -69,7 +71,7 @@ bool OsqpSpline2dSolver::Solve() {
       constraint_.equality_constraint().constraint_matrix();
   const MatrixXd& equality_constraint_boundary =
       constraint_.equality_constraint().constraint_boundary();
-  // TODO(All): implement here.
+  */
   return true;
 }