Browse Source

change detection_lidar_transfusion. compile ok, test not complete.

yuchuli 3 months ago
parent
commit
dd3065addd

+ 1 - 1
src/detection/detection_lidar_centerpoint/main.cpp

@@ -37,7 +37,7 @@ void init()
     yaw_norm_thresholds.push_back(0.3);yaw_norm_thresholds.push_back(0.0);
     const std::string densification_world_frame_id = "map";
     const int densification_num_past_frames = 0;//1;
-    const std::string trt_precision = "fp16";
+    const std::string trt_precision = "fp32";
     const std::string encoder_onnx_path = "/home/nvidia/models/pts_voxel_encoder_centerpoint.onnx";//this->declare_parameter<std::string>("encoder_onnx_path");
     const std::string encoder_engine_path ="/home/nvidia/models/pts_voxel_encoder_centerpoint.eng";//this->declare_parameter<std::string>("encoder_engine_path");
     const std::string head_onnx_path = "/home/nvidia/models/pts_backbone_neck_head_centerpoint.onnx";//this->declare_parameter<std::string>("head_onnx_path");

+ 71 - 0
src/detection/detection_lidar_transfusion/detection_class_remapper.cpp

@@ -0,0 +1,71 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// 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 <detection_class_remapper.hpp>
+
+namespace autoware::lidar_transfusion
+{
+
+void DetectionClassRemapper::setParameters(
+  const std::vector<int64_t> & allow_remapping_by_area_matrix,
+  const std::vector<double> & min_area_matrix, const std::vector<double> & max_area_matrix)
+{
+  assert(allow_remapping_by_area_matrix.size() == min_area_matrix.size());
+  assert(allow_remapping_by_area_matrix.size() == max_area_matrix.size());
+  assert(
+    std::pow(static_cast<int>(std::sqrt(min_area_matrix.size())), 2) == min_area_matrix.size());
+
+  num_labels_ = static_cast<int>(std::sqrt(min_area_matrix.size()));
+
+  Eigen::Map<const Eigen::Matrix<int64_t, Eigen::Dynamic, Eigen::Dynamic>>
+    allow_remapping_by_area_matrix_tmp(
+      allow_remapping_by_area_matrix.data(), num_labels_, num_labels_);
+  allow_remapping_by_area_matrix_ = allow_remapping_by_area_matrix_tmp.transpose()
+                                      .cast<bool>();  // Eigen is column major by default
+
+  Eigen::Map<const Eigen::MatrixXd> min_area_matrix_tmp(
+    min_area_matrix.data(), num_labels_, num_labels_);
+  min_area_matrix_ = min_area_matrix_tmp.transpose();  // Eigen is column major by default
+
+  Eigen::Map<const Eigen::MatrixXd> max_area_matrix_tmp(
+    max_area_matrix.data(), num_labels_, num_labels_);
+  max_area_matrix_ = max_area_matrix_tmp.transpose();  // Eigen is column major by default
+
+  min_area_matrix_ = min_area_matrix_.unaryExpr(
+    [](double v) { return std::isfinite(v) ? v : std::numeric_limits<double>::max(); });
+  max_area_matrix_ = max_area_matrix_.unaryExpr(
+    [](double v) { return std::isfinite(v) ? v : std::numeric_limits<double>::max(); });
+}
+
+//void DetectionClassRemapper::mapClasses(autoware_perception_msgs::msg::DetectedObjects & msg)
+//{
+//  for (auto & object : msg.objects) {
+//    const float bev_area = object.shape.dimensions.x * object.shape.dimensions.y;
+
+//    for (auto & classification : object.classification) {
+//      auto & label = classification.label;
+
+//      for (int i = 0; i < num_labels_; ++i) {
+//        if (
+//          allow_remapping_by_area_matrix_(label, i) && bev_area >= min_area_matrix_(label, i) &&
+//          bev_area <= max_area_matrix_(label, i)) {
+//          label = i;
+//          break;
+//        }
+//      }
+//    }
+//  }
+//}
+
+}  // namespace autoware::lidar_transfusion

+ 47 - 0
src/detection/detection_lidar_transfusion/detection_class_remapper.hpp

@@ -0,0 +1,47 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// 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 AUTOWARE__LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_
+#define AUTOWARE__LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_
+
+#include <Eigen/Core>
+
+//#include <autoware_perception_msgs/msg/detected_object_kinematics.hpp>
+//#include <autoware_perception_msgs/msg/detected_objects.hpp>
+//#include <autoware_perception_msgs/msg/object_classification.hpp>
+//#include <autoware_perception_msgs/msg/shape.hpp>
+
+#include <vector>
+
+namespace autoware::lidar_transfusion
+{
+
+class DetectionClassRemapper
+{
+public:
+  void setParameters(
+    const std::vector<int64_t> & allow_remapping_by_area_matrix,
+    const std::vector<double> & min_area_matrix, const std::vector<double> & max_area_matrix);
+//  void mapClasses(autoware_perception_msgs::msg::DetectedObjects & msg);
+
+protected:
+  Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> allow_remapping_by_area_matrix_;
+  Eigen::MatrixXd min_area_matrix_;
+  Eigen::MatrixXd max_area_matrix_;
+  int num_labels_;
+};
+
+}  // namespace autoware::lidar_transfusion
+
+#endif  // AUTOWARE__LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_

+ 22 - 0
src/detection/detection_lidar_transfusion/detection_lidar_transfusion.pro

@@ -8,6 +8,7 @@ CONFIG -= app_bundle
 #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
 
 SOURCES += \
+        detection_class_remapper.cpp \
         main.cpp \
         network/network_trt.cpp \
         postprocess/non_maximum_suppression.cpp \
@@ -15,6 +16,7 @@ SOURCES += \
         preprocess/voxel_generator.cpp \
         simple_profiler.cpp \
         tensorrt_common.cpp \
+        transfusion_call.cpp \
         transfusion_trt.cpp
 
 # Default rules for deployment.
@@ -29,6 +31,7 @@ DISTFILES += \
 
 HEADERS += \
     cuda_utils.hpp \
+    detection_class_remapper.hpp \
     include/network/network_trt.hpp \
     include/postprocess/circle_nms_kernel.hpp \
     include/postprocess/non_maximum_suppression.hpp \
@@ -39,6 +42,7 @@ HEADERS += \
     include/tensorrt_common/logger.hpp \
     include/tensorrt_common/simple_profiler.hpp \
     include/tensorrt_common/tensorrt_common.hpp \
+    transfusion_call.h \
     transfusion_trt.hpp \
     utils.hpp \
     visibility_control.hpp
@@ -143,5 +147,23 @@ else {
 # include paths
 
 
+unix:LIBS +=  -lpcl_common\
+        -lpcl_features\
+        -lpcl_filters\
+        -lpcl_io\
+        -lpcl_io_ply\
+        -lpcl_kdtree\
+        -lpcl_keypoints\
+        -lpcl_octree\
+        -lpcl_outofcore\
+        -lpcl_people\
+        -lpcl_recognition\
+        -lpcl_registration\
+        -lpcl_sample_consensus\
+        -lpcl_search\
+        -lpcl_segmentation\
+        -lpcl_surface\
+        -lpcl_tracking\
+        -lpcl_visualization
 
 LIBS += -lrt -ldl -lnvinfer -lcudnn  -lcudart -lnvparsers -lnvonnxparser -lnvinfer_plugin -lstdc++fs

+ 1 - 12
src/detection/detection_lidar_transfusion/include/postprocess/non_maximum_suppression.hpp

@@ -46,18 +46,7 @@ struct NMSParams
   // double distance_threshold_{};
 };
 
-std::vector<bool> classNamesToBooleanMask(const std::vector<std::string> & class_names)
-{
-  std::vector<bool> mask;
-  constexpr std::size_t num_object_classification = 8;
-  mask.resize(num_object_classification);
-  for (const auto & class_name : class_names) {
-    const auto semantic_type = getSemanticType(class_name);
-    mask.at(semantic_type) = true;
-  }
-
-  return mask;
-}
+
 
 class NonMaximumSuppression
 {

+ 27 - 0
src/detection/detection_lidar_transfusion/main.cpp

@@ -1,8 +1,35 @@
 #include <QCoreApplication>
 
+#include "transfusion_call.h"
+
+transfusion_call * gptf;
+
+#include <pcl/io/pcd_io.h>
+void testdet(std::string & path)
+{
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+
+    pcl::io::loadPCDFile<pcl::PointXYZI>(path,*point_cloud);
+
+    std::vector<Box3D> det_boxes3d;
+    int i;
+    for(i=0;i<10;i++)
+    {
+    gptf ->detect(point_cloud,det_boxes3d);
+    std::cout<<" box size: "<<det_boxes3d.size()<<std::endl;
+    }
+}
+
+
 int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
 
+    gptf = new transfusion_call();
+
+    std::string strpath = "/home/nvidia/1.pcd";
+    testdet(strpath);
+
     return a.exec();
 }

+ 16 - 0
src/detection/detection_lidar_transfusion/postprocess/non_maximum_suppression.cpp

@@ -61,9 +61,25 @@ uint8_t getSemanticType(const std::string & class_name)
 }
 
 
+
 namespace autoware::lidar_transfusion
 {
 
+
+static std::vector<bool> classNamesToBooleanMask(const std::vector<std::string> & class_names)
+{
+  std::vector<bool> mask;
+  constexpr std::size_t num_object_classification = 8;
+  mask.resize(num_object_classification);
+  for (const auto & class_name : class_names) {
+    const auto semantic_type = getSemanticType(class_name);
+    mask.at(semantic_type) = true;
+  }
+
+  return mask;
+}
+
+
 void NonMaximumSuppression::setParameters(const NMSParams & params)
 {
   assert(params.search_distance_2d_ >= 0.0);

+ 45 - 0
src/detection/detection_lidar_transfusion/transfusion_call.cpp

@@ -0,0 +1,45 @@
+#include "transfusion_call.h"
+
+transfusion_call::transfusion_call()
+{
+
+    std::string engine_path ="/home/nvidia/models/transfusion.eng";
+    std::string onnx_path = "/home/nvidia/models/transfusion.onnx";
+    std::string trt_precision ="fp16" ;
+
+    std::string densification_world_frame_id = "world";
+    int densification_num_past_frames = 0;
+
+    std::size_t cloud_capacity = 2000000;
+    std::vector<int64_t> voxels_num;
+    std::vector<double>  point_cloud_range;
+    std::vector<double>  voxel_size;
+    std::size_t num_proposals = 500;
+    float circle_nms_dist_threshold = 0.5;
+    std::vector<double>  yaw_norm_thresholds;
+    float score_threshold = 0.2;
+
+    voxels_num.push_back(5000);voxels_num.push_back(30000);voxels_num.push_back(60000);
+    point_cloud_range.push_back(-76.8);point_cloud_range.push_back(-76.8);point_cloud_range.push_back(-3.0);
+    point_cloud_range.push_back(76.8);point_cloud_range.push_back(76.8);point_cloud_range.push_back(5.0);
+    voxel_size.push_back(0.3);voxel_size.push_back(0.3);voxel_size.push_back(8.0);
+    yaw_norm_thresholds.push_back(0.3);yaw_norm_thresholds.push_back(0.3);yaw_norm_thresholds.push_back(0.3);
+    yaw_norm_thresholds.push_back(0.3);yaw_norm_thresholds.push_back(0.0);
+
+    NetworkParam network_param(onnx_path, engine_path, trt_precision);
+    DensificationParam densification_param(
+      densification_world_frame_id, densification_num_past_frames);
+    TransfusionConfig config(
+      cloud_capacity, voxels_num, point_cloud_range, voxel_size, num_proposals,
+      circle_nms_dist_threshold, yaw_norm_thresholds, score_threshold);
+
+    detector_ptr_ = std::make_unique<TransfusionTRT>(network_param, densification_param, config);
+}
+
+bool transfusion_call::detect(
+  const pcl::PointCloud<pcl::PointXYZI>::Ptr & pc_ptr,
+  std::vector<Box3D> & det_boxes3d)
+{
+    std::unordered_map<std::string, double> proc_timing;
+    return detector_ptr_->detect(pc_ptr,det_boxes3d,proc_timing);
+}

+ 33 - 0
src/detection/detection_lidar_transfusion/transfusion_call.h

@@ -0,0 +1,33 @@
+#ifndef TRANSFUSION_CALL_H
+#define TRANSFUSION_CALL_H
+
+
+#include "detection_class_remapper.hpp"
+#include "postprocess/non_maximum_suppression.hpp"
+#include "preprocess/pointcloud_densification.hpp"
+#include "transfusion_trt.hpp"
+#include "visibility_control.hpp"
+
+
+using namespace autoware::lidar_transfusion;
+
+class transfusion_call
+{
+public:
+    transfusion_call();
+
+    bool detect(
+      const pcl::PointCloud<pcl::PointXYZI>::Ptr & pc_ptr,
+      std::vector<Box3D> & det_boxes3d);
+
+private:
+    DetectionClassRemapper detection_class_remapper_;
+    float score_threshold_{0.0};
+    std::vector<std::string> class_names_;
+
+    NonMaximumSuppression iou_bev_nms_;
+
+    std::unique_ptr<TransfusionTRT> detector_ptr_{nullptr};
+};
+
+#endif // TRANSFUSION_CALL_H