pointpillas modified

src/detection/detection_lidar_PointPillars_MultiHead/main.cpp

@@ -21,11 +21,11 @@
 #include "roiaware_pool3d.h"
 #include "Eigen/Dense"
 
-iv::Ivfault *gfault = nullptr;
-iv::Ivlog *givlog = nullptr;
+std::unique_ptr<iv::Ivfault> gfault = nullptr;
+std::unique_ptr<iv::Ivlog> givlog = nullptr;
 
 std::thread * gpthread;
-PointPillars * pPillars = nullptr ;
+std::unique_ptr<PointPillars> pPillars = nullptr ;
 void * gpa;
 void * gpdetect;
 int gnothavedatatime = 0;
@@ -61,6 +61,7 @@ typedef struct {
 bool GetRotatedRect(std::vector<float> &out_detections,std::vector<int> &out_labels,
                     std::vector<float> &out_scores, std::vector<BBOX3D> &results)
 {
+    results.clear();
     int obj_size = out_detections.size()/kOutputNumBoxFeature;
     if(out_labels.size()==obj_size && out_scores.size()==obj_size)
     {
@@ -125,6 +126,7 @@ float calcdistance(cv::Point2f center1, cv::Point2f center2)
 //nms
 void nms(std::vector<BBOX3D> &vec_boxs,float threshold,std::vector<BBOX3D> &results)
 {
+    results.clear();
     std::sort(vec_boxs.begin(),vec_boxs.end(),sort_score);
     while(vec_boxs.size() > 0)
     {
@@ -146,6 +148,7 @@ void nms(std::vector<BBOX3D> &vec_boxs,float threshold,std::vector<BBOX3D> &resu
 }
 void GetLidarObj(std::vector<BBOX3D> &results,iv::lidar::objectarray & lidarobjvec)
 {
+    lidarobjvec.clear_obj();
     int i;
     int obj_size = results.size();
     //    givlog->verbose("OBJ","object size is %d",obj_size);;
@@ -230,63 +233,63 @@ void GetLidarObj(std::vector<BBOX3D> &results,iv::lidar::objectarray & lidarobjv
 ////////////////////用于nms////////////////////
 
 
-////////////////////用于获得3dbbox中点云个数////////////////////
-#if 0
-inline void lidar_to_local_coords_cpu(float shift_x, float shift_y, float rot_angle, float &local_x, float &local_y){
-    float cosa = cos(-rot_angle), sina = sin(-rot_angle);
-    local_x = shift_x * cosa + shift_y * (-sina);
-    local_y = shift_x * sina + shift_y * cosa;
-}
-
-
-inline int check_pt_in_box3d_cpu(const float *pt, std::vector<float> &box3d, float &local_x, float &local_y){
-    // param pt: (x, y, z)
-    // param box3d: [x, y, z, dx, dy, dz, heading], (x, y, z) is the box center
-    const float MARGIN = 1e-2;
-    float x = pt[0], y = pt[1], z = pt[2];
-    float cx = box3d[0], cy = box3d[1], cz = box3d[2];
-    float dx = box3d[3], dy = box3d[4], dz = box3d[5], rz = box3d[6];
-
-    if (fabsf(z - cz) > dz / 2.0) return 0;
-    lidar_to_local_coords_cpu(x - cx, y - cy, rz, local_x, local_y);
-    float in_flag = (fabs(local_x) < dx / 2.0 + MARGIN) & (fabs(local_y) < dy / 2.0 + MARGIN);
-    return in_flag;
-}
-
-int points_in_boxes_cpu(std::vector<BBOX3D>& boxes, float* pts_lidar,
-                        int pts_num, std::vector<std::vector<int>>& pts_indices){
-
-    std::vector<std::vector<float>> pts_bboxes;
-
-    int boxes_num = boxes.size();
-    float local_x = 0, local_y = 0;
-    for (int i = 0; i < boxes_num; i++){
-        std::vector<float>pts_bbox;
-        for (int j = 0; j < pts_num; j++){
-            int cur_in_flag = check_pt_in_box3d_cpu(pts_lidar + j * 5, boxes[i].detection, local_x, local_y);
-            pts_indices[i][j] = cur_in_flag;
-            if(cur_in_flag)
-            {
-                pts_bbox.push_back(pts_lidar[j*5+0]);
-                pts_bbox.push_back(pts_lidar[j*5+1]);
-                pts_bbox.push_back(pts_lidar[j*5+2]);
-                pts_bbox.push_back(pts_lidar[j*5+3]);
-                pts_bbox.push_back(pts_lidar[j*5+4]);
-            }
+//////////////////////用于获得3dbbox中点云个数////////////////////
+//#if 0
+//inline void lidar_to_local_coords_cpu(float shift_x, float shift_y, float rot_angle, float &local_x, float &local_y){
+//    float cosa = cos(-rot_angle), sina = sin(-rot_angle);
+//    local_x = shift_x * cosa + shift_y * (-sina);
+//    local_y = shift_x * sina + shift_y * cosa;
+//}
+
+
+//inline int check_pt_in_box3d_cpu(const float *pt, std::vector<float> &box3d, float &local_x, float &local_y){
+//    // param pt: (x, y, z)
+//    // param box3d: [x, y, z, dx, dy, dz, heading], (x, y, z) is the box center
+//    const float MARGIN = 1e-2;
+//    float x = pt[0], y = pt[1], z = pt[2];
+//    float cx = box3d[0], cy = box3d[1], cz = box3d[2];
+//    float dx = box3d[3], dy = box3d[4], dz = box3d[5], rz = box3d[6];
+
+//    if (fabsf(z - cz) > dz / 2.0) return 0;
+//    lidar_to_local_coords_cpu(x - cx, y - cy, rz, local_x, local_y);
+//    float in_flag = (fabs(local_x) < dx / 2.0 + MARGIN) & (fabs(local_y) < dy / 2.0 + MARGIN);
+//    return in_flag;
+//}
+
+//int points_in_boxes_cpu(std::vector<BBOX3D>& boxes, float* pts_lidar,
+//                        int pts_num, std::vector<std::vector<int>>& pts_indices){
+
+//    std::vector<std::vector<float>> pts_bboxes;
+
+//    int boxes_num = boxes.size();
+//    float local_x = 0, local_y = 0;
+//    for (int i = 0; i < boxes_num; i++){
+//        std::vector<float>pts_bbox;
+//        for (int j = 0; j < pts_num; j++){
+//            int cur_in_flag = check_pt_in_box3d_cpu(pts_lidar + j * 5, boxes[i].detection, local_x, local_y);
+//            pts_indices[i][j] = cur_in_flag;
+//            if(cur_in_flag)
+//            {
+//                pts_bbox.push_back(pts_lidar[j*5+0]);
+//                pts_bbox.push_back(pts_lidar[j*5+1]);
+//                pts_bbox.push_back(pts_lidar[j*5+2]);
+//                pts_bbox.push_back(pts_lidar[j*5+3]);
+//                pts_bbox.push_back(pts_lidar[j*5+4]);
+//            }
 
-        }
-        pts_bboxes.push_back(pts_bbox);
+//        }
+//        pts_bboxes.push_back(pts_bbox);
 
-        std::cout<<"the size of points: "<<i<<" : "<<pts_bbox.size() / 5 <<std::endl;
+//        std::cout<<"the size of points: "<<i<<" : "<<pts_bbox.size() / 5 <<std::endl;
 
-        pts_bbox.clear();
-    }
+//        pts_bbox.clear();
+//    }
 
-    return 1;
-}
+//    return 1;
+//}
 
-#endif
-////////////////////用于获得3dbbox中点云个数////////////////////
+//#endif
+//////////////////////用于获得3dbbox中点云个数////////////////////
 
 
 void PclToArray(
@@ -419,7 +422,7 @@ void GetLidarObj(std::vector<float> out_detections,std::vector<int> out_labels,
 
 void DectectOnePCD(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr)
 {
-    std::shared_ptr<float> points_array_ptr = std::shared_ptr<float>(new float[pc_ptr->size() * kNumPointFeature]);
+    std::unique_ptr<float> points_array_ptr = std::unique_ptr<float>(new float[pc_ptr->size() * kNumPointFeature]);
     //   float* points_array = new float[pc_ptr->size() * kNumPointFeature];
     PclXYZITToArray(pc_ptr, points_array_ptr.get(), 1.0);
 
@@ -472,67 +475,67 @@ void DectectOnePCD(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr)
 
 
 
-    //get lidar points in 3Dbbox in gpu
-    startTime = std::chrono::high_resolution_clock::now();
-    float* dev_points;
-    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&dev_points),
-                        in_num_points * 5 * sizeof(float))); // in_num_points , 5
-    GPU_CHECK(cudaMemset(dev_points, 0, in_num_points * 5 * sizeof(float)));
-    GPU_CHECK(cudaMemcpy(dev_points, points_array_ptr.get(),
-                        in_num_points * 5 * sizeof(float),
-                        cudaMemcpyHostToDevice));
-    int* box_idx_of_points_gpu;
-    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&box_idx_of_points_gpu),
-                        in_num_points * sizeof(int))); // in_num_points , 5
-    GPU_CHECK(cudaMemset(box_idx_of_points_gpu, -1, in_num_points * sizeof(float)));
-
-    int boxes_num = results_bbox.size();
-    float *boxes_cpu = new float[boxes_num*7];
-    for(int i=0;i<boxes_num;i++)
-    {
-        for(int j=0;j<7;j++)
-            *(boxes_cpu + (i*7+j)) = results_bbox[i].detection[j];
-    }
-    float *boxes_gpu;
-    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&boxes_gpu),
-                        boxes_num * 7 * sizeof(float))); // in_num_points , 5
-    GPU_CHECK(cudaMemset(boxes_gpu, 0, boxes_num * 7 * sizeof(float)));
-    GPU_CHECK(cudaMemcpy(boxes_gpu, boxes_cpu,boxes_num * 7 * sizeof(float),
-                        cudaMemcpyHostToDevice));
-    int batch_size = 1;
-    points_in_boxes_launcher(batch_size,boxes_num,in_num_points,boxes_gpu,dev_points,box_idx_of_points_gpu);
-    int* box_idx_of_points_cpu = new int[in_num_points]();
-    cudaMemcpy(box_idx_of_points_cpu, box_idx_of_points_gpu, in_num_points * sizeof(int), cudaMemcpyDeviceToHost);
-    //vector<int> box_idx_of_points(box_idx_of_points_cpu,box_idx_of_points_cpu+in_num_points);
-
-
-    //cv::Mat image=cv::Mat::zeros(1200,1200,CV_8UC3);
-    //存储bbox的点云
-    for(int i=0; i < in_num_points; i++)
-    {
-
-        for(int j=0; j<boxes_num; j++)
-        {
-            if (box_idx_of_points_cpu[i] == j)
-            {
-                for(int idx=0; idx<5; idx++)
-                    results_bbox[j].bbox_pts.push_back(points_array_ptr.get()[i*5+idx]);
-//                int x = int(points_array_ptr.get()[i*5]*10+600);
-//                int y = int(points_array_ptr.get()[i*5+1]*10+600);
-//                cv::circle(image,cv::Point(x,y),2,cv::Scalar(0,0,255));
-            }
+//    //get lidar points in 3Dbbox in gpu
+//    startTime = std::chrono::high_resolution_clock::now();
+//    float* dev_points;
+//    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&dev_points),
+//                        in_num_points * 5 * sizeof(float))); // in_num_points , 5
+//    GPU_CHECK(cudaMemset(dev_points, 0, in_num_points * 5 * sizeof(float)));
+//    GPU_CHECK(cudaMemcpy(dev_points, points_array_ptr.get(),
+//                        in_num_points * 5 * sizeof(float),
+//                        cudaMemcpyHostToDevice));
+//    int* box_idx_of_points_gpu;
+//    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&box_idx_of_points_gpu),
+//                        in_num_points * sizeof(int))); // in_num_points , 5
+//    GPU_CHECK(cudaMemset(box_idx_of_points_gpu, -1, in_num_points * sizeof(float)));
 
-        }
-    }
-//    for(int j=0; j<boxes_num; j++)
+//    int boxes_num = results_bbox.size();
+//    float *boxes_cpu = new float[boxes_num*7];
+//    for(int i=0;i<boxes_num;i++)
+//    {
+//        for(int j=0;j<7;j++)
+//            *(boxes_cpu + (i*7+j)) = results_bbox[i].detection[j];
+//    }
+//    float *boxes_gpu;
+//    GPU_CHECK(cudaMalloc(reinterpret_cast<void**>(&boxes_gpu),
+//                        boxes_num * 7 * sizeof(float))); // in_num_points , 5
+//    GPU_CHECK(cudaMemset(boxes_gpu, 0, boxes_num * 7 * sizeof(float)));
+//    GPU_CHECK(cudaMemcpy(boxes_gpu, boxes_cpu,boxes_num * 7 * sizeof(float),
+//                        cudaMemcpyHostToDevice));
+//    int batch_size = 1;
+//    points_in_boxes_launcher(batch_size,boxes_num,in_num_points,boxes_gpu,dev_points,box_idx_of_points_gpu);
+//    int* box_idx_of_points_cpu = new int[in_num_points]();
+//    cudaMemcpy(box_idx_of_points_cpu, box_idx_of_points_gpu, in_num_points * sizeof(int), cudaMemcpyDeviceToHost);
+//    //vector<int> box_idx_of_points(box_idx_of_points_cpu,box_idx_of_points_cpu+in_num_points);
+
+
+//    //cv::Mat image=cv::Mat::zeros(1200,1200,CV_8UC3);
+//    //存储bbox的点云
+//    for(int i=0; i < in_num_points; i++)
 //    {
 
-//        std::cout<<"num points in bbox: "<<results_bbox[j].bbox_pts.size()/5<<std::endl;
+//        for(int j=0; j<boxes_num; j++)
+//        {
+//            if (box_idx_of_points_cpu[i] == j)
+//            {
+//                for(int idx=0; idx<5; idx++)
+//                    results_bbox[j].bbox_pts.push_back(points_array_ptr.get()[i*5+idx]);
+////                int x = int(points_array_ptr.get()[i*5]*10+600);
+////                int y = int(points_array_ptr.get()[i*5+1]*10+600);
+////                cv::circle(image,cv::Point(x,y),2,cv::Scalar(0,0,255));
+//            }
+
+//        }
 //    }
+////    for(int j=0; j<boxes_num; j++)
+////    {
 
+////        std::cout<<"num points in bbox: "<<results_bbox[j].bbox_pts.size()/5<<std::endl;
+////    }
+//    delete dev_points;
 
-    endTime = std::chrono::high_resolution_clock::now();
-    double gpuDuration = std::chrono::duration_cast<std::chrono::nanoseconds>(endTime - startTime).count()/1000000.0;
+//    endTime = std::chrono::high_resolution_clock::now();
+//    double gpuDuration = std::chrono::duration_cast<std::chrono::nanoseconds>(endTime - startTime).count()/1000000.0;
 //    std::cout <<"3DBoxDuration_gpu Time : "<<gpuDuration<< endl;
 
 
@@ -589,7 +592,7 @@ void ListenPointCloud(const char *strdata,const unsigned int nSize,const unsigne
     int nNameSize;
     nNameSize = *pHeadSize - 4-4-8;
     char * strName = new char[nNameSize+1];strName[nNameSize] = 0;
-    std::shared_ptr<char> str_ptr;
+    std::unique_ptr<char> str_ptr;
     str_ptr.reset(strName);
     memcpy(strName,(char *)((char *)strdata +4),nNameSize);
     point_cloud->header.frame_id = strName;
@@ -687,8 +690,8 @@ int main(int argc, char *argv[])
 
     //    RegisterIVBackTrace();
     tracker.setSettings(settings);
-    gfault = new iv::Ivfault("lidar_pointpillar");
-    givlog = new iv::Ivlog("lidar_pointpillar");
+    gfault.reset(new iv::Ivfault("lidar_pointpillar"));
+    givlog.reset(new iv::Ivlog("lidar_pointpillar"));
 
     gfault->SetFaultState(0,0,"pointpillar initialize. ");
 
@@ -724,26 +727,26 @@ int main(int argc, char *argv[])
     {
 
         std::cout<<"use onnx model."<<std::endl;
-        pPillars = new PointPillars(
+        pPillars.reset(new PointPillars(
                     0.15,
                     0.10,
                     true,
                     pfe_file,
                     backbone_file,
                     pp_config
-                    );
+                    ));
     }
     else
     {
         std::cout<<"use trt model."<<std::endl;
-        pPillars = new PointPillars(
+        pPillars.reset(new PointPillars(
                     0.15,
                     0.10,
                     false,
                     pfe_trt_file,
                     backbone_trt_file,
                     pp_config
-                    );
+                    ));
     }
     std::cout<<"PointPillars Init OK."<<std::endl;
     Eigen::AngleAxisd r_z ( 0.0356, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转 yaw   +

src/detection/detection_lidar_PointPillars_MultiHead/pointpillars.cc

@@ -517,6 +517,7 @@ void PointPillars::DoInference(const float* in_points_array,
 //        std::cout << setiosflags(ios::left) << setw(14) << Modules[i]  << setw(8)  << Times[i] * 1000 << " ms" << resetiosflags(ios::left) << std::endl;
 //    }
 //    std::cout << "------------------------------------" << std::endl;
+    GPU_CHECK(cudaFree(dev_points));
     cudaStreamDestroy(stream);
 
 }

src/detection/detection_lidar_PointPillars_MultiHead/postprocess.cu

@@ -512,5 +512,6 @@ void PostprocessCuda::DoPostprocessCuda(
 
         GPU_CHECK(cudaFree(dev_indexes));
         GPU_CHECK(cudaFree(dev_sorted_filtered_box));
+        GPU_CHECK(cudaFree(dev_sorted_filtered_scores));
     }
 }