modularization/src/detection/detection_lidar_PointPillars_MultiHead/main.cpp

#include <QCoreApplication>
#include <QDateTime>
#include <iostream>
#include "pointpillars.h"
#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include "xmlparam.h"
#include "modulecomm.h"
#include "ivfault.h"
#include "ivlog.h"
#include "ivexit.h"
#include "ivversion.h"
#include <thread>
#include "objectarray.pb.h"
//#include "ivbacktrace.h"
#include "Tracking.hpp"

#include "roiaware_pool3d.h"
#include "Eigen/Dense"

iv::Ivfault *gfault = nullptr;
iv::Ivlog *givlog = nullptr;

std::thread * gpthread;
PointPillars * pPillars = nullptr ;
void * gpa;
void * gpdetect;
int gnothavedatatime = 0;
const int kNumPointFeature = 5;
const int kOutputNumBoxFeature = 7;
std::string gstrinput;
std::string gstroutput;
Eigen::Matrix3d rotation_matrix;
Eigen::Vector3d trans_matrix;
TrackerSettings settings;
CTracker tracker(settings);
bool m_isTrackerInitialized = false;


#include <random>
////////////////////用于nms////////////////////
#include<opencv2/opencv.hpp>
#define rad2Angle(rad) ((rad) * 180.0 / M_PI)

const float smallinbig_threshold = 0.8;
const float distance_threshold = 0.1;
const float secondnms_threshold = 0.1;

typedef struct {
    cv::RotatedRect box;
    std::vector<float> detection;
    std::vector<float> bbox_pts;
    int label;
    float score;
}BBOX3D;

//将检测结果转为RotatedRect以便于nms计算
bool GetRotatedRect(std::vector<float> &out_detections,std::vector<int> &out_labels,
                    std::vector<float> &out_scores, std::vector<BBOX3D> &results)
{
    int obj_size = out_detections.size()/kOutputNumBoxFeature;
    if(out_labels.size()==obj_size && out_scores.size()==obj_size)
    {
        for(int i=0;i<obj_size;i++)
        {
            BBOX3D result;
            result.box = cv::RotatedRect(cv::Point2f(out_detections.at(i*7),out_detections.at(i*7+1)),
                                         cv::Size2f(out_detections.at(i*7+3),out_detections.at(i*7+4)),
                                         rad2Angle(out_detections.at(i*7+6)));
            for(int j=0;j<kOutputNumBoxFeature;j++)
                result.detection.push_back(out_detections.at(i*7+j));
            result.label = out_labels.at(i);
            result.score = out_scores.at(i);
            results.push_back(result);
        }
        return true;
    }
    else
    {
        std::cout<<"the size of detections labels scores is not equal !!!"<<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;
    vector<cv::Point2f> vertices;
    int intersectionType = cv::rotatedRectangleIntersection(rect1, rect2, vertices);
    if (vertices.size()==0)
        return 0.0;
    else{
        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;

    }
}
void GetLidarObj(std::vector<BBOX3D> &results,iv::lidar::objectarray & lidarobjvec)
{
    int i;
    int obj_size = results.size();
    //    givlog->verbose("OBJ","object size is %d",obj_size);;
    for(i=0;i<obj_size;i++)
    {
        iv::lidar::lidarobject lidarobj;
        if (results.at(i).score < 0.10) {
            std::cout<<"///////: "<<results.at(i).score<<std::endl;
            continue;
        }


        //if (results.at(i).label == 5) continue;

        vector<float>out_detection = results.at(i).detection;
        lidarobj.set_tyaw(out_detection.at(6));
        iv::lidar::PointXYZ centroid;
        iv::lidar::PointXYZ * _centroid;
        centroid.set_x(out_detection.at(0));
        centroid.set_y(out_detection.at(1));
        centroid.set_z(out_detection.at(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(out_detection.at(0));
        position.set_y(out_detection.at(1));
        position.set_z(out_detection.at(2));
        _position = lidarobj.mutable_position();
        _position->CopyFrom(position);
        lidarobj.set_mntype(results.at(i).label);
        // label 2  8
        if(results.at(i).label==2){
            lidarobj.set_mntype(8);
        }else if(results.at(i).label==8){
            lidarobj.set_mntype(2);
        }
        lidarobj.set_score(results.at(i).score);
        lidarobj.add_type_probs(results.at(i).score);
        iv::lidar::PointXYZI point_cloud;
        iv::lidar::PointXYZI * _point_cloud;
        point_cloud.set_x(out_detection.at(0));
        point_cloud.set_y(out_detection.at(1));
        point_cloud.set_z(out_detection.at(2));
        point_cloud.set_i(results.at(i).label);
        _point_cloud = lidarobj.add_cloud();
        _point_cloud->CopyFrom(point_cloud);
        iv::lidar::Dimension ld;
        iv::lidar::Dimension * pld;
        ld.set_x(out_detection.at(3));
        ld.set_y(out_detection.at(4));
        ld.set_z(out_detection.at(5));
        pld = lidarobj.mutable_dimensions();
        pld->CopyFrom(ld);

//        std::cout<<"x y z:  "<<out_detection.at(0)<<" "<<out_detection.at(1)<<" "<<
//        out_detection.at(2)<<" "<<out_detection.at(3)<<" "<< out_detection.at(4)<<" "
//        <<out_detection.at(5)<<" "<<out_detection.at(6)<<std::endl;

        iv::lidar::lidarobject * po = lidarobjvec.add_obj();
        po->CopyFrom(lidarobj);
    }

//   std::cout<<"the lidarobjvec: "<<lidarobjvec.obj_size()<<std::endl;
}
////////////////////用于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]);
            }

        }
        pts_bboxes.push_back(pts_bbox);

        std::cout<<"the size of points: "<<i<<" : "<<pts_bbox.size() / 5 <<std::endl;

        pts_bbox.clear();
    }

    return 1;
}

#endif
////////////////////用于获得3dbbox中点云个数////////////////////


void PclToArray(
        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);
    }
}

void PclXYZITToArray(
        const pcl::PointCloud<pcl::PointXYZI>::Ptr& in_pcl_pc_ptr,
        float* out_points_array, const float normalizing_factor) {

    /////shuffle the index array/////
    bool shuffle = true;
    int point_num = in_pcl_pc_ptr->size();
    std::vector<int>indices(point_num);
    std::iota(indices.begin(),indices.end(),0);
    if(shuffle)
    {
//        unsigned seed = 0;
//        std::shuffle(indices.begin(),indices.end(),std::default_random_engine(seed));

        std::random_device rd;
        std::mt19937 g(rd());
        std::shuffle(indices.begin(),indices.end(),g);

    }
    /////shuffle the index array/////

    for (size_t i = 0; i < in_pcl_pc_ptr->size(); ++i) {
        //pcl::PointXYZI point = in_pcl_pc_ptr->at(i);
        int indice = indices[i];
        pcl::PointXYZI point = in_pcl_pc_ptr->at(indice);
        Eigen::Vector3d new_point, old_point;
        old_point<<point.x, point.y, point.z;
        new_point = rotation_matrix * (old_point) + trans_matrix;
        out_points_array[i * 5 + 0] = new_point[0];
        out_points_array[i * 5 + 1] = new_point[1];
        out_points_array[i * 5 + 2] = new_point[2];
        out_points_array[i * 5 + 3] =
                static_cast<float>(point.intensity / normalizing_factor);
        out_points_array[i * 5 + 4] = 0;
    }
}

void GetLidarObj(std::vector<float> out_detections,std::vector<int> out_labels,
                 std::vector<float> out_scores,iv::lidar::objectarray & lidarobjvec)
{
    int i;
    int obj_size = out_detections.size()/kOutputNumBoxFeature;
    //    givlog->verbose("OBJ","object size is %d",obj_size);
    for(i=0;i<obj_size;i++)
    {
        iv::lidar::lidarobject lidarobj;
        if (out_scores.at(i) < 0.10) continue;

        lidarobj.set_tyaw(out_detections.at(i*7+6));
        iv::lidar::PointXYZ centroid;
        iv::lidar::PointXYZ * _centroid;
        centroid.set_x(out_detections.at(i*7));
        centroid.set_y(out_detections.at(i*7+1));
        centroid.set_z(out_detections.at(i*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(out_detections.at(i*7));
        position.set_y(out_detections.at(i*7+1));
        position.set_z(out_detections.at(i*7+2));
        _position = lidarobj.mutable_position();
        _position->CopyFrom(position);
        lidarobj.set_mntype(out_labels.at(i));
        // label 2  8
        if(out_labels.at(i)==2){
            lidarobj.set_mntype(8);
        }else if(out_labels.at(i)==8){
            lidarobj.set_mntype(2);
        }
        lidarobj.set_score(out_scores.at(i));
        lidarobj.add_type_probs(out_scores.at(i));

        iv::lidar::PointXYZI point_cloud;
        iv::lidar::PointXYZI * _point_cloud;
        point_cloud.set_x(out_detections.at(i*7));
        point_cloud.set_y(out_detections.at(i*7+1));
        point_cloud.set_z(out_detections.at(i*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(out_detections.at(i*7+3));// w
        ld.set_y(out_detections.at(i*7+4));// l
        ld.set_z(out_detections.at(i*7+5));// h
        pld = lidarobj.mutable_dimensions();
        pld->CopyFrom(ld);

        //        std::cout<<"x y z   :  "<<out_detections.at(i*7+3)<<"    "<< out_detections.at(i*7+4)<<"    "<<out_detections.at(i*7+5)<<std::endl;
        iv::lidar::lidarobject * po = lidarobjvec.add_obj();
        po->CopyFrom(lidarobj);
    }

}

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]);
    //   float* points_array = new float[pc_ptr->size() * kNumPointFeature];
    PclXYZITToArray(pc_ptr, points_array_ptr.get(), 1.0);

    int in_num_points = pc_ptr->width;

    std::vector<float> out_detections;
    std::vector<int> out_labels;
    std::vector<float> out_scores;

    QTime xTime;

    xTime.start();
    auto startTime = std::chrono::high_resolution_clock::now();
    cudaDeviceSynchronize();
    pPillars->DoInference(points_array_ptr.get(), in_num_points, &out_detections, &out_labels , &out_scores);
    cudaDeviceSynchronize();
    auto endTime = std::chrono::high_resolution_clock::now();
    double inferenceDuration = std::chrono::duration_cast<std::chrono::nanoseconds>(endTime - startTime).count()/1000000.0;
//    std::cout<< "inferenceDuration Time: " << inferenceDuration << " ms"<< std::endl;


//    int BoxFeature = 7;
//    int num_objects = out_detections.size() / BoxFeature;
//    givlog->verbose("obj size is %d", num_objects);
//    std::cout<<"obj size is "<<num_objects<<std::endl;


//    iv::lidar::objectarray lidarobjvec;
//    GetLidarObj(out_detections,out_labels,out_scores,lidarobjvec);

    //////////nms//////////
    //startTime = std::chrono::high_resolution_clock::now();
    std::vector<BBOX3D>results_rect;
    GetRotatedRect(out_detections,out_labels,out_scores,results_rect);
//    std::cout<<"results_rect size: "<<results_rect.size()<<std::endl;

    std::vector<BBOX3D>results_bbox;
    nms(results_rect,secondnms_threshold,results_bbox);
//    std::cout<<"results_bbox size: "<<results_bbox.size()<<std::endl;

      //get lidar points in 3Dbbox in cpu
//    startTime = std::chrono::high_resolution_clock::now();
//    //get lidar points in 3Dbbox
//    int boxes_num = results_bbox.size();
//    std::vector<std::vector<int>>pts_indices(boxes_num, vector<int>(in_num_points, 0));
//    points_in_boxes_cpu(results_bbox,points_array_ptr.get(),in_num_points,pts_indices);
//    endTime = std::chrono::high_resolution_clock::now();
//    double nmsDuration = std::chrono::duration_cast<std::chrono::nanoseconds>(endTime - startTime).count()/1000000.0;
//    std::cout <<"3DBoxDuration Time : "<<nmsDuration<< endl;



    //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));
            }

        }
    }
//    for(int j=0; j<boxes_num; j++)
//    {

//        std::cout<<"num points in bbox: "<<results_bbox[j].bbox_pts.size()/5<<std::endl;
//    }


    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;


    //endTime = std::chrono::high_resolution_clock::now();
    //double nmsDuration = std::chrono::duration_cast<std::chrono::nanoseconds>(endTime - startTime).count()/1000000.0;
    //std::cout <<"nmsDuration Time : "<<(double)(ends - start)/ CLOCKS_PER_SEC*1000 << endl;

    iv::lidar::objectarray lidarobjvec;
    GetLidarObj(results_bbox,lidarobjvec);
    //////////nms//////////

    double timex = pc_ptr->header.stamp;
    timex = timex/1000.0;
    lidarobjvec.set_timestamp(pc_ptr->header.stamp);

    //---------------------------------------------  init tracker  -------------------------------------------------
//    if (!m_isTrackerInitialized)
//    {
//        m_isTrackerInitialized = InitTracker(tracker);
//        if (!m_isTrackerInitialized)
//        {
//            std::cerr << "Tracker initialize error!!!" << std::endl;
//        }
//    }
//    iv::lidar::objectarray trackedobjvec = Tracking(lidarobjvec, tracker);

    //--------------------------------------------  end tracking  --------------------------------------------------

    int ntlen;
    std::string out = lidarobjvec.SerializeAsString();

    iv::modulecomm::ModuleSendMsg(gpdetect,out.data(),out.length());

    //    givlog->verbose("lenth is %d",out.length());
}

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;
    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;p++;
        if (((abs(xp.x)<0.5)&&(xp.y<0.5 && xp.y > -1.0)) || xp.z > 3.0 ) continue;
        point_cloud->push_back(xp);
    }

    DectectOnePCD(point_cloud);
    std::cout<<"time is "<<(QDateTime::currentMSecsSinceEpoch() % 1000)<<" "<<xTime.elapsed()<<std::endl;
    gfault->SetFaultState(0, 0, "ok");

}

bool gbstate = true;
void statethread()
{
    int nstate = 0;
    int nlaststate = 0;
    while (gbstate)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
        if(gnothavedatatime < 100000) gnothavedatatime++;

        if (gnothavedatatime  < 100){
            nstate = 0;
        }
        if (gnothavedatatime > 1000)
        {
            nstate = 1;
        }
        if (gnothavedatatime > 6000)
        {
            nstate = 2;
        }
        if (nstate != nlaststate) {
            switch (nstate) {
            case 0:
                givlog->info("detection_lidar_pointpillar is ok");
                gfault->SetFaultState(0,0,"data is ok.");
                break;
            case 1:
                givlog->info(" more than 10 seconds not have lidar pointcloud.");
                gfault->SetFaultState(1,1,"more than 10 seconds not have lidar pointcloud.");
                break;
            case 2:
                givlog->info(" more than 60 seconds not have lidar pointcloud.");
                gfault->SetFaultState(2,2, "more than 60 seconds not have lidar pointcloud.");
                break;
            default:
                break;
            }
        }
    }
}

void exitfunc()
{
    gbstate = false;
    gpthread->join();
    std::cout<<" state thread closed."<<std::endl;
    iv::modulecomm::Unregister(gpa);
    iv::modulecomm::Unregister(gpdetect);
    std::cout<<"exit func complete"<<std::endl;
}
#include <QFile>
bool trtisexist(std::string strpfe,std::string strbackbone)
{
    QFile xFile;
    xFile.setFileName(strpfe.data());
    if(xFile.exists() == false)
    {
        return false;
    }
    xFile.setFileName(strbackbone.data());
    if(xFile.exists() == false)
    {
        return false;
    }
    return true;
}
int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);

    //    RegisterIVBackTrace();
    tracker.setSettings(settings);
    gfault = new iv::Ivfault("lidar_pointpillar");
    givlog = new iv::Ivlog("lidar_pointpillar");

    gfault->SetFaultState(0,0,"pointpillar initialize. ");

    char * strhome = getenv("HOME");
    std::string pfe_file = strhome;
    pfe_file += "/models/lidar/cbgs_pp_multihead_pfe.onnx";
    std::string pfe_trt_file = pfe_file.substr(0, pfe_file.find(".")) + ".trt";

    std::string backbone_file = strhome;
    backbone_file += "/models/lidar/cbgs_pp_multihead_backbone.onnx";
    std::string backbone_trt_file = backbone_file.substr(0, backbone_file.find(".")) + ".trt";

    bool btrtexist = trtisexist(pfe_trt_file,backbone_trt_file);


    QString strpath = QCoreApplication::applicationDirPath();
    std::string pp_config = strpath.toStdString() ;
    pp_config += "/cfgs/cbgs_pp_multihead.yaml";
    if (argc < 2)
        strpath = strpath + "/detection_lidar_pointpillar.xml";
    else
        strpath = argv[1];

    std::cout<<pp_config<<std::endl;

    iv::xmlparam::Xmlparam xparam(strpath.toStdString());
    pfe_file = xparam.GetParam("pfe_file",pfe_file.data());
    backbone_file = xparam.GetParam("backbone_file",backbone_file.data());
    gstrinput = xparam.GetParam("input","lidar_pc");
    gstroutput = xparam.GetParam("output","lidar_pointpillar");

    if(btrtexist == false)
    {

        std::cout<<"use onnx model."<<std::endl;
        pPillars = new PointPillars(
                    0.15,
                    0.10,
                    true,
                    pfe_file,
                    backbone_file,
                    pp_config
                    );
    }
    else
    {
        std::cout<<"use trt model."<<std::endl;
        pPillars = 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.00654, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转 yaw   +
    Eigen::AngleAxisd r_y ( 0.13, Eigen::Vector3d ( 0,1,0 ) ); //沿 Y 轴旋转 roll  +
    Eigen::AngleAxisd r_x ( -0.0377, Eigen::Vector3d ( 1,0,0 ) ); //沿 X 轴旋转 pitch -
    Eigen::Quaterniond q_zyx = r_z*r_y*r_x; //ZYX旋转顺序（绕旋转后的轴接着旋转）
    // 四元数-->>旋转矩阵
    rotation_matrix = q_zyx.toRotationMatrix();
    trans_matrix << 0, 1.1, 0.35;//x,y,z
    gpa = iv::modulecomm::RegisterRecv(gstrinput.data(),ListenPointCloud);
    gpdetect = iv::modulecomm::RegisterSend(gstroutput.data(), 10000000,1);
    gpthread = new std::thread(statethread);

    iv::ivexit::RegIVExitCall(exitfunc);
    return a.exec();
}