|
|
@@ -0,0 +1,796 @@
|
|
|
+// Copyright (c) 2022 PaddlePaddle 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 <gflags/gflags.h>
|
|
|
+#include <glog/logging.h>
|
|
|
+
|
|
|
+#include <chrono>
|
|
|
+#include <cmath>
|
|
|
+#include <fstream>
|
|
|
+#include <iostream>
|
|
|
+#include <numeric>
|
|
|
+#include <string>
|
|
|
+#include <dlfcn.h>
|
|
|
+
|
|
|
+#include "paddle/include/paddle_inference_api.h"
|
|
|
+
|
|
|
+
|
|
|
+#include "modulecomm.h"
|
|
|
+#include "objectarray.pb.h"
|
|
|
+#include <thread>
|
|
|
+#include <pcl/point_cloud.h>
|
|
|
+#include <pcl/point_types.h>
|
|
|
+#include <pcl/io/io.h>
|
|
|
+#include <pcl/io/pcd_io.h>
|
|
|
+#include <chrono>
|
|
|
+#include <opencv2/opencv.hpp>
|
|
|
+
|
|
|
+////////////////////用于nms////////////////////
|
|
|
+#define M_PI 3.14159265358979323846
|
|
|
+#include<opencv2/opencv.hpp>
|
|
|
+#define rad2Angle(rad) ((rad) * 180.0 / M_PI)
|
|
|
+
|
|
|
+
|
|
|
+float score_threshold = 0.4;
|
|
|
+float smallinbig_threshold = 0.8;
|
|
|
+float distance_threshold = 0.2;
|
|
|
+float second_nms_threshold = 0.5;
|
|
|
+
|
|
|
+std::string config_file = "./yaml/centerpoint_paddle.yaml";
|
|
|
+
|
|
|
+using paddle_infer::Config;
|
|
|
+using paddle_infer::CreatePredictor;
|
|
|
+using paddle_infer::Predictor;
|
|
|
+
|
|
|
+void * gpa;
|
|
|
+std::string lidarname = "lidar_pc";
|
|
|
+void * gpdetect;
|
|
|
+//std::string detectname = "lidar_track";
|
|
|
+std::string detectname = "lidar_pointpillar";
|
|
|
+
|
|
|
+int gnothavedatatime = 0;
|
|
|
+std::thread * gpthread;
|
|
|
+
|
|
|
+std::shared_ptr<paddle_infer::Predictor> predictor;
|
|
|
+
|
|
|
+static const char *labels[] = {"smallMot","bigMot","nonMot","pedestrian","trafficCone"};
|
|
|
+
|
|
|
+
|
|
|
+DEFINE_string(model_file, "./model/centerpoint.pdmodel", "Path of a inference model");
|
|
|
+DEFINE_string(params_file, "./model/centerpoint.pdiparams", "Path of a inference params");
|
|
|
+DEFINE_string(pd_infer_custom_op,"./lib/libpd_infer_custom_op.so","Path of the libpd_infer_custom_op.so");
|
|
|
+DEFINE_string(lidar_file, "", "Path of a lidar file to be predicted");
|
|
|
+DEFINE_int32(num_point_dim, 4, "Dimension of a point in the lidar file");
|
|
|
+DEFINE_int32(with_timelag, 0,
|
|
|
+ "Whether timelag is the 5-th dimension of each point feature, "
|
|
|
+ "like: x, y, z, intensive, timelag");
|
|
|
+DEFINE_int32(gpu_id, 0, "GPU card id");
|
|
|
+DEFINE_int32(use_trt, 0,
|
|
|
+ "Whether to use tensorrt to accelerate when using gpu");
|
|
|
+DEFINE_int32(trt_precision, 0,
|
|
|
+ "Precision type of tensorrt, 0: kFloat32, 1: kHalf");
|
|
|
+DEFINE_int32(
|
|
|
+ trt_use_static, 0,
|
|
|
+ "Whether to load the tensorrt graph optimization from a disk path");
|
|
|
+DEFINE_string(trt_static_dir, "",
|
|
|
+ "Path of a tensorrt graph optimization directory");
|
|
|
+DEFINE_int32(collect_shape_info, 0,
|
|
|
+ "Whether to collect dynamic shape before using tensorrt");
|
|
|
+DEFINE_string(dynamic_shape_file, "",
|
|
|
+ "Path of a dynamic shape file for tensorrt");
|
|
|
+
|
|
|
+
|
|
|
+/////////////nms算法去除重叠交叉的框/////////////
|
|
|
+struct Box{
|
|
|
+ float x;
|
|
|
+ float y;
|
|
|
+ float z;
|
|
|
+ float l;
|
|
|
+ float h;
|
|
|
+ float w;
|
|
|
+ float theta;
|
|
|
+
|
|
|
+ float score;
|
|
|
+ int cls;
|
|
|
+ bool isDrop; // for nms
|
|
|
+};
|
|
|
+typedef struct {
|
|
|
+ cv::RotatedRect box;
|
|
|
+ Box detection;
|
|
|
+ int label;
|
|
|
+ float score;
|
|
|
+}BBOX3D;
|
|
|
+
|
|
|
+
|
|
|
+//将检测结果转为RotatedRect以便于nms计算
|
|
|
+bool GetRotatedRect(const std::vector<float> &box3d_lidar,
|
|
|
+ const std::vector<int64_t> &label_preds,
|
|
|
+ const std::vector<float> &scores,
|
|
|
+ std::vector<BBOX3D> &results)
|
|
|
+{
|
|
|
+ int num_bbox3d = scores.size();
|
|
|
+ if(num_bbox3d>0)
|
|
|
+ {
|
|
|
+ for(int box_idx=0;box_idx<num_bbox3d;box_idx++)
|
|
|
+ {
|
|
|
+
|
|
|
+ if(scores[box_idx] < score_threshold)
|
|
|
+ {
|
|
|
+ //std::cout<<" the score < threshold"<<std::endl;
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+
|
|
|
+ Box parse_bbox;
|
|
|
+ parse_bbox.x = box3d_lidar[box_idx * 7 + 0];
|
|
|
+ parse_bbox.y = box3d_lidar[box_idx * 7 + 1];
|
|
|
+ parse_bbox.z = box3d_lidar[box_idx * 7 + 2];
|
|
|
+ parse_bbox.l = box3d_lidar[box_idx * 7 + 3];
|
|
|
+ parse_bbox.w = box3d_lidar[box_idx * 7 + 4];
|
|
|
+ parse_bbox.h = box3d_lidar[box_idx * 7 + 5];
|
|
|
+ parse_bbox.theta = box3d_lidar[box_idx * 7 + 6];
|
|
|
+ parse_bbox.score = scores[box_idx];
|
|
|
+ parse_bbox.cls = label_preds[box_idx];
|
|
|
+
|
|
|
+ BBOX3D result;
|
|
|
+ result.box = cv::RotatedRect(cv::Point2f(parse_bbox.x,parse_bbox.y),
|
|
|
+ cv::Size2f(parse_bbox.l,parse_bbox.w),
|
|
|
+ rad2Angle(parse_bbox.theta));
|
|
|
+
|
|
|
+ result.detection = parse_bbox;
|
|
|
+ result.label = parse_bbox.cls;
|
|
|
+ result.score = parse_bbox.score;
|
|
|
+ results.push_back(result);
|
|
|
+ }
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ else{
|
|
|
+ std::cout<<"The out_detections size == 0 "<<std::endl;
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+bool sort_score(BBOX3D box1,BBOX3D box2)
|
|
|
+{
|
|
|
+ return (box1.score > box2.score);
|
|
|
+}
|
|
|
+
|
|
|
+//计算两个旋转矩形的IOU
|
|
|
+float calcIOU(cv::RotatedRect rect1, cv::RotatedRect rect2)
|
|
|
+{
|
|
|
+ float areaRect1 = rect1.size.width * rect1.size.height;
|
|
|
+ float areaRect2 = rect2.size.width * rect2.size.height;
|
|
|
+ std::vector<cv::Point2f> vertices;
|
|
|
+ int intersectionType = cv::rotatedRectangleIntersection(rect1, rect2, vertices);
|
|
|
+ if (vertices.size()==0)
|
|
|
+ return 0.0;
|
|
|
+ else{
|
|
|
+ std::vector<cv::Point2f> order_pts;
|
|
|
+ cv::convexHull(cv::Mat(vertices), order_pts, true);
|
|
|
+ double area = cv::contourArea(order_pts);
|
|
|
+ float inner = (float) (area / (areaRect1 + areaRect2 - area + 0.0001));
|
|
|
+ //排除小框完全在大框里面的case
|
|
|
+ float areaMin = (areaRect1 < areaRect2)?areaRect1:areaRect2;
|
|
|
+ float innerMin = (float)(area / (areaMin + 0.0001));
|
|
|
+ if(innerMin > smallinbig_threshold)
|
|
|
+ inner = innerMin;
|
|
|
+ return inner;
|
|
|
+ }
|
|
|
+}
|
|
|
+//计算两个点的欧式距离
|
|
|
+float calcdistance(cv::Point2f center1, cv::Point2f center2)
|
|
|
+{
|
|
|
+ float distance = sqrt((center1.x-center2.x)*(center1.x-center2.x)+
|
|
|
+ (center1.y-center2.y)*(center1.y-center2.y));
|
|
|
+ return distance;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+//nms
|
|
|
+void nms(std::vector<BBOX3D> &vec_boxs,float threshold,std::vector<BBOX3D> &results)
|
|
|
+{
|
|
|
+ std::sort(vec_boxs.begin(),vec_boxs.end(),sort_score);
|
|
|
+ while(vec_boxs.size() > 0)
|
|
|
+ {
|
|
|
+ results.push_back(vec_boxs[0]);
|
|
|
+ vec_boxs.erase(vec_boxs.begin());
|
|
|
+ for (auto it = vec_boxs.begin(); it != vec_boxs.end();)
|
|
|
+ {
|
|
|
+ float iou_value =calcIOU(results.back().box,(*it).box);
|
|
|
+ float distance_value = calcdistance(results.back().box.center,(*it).box.center);
|
|
|
+ if ((iou_value > threshold) || (distance_value<distance_threshold))
|
|
|
+ it = vec_boxs.erase(it);
|
|
|
+ else it++;
|
|
|
+ }
|
|
|
+
|
|
|
+// std::cout<<"results: "<<results.back().detection.at(0)<<" "<<results.back().detection.at(1)<<
|
|
|
+// " "<<results.back().detection.at(2)<<std::endl;
|
|
|
+
|
|
|
+ }
|
|
|
+}
|
|
|
+/////////////nms算法去除重叠交叉的框/////////////
|
|
|
+
|
|
|
+//change lidarpoints to waymo type,x-->forward,y-->left
|
|
|
+void PclXYZITToArray(
|
|
|
+ const pcl::PointCloud<pcl::PointXYZI>::Ptr& in_pcl_pc_ptr,
|
|
|
+ float* out_points_array, const float normalizing_factor) {
|
|
|
+ for (size_t i = 0; i < in_pcl_pc_ptr->size(); ++i) {
|
|
|
+ pcl::PointXYZI point = in_pcl_pc_ptr->at(i);
|
|
|
+ out_points_array[i * 4 + 0] = point.x;
|
|
|
+ out_points_array[i * 4 + 1] = point.y;
|
|
|
+ out_points_array[i * 4 + 2] = point.z;
|
|
|
+ out_points_array[i * 4 + 3] = static_cast<float>(point.intensity / normalizing_factor);
|
|
|
+ //out_points_array[i * 5 + 4] = 0;
|
|
|
+
|
|
|
+ //std::cout<<"the intensity = "<< out_points_array[i * 5 + 3]<< std::endl;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+bool preprocess_new(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr, const int num_point_dim,
|
|
|
+ std::vector<int> *points_shape,
|
|
|
+ std::vector<float> *points_data)
|
|
|
+{
|
|
|
+ int num_points;
|
|
|
+ std::shared_ptr<float> points_array_ptr = std::shared_ptr<float>(new float[pc_ptr->size() * num_point_dim]);
|
|
|
+ PclXYZITToArray(pc_ptr, points_array_ptr.get(), 255.0);
|
|
|
+ num_points = pc_ptr->width;
|
|
|
+
|
|
|
+ if(num_points < 0)
|
|
|
+ return false;
|
|
|
+
|
|
|
+ float *points = points_array_ptr.get();
|
|
|
+ points_data->assign(points, points + num_points * num_point_dim);
|
|
|
+ points_shape->push_back(num_points);
|
|
|
+ points_shape->push_back(num_point_dim);
|
|
|
+
|
|
|
+ //free(points);
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+bool read_point(const std::string &file_path, const int num_point_dim,
|
|
|
+ void **buffer, int *num_points) {
|
|
|
+ std::ifstream file_in(file_path, std::ios::in | std::ios::binary);
|
|
|
+ if (num_point_dim < 4) {
|
|
|
+ LOG(ERROR) << "Point dimension must not be less than 4, but received "
|
|
|
+ << "num_point_dim is " << num_point_dim << ".\n";
|
|
|
+ }
|
|
|
+
|
|
|
+ if (!file_in) {
|
|
|
+ LOG(ERROR) << "Failed to read file: " << file_path << "\n";
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+
|
|
|
+ std::streampos file_size;
|
|
|
+ file_in.seekg(0, std::ios::end);
|
|
|
+ file_size = file_in.tellg();
|
|
|
+ file_in.seekg(0, std::ios::beg);
|
|
|
+
|
|
|
+ *buffer = malloc(file_size);
|
|
|
+ if (*buffer == nullptr) {
|
|
|
+ LOG(ERROR) << "Failed to malloc memory of size: " << file_size << "\n";
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ file_in.read(reinterpret_cast<char *>(*buffer), file_size);
|
|
|
+ file_in.close();
|
|
|
+
|
|
|
+ if (file_size / sizeof(float) % num_point_dim != 0) {
|
|
|
+ LOG(ERROR) << "Loaded file size (" << file_size
|
|
|
+ << ") is not evenly divisible by num_point_dim ("
|
|
|
+ << num_point_dim << ")\n";
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ *num_points = file_size / sizeof(float) / num_point_dim;
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+bool insert_time_to_points(const int num_points, const int num_point_dim,
|
|
|
+ float *points) {
|
|
|
+ for (int i = 0; i < num_points; ++i) {
|
|
|
+ *(points + i * num_point_dim + 4) = 0.;
|
|
|
+ }
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+bool preprocess(const std::string &file_path, const int num_point_dim,
|
|
|
+ const int with_timelag, std::vector<int> *points_shape,
|
|
|
+ std::vector<float> *points_data) {
|
|
|
+ void *buffer = nullptr;
|
|
|
+ int num_points;
|
|
|
+ if (!read_point(file_path, num_point_dim, &buffer, &num_points)) {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ float *points = static_cast<float *>(buffer);
|
|
|
+
|
|
|
+ if (!with_timelag && num_point_dim == 5 || num_point_dim > 5) {
|
|
|
+ // the origin points dim is [x, y, z, intensity, ring_index],
|
|
|
+ // but we need [x, y, z, intensity] and the sweep time index should be
|
|
|
+ // inserted into points
|
|
|
+ // so these two steps will be done in function insert_time_to_points
|
|
|
+ insert_time_to_points(num_points, num_point_dim, points);
|
|
|
+ }
|
|
|
+
|
|
|
+ points_data->assign(points, points + num_points * num_point_dim);
|
|
|
+ points_shape->push_back(num_points);
|
|
|
+ points_shape->push_back(num_point_dim);
|
|
|
+
|
|
|
+ free(points);
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+std::shared_ptr<paddle_infer::Predictor> create_predictor(
|
|
|
+ const std::string &model_path, const std::string ¶ms_path,
|
|
|
+ const int gpu_id, const int use_trt, const int trt_precision,
|
|
|
+ const int trt_use_static, const std::string trt_static_dir,
|
|
|
+ const int collect_shape_info, const std::string dynamic_shape_file) {
|
|
|
+ paddle::AnalysisConfig config;
|
|
|
+ config.EnableUseGpu(1000, gpu_id);
|
|
|
+ config.SetModel(model_path, params_path);
|
|
|
+ if (use_trt) {
|
|
|
+ paddle::AnalysisConfig::Precision precision;
|
|
|
+ if (trt_precision == 0) {
|
|
|
+ precision = paddle_infer::PrecisionType::kFloat32;
|
|
|
+ } else if (trt_precision == 1) {
|
|
|
+ precision = paddle_infer::PrecisionType::kHalf;
|
|
|
+ } else {
|
|
|
+ LOG(ERROR) << "Tensorrt type can only support 0 or 1, but received is"
|
|
|
+ << trt_precision << "\n";
|
|
|
+ return nullptr;
|
|
|
+ }
|
|
|
+ config.EnableTensorRtEngine(1 << 30, 1, 3, precision, trt_use_static,
|
|
|
+ false);
|
|
|
+
|
|
|
+ if (dynamic_shape_file == "") {
|
|
|
+ LOG(ERROR) << "dynamic_shape_file should be set, but received is "
|
|
|
+ << dynamic_shape_file << "\n";
|
|
|
+ return nullptr;
|
|
|
+ }
|
|
|
+ if (collect_shape_info) {
|
|
|
+ config.CollectShapeRangeInfo(dynamic_shape_file);
|
|
|
+ } else {
|
|
|
+ config.EnableTunedTensorRtDynamicShape(dynamic_shape_file, true);
|
|
|
+ }
|
|
|
+
|
|
|
+ if (trt_use_static) {
|
|
|
+ if (trt_static_dir == "") {
|
|
|
+ LOG(ERROR) << "trt_static_dir should be set, but received is "
|
|
|
+ << trt_static_dir << "\n";
|
|
|
+ return nullptr;
|
|
|
+ }
|
|
|
+ config.SetOptimCacheDir(trt_static_dir);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ config.SwitchIrOptim(true);
|
|
|
+ return paddle_infer::CreatePredictor(config);
|
|
|
+}
|
|
|
+
|
|
|
+void run(Predictor *predictor, const std::vector<int> &points_shape,
|
|
|
+ const std::vector<float> &points_data, std::vector<float> *box3d_lidar,
|
|
|
+ std::vector<int64_t> *label_preds, std::vector<float> *scores) {
|
|
|
+ auto input_names = predictor->GetInputNames();
|
|
|
+ for (const auto &tensor_name : input_names) {
|
|
|
+ auto in_tensor = predictor->GetInputHandle(tensor_name);
|
|
|
+ if (tensor_name == "data") {
|
|
|
+ in_tensor->Reshape(points_shape);
|
|
|
+ in_tensor->CopyFromCpu(points_data.data());
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ CHECK(predictor->Run());
|
|
|
+
|
|
|
+ auto output_names = predictor->GetOutputNames();
|
|
|
+ for (size_t i = 0; i != output_names.size(); i++) {
|
|
|
+ auto output = predictor->GetOutputHandle(output_names[i]);
|
|
|
+ std::vector<int> output_shape = output->shape();
|
|
|
+ int out_num = std::accumulate(output_shape.begin(), output_shape.end(), 1,
|
|
|
+ std::multiplies<int>());
|
|
|
+ if (i == 0) {
|
|
|
+ box3d_lidar->resize(out_num);
|
|
|
+ output->CopyToCpu(box3d_lidar->data());
|
|
|
+ } else if (i == 1) {
|
|
|
+ label_preds->resize(out_num);
|
|
|
+ output->CopyToCpu(label_preds->data());
|
|
|
+ } else if (i == 2) {
|
|
|
+ scores->resize(out_num);
|
|
|
+ output->CopyToCpu(scores->data());
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+bool parse_result(const std::vector<float> &box3d_lidar,
|
|
|
+ const std::vector<int64_t> &label_preds,
|
|
|
+ const std::vector<float> &scores) {
|
|
|
+ int num_bbox3d = scores.size();
|
|
|
+ int bbox3d_dims = box3d_lidar.size() / num_bbox3d;
|
|
|
+ for (size_t box_idx = 0; box_idx != num_bbox3d; ++box_idx) {
|
|
|
+ // filter fake results: score = -1
|
|
|
+ if (scores[box_idx] < 0) {
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+ LOG(INFO) << "Score: " << scores[box_idx]
|
|
|
+ << " Label: " << label_preds[box_idx] << " ";
|
|
|
+ if (bbox3d_dims == 9) {
|
|
|
+ LOG(INFO) << "Box (x_c, y_c, z_c, w, l, h, vec_x, vec_y, -rot): "
|
|
|
+ << box3d_lidar[box_idx * 9 + 0] << " "
|
|
|
+ << box3d_lidar[box_idx * 9 + 1] << " "
|
|
|
+ << box3d_lidar[box_idx * 9 + 2] << " "
|
|
|
+ << box3d_lidar[box_idx * 9 + 3] << " "
|
|
|
+ << box3d_lidar[box_idx * 9 + 4] << " "
|
|
|
+ << box3d_lidar[box_idx * 9 + 5] << " "
|
|
|
+ << box3d_lidar[box_idx * 9 + 6] << " "
|
|
|
+ << box3d_lidar[box_idx * 9 + 7] << " "
|
|
|
+ << box3d_lidar[box_idx * 9 + 8] << "\n";
|
|
|
+ } else if (bbox3d_dims == 7) {
|
|
|
+ LOG(INFO) << "Box (x_c, y_c, z_c, w, l, h, -rot): "
|
|
|
+ << box3d_lidar[box_idx * 7 + 0] << " "
|
|
|
+ << box3d_lidar[box_idx * 7 + 1] << " "
|
|
|
+ << box3d_lidar[box_idx * 7 + 2] << " "
|
|
|
+ << box3d_lidar[box_idx * 7 + 3] << " "
|
|
|
+ << box3d_lidar[box_idx * 7 + 4] << " "
|
|
|
+ << box3d_lidar[box_idx * 7 + 5] << " "
|
|
|
+ << box3d_lidar[box_idx * 7 + 6] << "\n";
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+void GetLidarObj(const std::vector<float> &box3d_lidar,
|
|
|
+ const std::vector<int64_t> &label_preds,
|
|
|
+ const std::vector<float> &scores,
|
|
|
+ iv::lidar::objectarray & lidarobjvec)
|
|
|
+{
|
|
|
+ int num_bbox3d = scores.size();
|
|
|
+ int bbox3d_dims = box3d_lidar.size() / num_bbox3d;
|
|
|
+ for (size_t box_idx = 0; box_idx != num_bbox3d; ++box_idx)
|
|
|
+ {
|
|
|
+ // filter fake results: score = -1
|
|
|
+ if (scores[box_idx] < 0) {
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+
|
|
|
+ iv::lidar::lidarobject lidarobj;
|
|
|
+ //std::cout<<" The scores = "<<result.score<<std::endl;
|
|
|
+
|
|
|
+ lidarobj.set_tyaw( -box3d_lidar[box_idx * 7 + 6]);
|
|
|
+
|
|
|
+ //std::cout<<" The theta = "<<result.theta<<std::endl;
|
|
|
+ //std::cout<<" The xyz = "<<result.cls<<" "<<result.w<<" "<<result.l<<" "<<result.h<<std::endl;
|
|
|
+
|
|
|
+ std::cout<<"obstacle id is: "<<box_idx<<std::endl;
|
|
|
+ std::cout<<"obstacle score is: "<<scores[box_idx]<<std::endl;
|
|
|
+ std::cout<<"(x,y,z,dx,dy,dz,yaw,class)=("<<box3d_lidar[box_idx * 7 + 0]<<","
|
|
|
+ <<box3d_lidar[box_idx * 7 + 1]<<","
|
|
|
+ <<box3d_lidar[box_idx * 7 + 2]<<","
|
|
|
+ <<box3d_lidar[box_idx * 7 + 3]<<","
|
|
|
+ <<box3d_lidar[box_idx * 7 + 4]<<","
|
|
|
+ <<box3d_lidar[box_idx * 7 + 5]<<","
|
|
|
+ <<box3d_lidar[box_idx * 7 + 6]<<","
|
|
|
+ <<label_preds[box_idx]<<")"<<std::endl;
|
|
|
+
|
|
|
+ //givlog->verbose("obstacle id is: %d",idx);
|
|
|
+ //givlog->verbose("(x,y,z,dx,dy,dz,yaw,class)=(%f,%f,%f,%f,%f,%f,%f,%s)",-result.y,result.x,result.z,
|
|
|
+ //result.l,result.w,result.h,-result.theta,labels[result.cls]);
|
|
|
+
|
|
|
+
|
|
|
+ iv::lidar::PointXYZ centroid;
|
|
|
+ iv::lidar::PointXYZ * _centroid;
|
|
|
+ centroid.set_x(box3d_lidar[box_idx * 7 + 0]);
|
|
|
+ centroid.set_y(box3d_lidar[box_idx * 7 + 1]);
|
|
|
+ centroid.set_z(box3d_lidar[box_idx * 7 + 2]);
|
|
|
+ _centroid = lidarobj.mutable_centroid();
|
|
|
+ _centroid->CopyFrom(centroid);
|
|
|
+
|
|
|
+ iv::lidar::PointXYZ min_point;
|
|
|
+ iv::lidar::PointXYZ * _min_point;
|
|
|
+ min_point.set_x(0);
|
|
|
+ min_point.set_y(0);
|
|
|
+ min_point.set_z(0);
|
|
|
+ _min_point = lidarobj.mutable_min_point();
|
|
|
+ _min_point->CopyFrom(min_point);
|
|
|
+
|
|
|
+ iv::lidar::PointXYZ max_point;
|
|
|
+ iv::lidar::PointXYZ * _max_point;
|
|
|
+ max_point.set_x(0);
|
|
|
+ max_point.set_y(0);
|
|
|
+ max_point.set_z(0);
|
|
|
+ _max_point = lidarobj.mutable_max_point();
|
|
|
+ _max_point->CopyFrom(max_point);
|
|
|
+
|
|
|
+ iv::lidar::PointXYZ position;
|
|
|
+ iv::lidar::PointXYZ * _position;
|
|
|
+ position.set_x(box3d_lidar[box_idx * 7 + 0]);
|
|
|
+ position.set_y(box3d_lidar[box_idx * 7 + 1]);
|
|
|
+ position.set_z(box3d_lidar[box_idx * 7 + 2]);
|
|
|
+ _position = lidarobj.mutable_position();
|
|
|
+ _position->CopyFrom(position);
|
|
|
+
|
|
|
+ lidarobj.set_mntype(label_preds[box_idx]);
|
|
|
+
|
|
|
+ lidarobj.set_score(scores[box_idx]);
|
|
|
+ lidarobj.add_type_probs(scores[box_idx]);
|
|
|
+
|
|
|
+ iv::lidar::PointXYZI point_cloud;
|
|
|
+ iv::lidar::PointXYZI * _point_cloud;
|
|
|
+
|
|
|
+ point_cloud.set_x(box3d_lidar[box_idx * 7 + 0]);
|
|
|
+ point_cloud.set_y(box3d_lidar[box_idx * 7 + 1]);
|
|
|
+ point_cloud.set_z(box3d_lidar[box_idx * 7 + 2]);
|
|
|
+
|
|
|
+ point_cloud.set_i(0);
|
|
|
+
|
|
|
+ _point_cloud = lidarobj.add_cloud();
|
|
|
+ _point_cloud->CopyFrom(point_cloud);
|
|
|
+
|
|
|
+ iv::lidar::Dimension ld;
|
|
|
+ iv::lidar::Dimension * pld;
|
|
|
+
|
|
|
+
|
|
|
+ ld.set_x(box3d_lidar[box_idx * 7 + 3]);// w
|
|
|
+ ld.set_y(box3d_lidar[box_idx * 7 + 4]);// l
|
|
|
+ ld.set_z(box3d_lidar[box_idx * 7 + 5]);// h
|
|
|
+
|
|
|
+
|
|
|
+ pld = lidarobj.mutable_dimensions();
|
|
|
+ pld->CopyFrom(ld);
|
|
|
+ iv::lidar::lidarobject * po = lidarobjvec.add_obj();
|
|
|
+ po->CopyFrom(lidarobj);
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+void GetLidarObj_nms(std::vector<BBOX3D> &predResult,iv::lidar::objectarray & lidarobjvec)
|
|
|
+{
|
|
|
+ // givlog->verbose("OBJ","object size is %d",obj_size);
|
|
|
+ for(size_t idx = 0; idx < predResult.size(); idx++)
|
|
|
+ {
|
|
|
+ iv::lidar::lidarobject lidarobj;
|
|
|
+ Box result = predResult[idx].detection;
|
|
|
+ lidarobj.set_tyaw(-result.theta);
|
|
|
+
|
|
|
+ std::cout<<"obstacle id is: "<<idx<<std::endl;
|
|
|
+ std::cout<<"obstacle score is: "<<result.score<<std::endl;
|
|
|
+ std::cout<<"obstacle class is: "<<labels[result.cls]<<std::endl;
|
|
|
+ std::cout<<"(x,y,z,dx,dy,dz,yaw)=("<<result.x<<","<<result.y<<","<<result.z<<","
|
|
|
+ <<result.l<<","<<result.w<<","<<result.h<<","<<-result.theta<<")"<<std::endl;
|
|
|
+
|
|
|
+// givlog->verbose("obstacle id is: %d",idx);
|
|
|
+// givlog->verbose("(x,y,z,dx,dy,dz,yaw,classs)=(%f,%f,%f,%f,%f,%f,%f,%s)",result.x,result.y,result.z,
|
|
|
+// result.l,result.w,result.h,-result.theta,labels[result.cls]);
|
|
|
+
|
|
|
+
|
|
|
+ iv::lidar::PointXYZ centroid;
|
|
|
+ iv::lidar::PointXYZ * _centroid;
|
|
|
+ centroid.set_x(result.x);
|
|
|
+ centroid.set_y(result.y);
|
|
|
+ centroid.set_z(result.z);
|
|
|
+ _centroid = lidarobj.mutable_centroid();
|
|
|
+ _centroid->CopyFrom(centroid);
|
|
|
+
|
|
|
+ iv::lidar::PointXYZ min_point;
|
|
|
+ iv::lidar::PointXYZ * _min_point;
|
|
|
+ min_point.set_x(0);
|
|
|
+ min_point.set_y(0);
|
|
|
+ min_point.set_z(0);
|
|
|
+ _min_point = lidarobj.mutable_min_point();
|
|
|
+ _min_point->CopyFrom(min_point);
|
|
|
+
|
|
|
+ iv::lidar::PointXYZ max_point;
|
|
|
+ iv::lidar::PointXYZ * _max_point;
|
|
|
+ max_point.set_x(0);
|
|
|
+ max_point.set_y(0);
|
|
|
+ max_point.set_z(0);
|
|
|
+ _max_point = lidarobj.mutable_max_point();
|
|
|
+ _max_point->CopyFrom(max_point);
|
|
|
+
|
|
|
+ iv::lidar::PointXYZ position;
|
|
|
+ iv::lidar::PointXYZ * _position;
|
|
|
+ position.set_x(result.x);
|
|
|
+ position.set_y(result.y);
|
|
|
+ position.set_z(result.z);
|
|
|
+ _position = lidarobj.mutable_position();
|
|
|
+ _position->CopyFrom(position);
|
|
|
+
|
|
|
+ lidarobj.set_mntype(result.cls);
|
|
|
+
|
|
|
+ lidarobj.set_score(result.score);
|
|
|
+ lidarobj.add_type_probs(result.score);
|
|
|
+
|
|
|
+ iv::lidar::PointXYZI point_cloud;
|
|
|
+ iv::lidar::PointXYZI * _point_cloud;
|
|
|
+
|
|
|
+ point_cloud.set_x(result.x);
|
|
|
+ point_cloud.set_y(result.y);
|
|
|
+ point_cloud.set_z(result.z);
|
|
|
+
|
|
|
+ point_cloud.set_i(0);
|
|
|
+
|
|
|
+ _point_cloud = lidarobj.add_cloud();
|
|
|
+ _point_cloud->CopyFrom(point_cloud);
|
|
|
+
|
|
|
+ iv::lidar::Dimension ld;
|
|
|
+ iv::lidar::Dimension * pld;
|
|
|
+
|
|
|
+
|
|
|
+ ld.set_x(result.l);// w
|
|
|
+ ld.set_y(result.w);// l
|
|
|
+ ld.set_z(result.h);// h
|
|
|
+
|
|
|
+ pld = lidarobj.mutable_dimensions();
|
|
|
+ pld->CopyFrom(ld);
|
|
|
+ iv::lidar::lidarobject * po = lidarobjvec.add_obj();
|
|
|
+ po->CopyFrom(lidarobj);
|
|
|
+ }
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+void ListenPointCloud(const char *strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
|
|
|
+{
|
|
|
+ // std::cout<<" is ok ------------ "<<std::endl;
|
|
|
+
|
|
|
+ std::cout<<"ListenPointCloud is ok ------------ "<<std::endl;
|
|
|
+
|
|
|
+ if(nSize <=16)return;
|
|
|
+ unsigned int * pHeadSize = (unsigned int *)strdata;
|
|
|
+ if(*pHeadSize > nSize)
|
|
|
+ {
|
|
|
+ //givlog->verbose("ListenPointCloud data is small headsize = %d, data size is %d", *pHeadSize, nSize);
|
|
|
+ std::cout<<"ListenPointCloud data is small headsize ="<<*pHeadSize<<" data size is"<<nSize<<std::endl;
|
|
|
+ }
|
|
|
+
|
|
|
+ gnothavedatatime = 0;
|
|
|
+ QTime xTime;
|
|
|
+ xTime.start();
|
|
|
+
|
|
|
+ pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
|
|
|
+ new pcl::PointCloud<pcl::PointXYZI>());
|
|
|
+ int nNameSize;
|
|
|
+ nNameSize = *pHeadSize - 4-4-8;
|
|
|
+ char * strName = new char[nNameSize+1];strName[nNameSize] = 0;
|
|
|
+ std::shared_ptr<char> str_ptr;
|
|
|
+ str_ptr.reset(strName);
|
|
|
+ memcpy(strName,(char *)((char *)strdata +4),nNameSize);
|
|
|
+ point_cloud->header.frame_id = strName;
|
|
|
+ memcpy(&point_cloud->header.seq,(char *)strdata+4+nNameSize,4);
|
|
|
+ memcpy(&point_cloud->header.stamp,(char *)strdata+4+nNameSize+4,8);
|
|
|
+ int nPCount = (nSize - *pHeadSize)/sizeof(pcl::PointXYZI);
|
|
|
+ int i;
|
|
|
+ pcl::PointXYZI * p;
|
|
|
+ p = (pcl::PointXYZI *)((char *)strdata + *pHeadSize);
|
|
|
+ for(i=0;i<nPCount;i++)
|
|
|
+ {
|
|
|
+ pcl::PointXYZI xp;
|
|
|
+ memcpy(&xp,p,sizeof(pcl::PointXYZI));
|
|
|
+ xp.z = xp.z;
|
|
|
+ point_cloud->push_back(xp);
|
|
|
+ p++;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ std::chrono::time_point<std::chrono::system_clock> start,end;
|
|
|
+
|
|
|
+ start = std::chrono::system_clock::now(); //时间函数
|
|
|
+ std::vector<int> points_shape;
|
|
|
+ std::vector<float> points_data;
|
|
|
+
|
|
|
+ if (!preprocess_new(point_cloud, 4,&points_shape, &points_data)) {
|
|
|
+ LOG(ERROR) << "Failed to preprocess!\n";
|
|
|
+ }
|
|
|
+
|
|
|
+ std::vector<float> box3d_lidar;
|
|
|
+ std::vector<int64_t> label_preds;
|
|
|
+ std::vector<float> scores;
|
|
|
+
|
|
|
+ run(predictor.get(), points_shape, points_data, &box3d_lidar, &label_preds,
|
|
|
+ &scores);
|
|
|
+
|
|
|
+ //parse_result(box3d_lidar, label_preds, scores);
|
|
|
+
|
|
|
+ end = std::chrono::system_clock::now(); //时间函数
|
|
|
+ std::cout <<"centerpoint infer time: "<<
|
|
|
+ std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count()<< "ms" << std::endl;
|
|
|
+
|
|
|
+ /////////////////增加nms算法进一步去除重叠交叉的框//////////////////
|
|
|
+ std::vector<BBOX3D>results_rect;
|
|
|
+ GetRotatedRect(box3d_lidar, label_preds, scores,results_rect);
|
|
|
+ //std::cout<<"results_rect size: "<<results_rect.size()<<std::endl;
|
|
|
+
|
|
|
+ std::vector<BBOX3D>results_bbox;
|
|
|
+ nms(results_rect,second_nms_threshold,results_bbox);
|
|
|
+ //std::cout<<"results_bbox size: "<<results_bbox.size()<<std::endl;
|
|
|
+ //std::cout<<"obj size is "<<predResult.size()<<std::endl;
|
|
|
+ /////////////////增加nms算法进一步去除重叠交叉的框//////////////////
|
|
|
+
|
|
|
+ iv::lidar::objectarray lidarobjvec;
|
|
|
+ //GetLidarObj(box3d_lidar, label_preds, scores,lidarobjvec);
|
|
|
+ GetLidarObj_nms(results_bbox,lidarobjvec);
|
|
|
+
|
|
|
+
|
|
|
+ double timex = point_cloud->header.stamp;
|
|
|
+ timex = timex/1000.0;
|
|
|
+ lidarobjvec.set_timestamp(point_cloud->header.stamp);
|
|
|
+
|
|
|
+ int ntlen;
|
|
|
+ std::string out = lidarobjvec.SerializeAsString();
|
|
|
+ // char * strout = lidarobjtostr(lidarobjvec,ntlen);
|
|
|
+ iv::modulecomm::ModuleSendMsg(gpdetect,out.data(),out.length());
|
|
|
+
|
|
|
+ end = std::chrono::system_clock::now(); //时间函数
|
|
|
+ std::cout <<"total process time: "<<
|
|
|
+ std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count()<< "ms" << std::endl;
|
|
|
+
|
|
|
+ std::cout<<"ListenPointCloud is end ------------ "<<std::endl;
|
|
|
+}
|
|
|
+
|
|
|
+int main(int argc, char *argv[]) {
|
|
|
+
|
|
|
+
|
|
|
+// google::ParseCommandLineFlags(&argc, &argv, true);
|
|
|
+// if (FLAGS_model_file == "" || FLAGS_params_file == "" ||
|
|
|
+// FLAGS_lidar_file == "") {
|
|
|
+// LOG(INFO) << "Missing required parameter"
|
|
|
+// << "\n";
|
|
|
+// LOG(INFO) << "Usage: " << std::string(argv[0])
|
|
|
+// << " --model_file ${MODEL_FILE} "
|
|
|
+// << "--params_file ${PARAMS_FILE} "
|
|
|
+// << "--lidar_file ${LIDAR_FILE}"
|
|
|
+// << "\n";
|
|
|
+// return -1;
|
|
|
+// }
|
|
|
+
|
|
|
+ cv::FileStorage config(config_file, cv::FileStorage::READ);
|
|
|
+ bool config_isOpened = config.isOpened();
|
|
|
+ //const char* onnx_path_;
|
|
|
+ if(config_isOpened)
|
|
|
+ {
|
|
|
+ FLAGS_model_file = std::string(config["model_file"]);
|
|
|
+ FLAGS_params_file = std::string(config["params_file"]);
|
|
|
+ FLAGS_pd_infer_custom_op = std::string(config["pd_infer_custom_op"]);
|
|
|
+ score_threshold = float(config["score_threshold"]);
|
|
|
+ smallinbig_threshold = float(config["smallinbig_threshold"]);
|
|
|
+ distance_threshold = float(config["distance_threshold"]);
|
|
|
+ second_nms_threshold = float(config["second_nms_threshold"]);
|
|
|
+ }
|
|
|
+
|
|
|
+ ///////////////////////////////add code
|
|
|
+ void *handle = dlopen(FLAGS_pd_infer_custom_op.c_str(),RTLD_NOW);
|
|
|
+ if(!handle)
|
|
|
+ {
|
|
|
+ fprintf(stderr,"%s\n",dlerror());
|
|
|
+ exit(EXIT_FAILURE);
|
|
|
+ }
|
|
|
+ ///////////////////////////////add code
|
|
|
+
|
|
|
+
|
|
|
+ predictor = create_predictor(
|
|
|
+ FLAGS_model_file, FLAGS_params_file, FLAGS_gpu_id, FLAGS_use_trt,
|
|
|
+ FLAGS_trt_precision, FLAGS_trt_use_static, FLAGS_trt_static_dir,
|
|
|
+ FLAGS_collect_shape_info, FLAGS_dynamic_shape_file);
|
|
|
+ if (predictor == nullptr) {
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ gpa = iv::modulecomm::RegisterRecv(&lidarname[0],ListenPointCloud);
|
|
|
+ gpdetect = iv::modulecomm::RegisterSend(&detectname[0], 10000000,1);
|
|
|
+ int num = 0;
|
|
|
+ while(1)
|
|
|
+ {
|
|
|
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
+ }
|
|
|
+
|
|
|
+ return 0;
|
|
|
+}
|
