pointpillar and fusion_pointpillar

src/detection/detection_lidar_PointPillars_MultiHead/.gitignore

@@ -18,7 +18,9 @@ DEFINES += QT_DEPRECATED_WARNINGS
 #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
 
 SOURCES += main.cpp \
-    pointpillars.cc
+    pointpillars.cc \
+    ../../include/msgtype/object.pb.cc \
+    ../../include/msgtype/objectarray.pb.cc
 
 DISTFILES += \
     nms.cu \
@@ -32,7 +34,9 @@ HEADERS += \
     pointpillars.h \
     postprocess.h \
     preprocess.h \
-    scatter.h
+    scatter.h \
+    ../../include/msgtype/object.pb.h \
+    ../../include/msgtype/objectarray.pb.h
 
 
 CUDA_SOURCES +=  \
@@ -132,6 +136,13 @@ unix:INCLUDEPATH += /usr/include/eigen3
 unix:INCLUDEPATH += /usr/include/pcl-1.7
 unix:INCLUDEPATH += /usr/include/pcl-1.8
 
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
 
 LIBS += -lboost_system
 

src/detection/detection_lidar_PointPillars_MultiHead/main.cpp

@@ -1,18 +1,35 @@
 #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"
+
+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;
+
 
 void PclToArray(
     const pcl::PointCloud<pcl::PointXYZI>::Ptr& in_pcl_pc_ptr,
@@ -41,103 +58,255 @@ void PclXYZITToArray(
   }
 }
 
-void Boxes2Txt( std::vector<float> boxes , string file_name , int num_feature = 7)
+void GetLidarObj(std::vector<float> out_detections,std::vector<int> out_labels,
+                 std::vector<float> out_scores,iv::lidar::objectarray & lidarobjvec)
 {
-    ofstream ofFile;
-    ofFile.open(file_name , std::ios::out );
-    if (ofFile.is_open()) {
-        for (int i = 0 ; i < boxes.size() / num_feature ; ++i) {
-            for (int j = 0 ; j < num_feature ; ++j) {
-                ofFile << boxes.at(i * num_feature + j) << " ";
-            }
-            ofFile << "\n";
-        }
+    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++)
+    {
+        if (out_scores.at(i) < 0.10) continue;
+        iv::lidar::lidarobject lidarobj;
+        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));
+        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(out_detections.at(out_labels.at(i)));
+        _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));
+        ld.set_y(out_detections.at(i*7+4));
+        ld.set_z(out_detections.at(i*7+5));
+        pld = lidarobj.mutable_dimensions();
+        pld->CopyFrom(ld);
+
+        iv::lidar::lidarobject * po = lidarobjvec.add_obj();
+        po->CopyFrom(lidarobj);
     }
-    ofFile.close();
-    return ;
-};
-
+}
 
-void test(std::string pfe_file,std::string backbone_file,std::string pp_config)
+void DectectOnePCD(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr)
 {
+    float* points_array = new float[pc_ptr->size() * kNumPointFeature];
+    PclXYZITToArray(pc_ptr, points_array, 1.0);
 
-    PointPillars * pPillars;
-    pPillars = new PointPillars(
-      0.1,
-      0.2,
-      true,
-      pfe_file,
-      backbone_file,
-      pp_config
-    );
-    std::cout<<"PointPillars Init OK."<<std::endl;
+    int    in_num_points = pc_ptr->width;
 
-    pcl::PointCloud<pcl::PointXYZI>::Ptr xpc(
-        new pcl::PointCloud<pcl::PointXYZI>);
+    std::vector<float> out_detections;
+    std::vector<int> out_labels;
+    std::vector<float> out_scores;
 
+    QTime xTime;
 
-    pcl::PointCloud<pcl::PointXYZI>::Ptr xpcfile;
+    xTime.start();
 
-    xpcfile = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI>>(new pcl::PointCloud<pcl::PointXYZI>);
+    cudaDeviceSynchronize();
+    pPillars->DoInference(points_array, in_num_points, &out_detections, &out_labels , &out_scores);
+    cudaDeviceSynchronize();
+    int BoxFeature = 7;
+    int num_objects = out_detections.size() / BoxFeature;
 
-    if(0 == pcl::io::loadPCDFile("/home/nvidia/20200604054241688.pcd",*xpc))
-    {
+//    givlog->verbose("obj size is %d", num_objects);
+//    std::cout<<"obj size is "<<num_objects<<std::endl;
 
+//    std::vector<iv::lidar::lidarobject> lidarobjvec;
+    iv::lidar::objectarray lidarobjvec;
+    GetLidarObj(out_detections,out_labels,out_scores,lidarobjvec);
+
+    double timex = pc_ptr->header.stamp;
+    timex = timex/1000.0;
+    lidarobjvec.set_timestamp(pc_ptr->header.stamp);
+
+    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;
     }
-    else
+
+    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;
+    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++)
     {
-        std::cout<<"load pcd fail"<<std::endl;
-        return;
+        pcl::PointXYZI xp;
+        memcpy(&xp,p,sizeof(pcl::PointXYZI));
+        xp.z = xp.z;
+        point_cloud->push_back(xp);
+        p++;
     }
 
-    const int kNumPointFeature = 5;
-    const int kOutputNumBoxFeature = 7;
+    DectectOnePCD(point_cloud);
+    std::cout<<"time is "<<(QDateTime::currentMSecsSinceEpoch() % 1000)<<" "<<xTime.elapsed()<<std::endl;
+    gfault->SetFaultState(0, 0, "ok");
 
+}
 
-//    for (size_t i = 0; i < xpcfile->size(); ++i) {
-//      pcl::PointXYZI point;
-//      point.x = xpcfile->at(i).x;
-//      point.y = xpcfile->at(i).y;qt
-//      point.z = xpcfile->at(i).z;
-//      point.intensity = 0;
-//      xpc->push_back(point);
-//    }
+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++;
 
-    std::cout<<"point size is "<<xpc->width<<std::endl;
+        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;
+}
 
-    float* points_array = new float[xpc->size() * kNumPointFeature];
-    PclXYZITToArray(xpc, points_array, 1.0);
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
 
-    int    in_num_points = xpc->width;
+//    RegisterIVBackTrace();
 
-    std::vector<float> out_detections;
-    std::vector<int> out_labels;
-    std::vector<float> out_scores;
+    gfault = new iv::Ivfault("lidar_pointpillar");
+    givlog = new iv::Ivlog("lidar_pointpillar");
 
-    QTime xTime;
+    gfault->SetFaultState(0,0,"pointpillar initialize. ");
 
-    xTime.start();
+    char * strhome = getenv("HOME");
+    std::string pfe_file = strhome;
+    pfe_file += "/models/lidar/cbgs_pp_multihead_pfe.onnx";
+    std::string backbone_file = strhome;
+    backbone_file += "/models/lidar/cbgs_pp_multihead_backbone.onnx";
 
-    cudaDeviceSynchronize();
-    pPillars->DoInference(points_array, in_num_points, &out_detections, &out_labels , &out_scores);
-    cudaDeviceSynchronize();
-    int BoxFeature = 7;
-    int num_objects = out_detections.size() / BoxFeature;
 
-    std::cout<<"objects num: "<<num_objects<<std::endl;
-    std::cout<<"infer time: "<<xTime.elapsed()<<std::endl;
+    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::string boxes_file_name = "/home/nvidia/pointpillarsout.txt";
-    Boxes2Txt(out_detections , boxes_file_name );
-}
+    std::cout<<pp_config<<std::endl;
 
-int main(int argc, char *argv[])
-{
-    QCoreApplication a(argc, argv);
+    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");
+    pPillars = new PointPillars(
+      0.1,
+      0.2,
+      true,
+      pfe_file,
+      backbone_file,
+      pp_config
+    );
+    std::cout<<"PointPillars Init OK."<<std::endl;
+
+    gpa = iv::modulecomm::RegisterRecv(gstrinput.data(),ListenPointCloud);
+    gpdetect = iv::modulecomm::RegisterSend(gstroutput.data(), 10000000,1);
+    gpthread = new std::thread(statethread);
 
-    test("/home/nvidia/Downloads/cbgs_pp_multihead_pfe.onnx",
-         "/home/nvidia/Downloads/cbgs_pp_multihead_backbone.onnx",
-         "/home/nvidia/code/modularization/src/detection/detection_lidar_PointPillars_MultiHead/cfgs/cbgs_pp_multihead.yaml");
+    iv::ivexit::RegIVExitCall(exitfunc);
     return a.exec();
 }

src/detection/detection_lidar_PointPillars_MultiHead/nms.cu

@@ -473,23 +473,23 @@ void PointPillars::DoInference(const float* in_points_array,
     auto postprocess_end = std::chrono::high_resolution_clock::now();
 
     // release the stream and the buffers
-    std::chrono::duration<double> preprocess_cost = preprocess_end - preprocess_start;
-    std::chrono::duration<double> pfe_cost = pfe_end - pfe_start;
-    std::chrono::duration<double> scatter_cost = scatter_end - scatter_start;
-    std::chrono::duration<double> backbone_cost = backbone_end - backbone_start;
-    std::chrono::duration<double> postprocess_cost = postprocess_end - postprocess_start;
-
-    std::chrono::duration<double> pointpillars_cost = postprocess_end - preprocess_start;
-    std::cout << "------------------------------------" << std::endl;
-    std::cout << setiosflags(ios::left)  << setw(14) << "Module" << setw(12)  << "Time"  << resetiosflags(ios::left) << std::endl;
-    std::cout << "------------------------------------" << std::endl;
-    std::string Modules[] = {"Preprocess" , "Pfe" , "Scatter" , "Backbone" , "Postprocess" , "Summary"};
-    double Times[] = {preprocess_cost.count() , pfe_cost.count() , scatter_cost.count() , backbone_cost.count() , postprocess_cost.count() , pointpillars_cost.count()}; 
-
-    for (int i =0 ; i < 6 ; ++i) {
-        std::cout << setiosflags(ios::left) << setw(14) << Modules[i]  << setw(8)  << Times[i] * 1000 << " ms" << resetiosflags(ios::left) << std::endl;
-    }
-    std::cout << "------------------------------------" << std::endl;
+//    std::chrono::duration<double> preprocess_cost = preprocess_end - preprocess_start;
+//    std::chrono::duration<double> pfe_cost = pfe_end - pfe_start;
+//    std::chrono::duration<double> scatter_cost = scatter_end - scatter_start;
+//    std::chrono::duration<double> backbone_cost = backbone_end - backbone_start;
+//    std::chrono::duration<double> postprocess_cost = postprocess_end - postprocess_start;
+
+//    std::chrono::duration<double> pointpillars_cost = postprocess_end - preprocess_start;
+//    std::cout << "------------------------------------" << std::endl;
+//    std::cout << setiosflags(ios::left)  << setw(14) << "Module" << setw(12)  << "Time"  << resetiosflags(ios::left) << std::endl;
+//    std::cout << "------------------------------------" << std::endl;
+//    std::string Modules[] = {"Preprocess" , "Pfe" , "Scatter" , "Backbone" , "Postprocess" , "Summary"};
+//    double Times[] = {preprocess_cost.count() , pfe_cost.count() , scatter_cost.count() , backbone_cost.count() , postprocess_cost.count() , pointpillars_cost.count()};
+
+//    for (int i =0 ; i < 6 ; ++i) {
+//        std::cout << setiosflags(ios::left) << setw(14) << Modules[i]  << setw(8)  << Times[i] * 1000 << " ms" << resetiosflags(ios::left) << std::endl;
+//    }
+//    std::cout << "------------------------------------" << std::endl;
     cudaStreamDestroy(stream);
 
 }

src/detection/detection_lidar_PointPillars_MultiHead/pointpillars.h

@@ -129,7 +129,7 @@ int main(int argc, char *argv[])
     std::cout<<strpath.toStdString()<<std::endl;
 
     iv::xmlparam::Xmlparam xparam(strpath.toStdString());
-    gstrinput = xparam.GetParam("input","lidar_cnn_dectect");
+    gstrinput = xparam.GetParam("input","lidar_pointpillar");
     gstroutput = xparam.GetParam("output","lidar_track");
 
     gpatrack = iv::modulecomm::RegisterSend(gstroutput.data(),10000000,1);

src/fusion/lidar_radar_fusion_cnn/fusion.hpp

@@ -172,10 +172,10 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
                     float nomal_x = ix*0.2 - xp*0.2;
                     float nomal_y = iy*0.2 - yp*0.2;
                     float nomal_z = lidar_object_arr.obj(match_idx[i].nlidar).centroid().z();
-                    float s = nomal_x*cos(0) -
-                            nomal_y*sin(0);
-                    float t = nomal_x*sin(0) +
-                            nomal_y*cos(0);
+                    float s = nomal_x*cos(lidar_object_arr.obj(match_idx[i].nlidar).tyaw()) -
+                            nomal_y*sin(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
+                    float t = nomal_x*sin(lidar_object_arr.obj(match_idx[i].nlidar).tyaw()) +
+                            nomal_y*cos(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
                     nomal_centroid.set_nomal_x(lidar_object_arr.obj(match_idx[i].nlidar).centroid().x() + s);
                     nomal_centroid.set_nomal_y(lidar_object_arr.obj(match_idx[i].nlidar).centroid().y() + t);
                     nomal_centroid_ = fusion_object.add_nomal_centroid();
@@ -183,56 +183,6 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
                 }
             }
         }
-        for(int k = 0; k<10; k++)
-        {
-            //            std::cout<<"fusion    begin"<<std::endl;
-            iv::fusion::PointXYZ point_forcaste;
-            iv::fusion::PointXYZ *point_forcaste_;
-            float forcast_x = lidar_object_arr.obj(match_idx[i].nlidar).centroid().x() +
-                    lidar_object_arr.obj(match_idx[i].nlidar).velocity_linear_x()*time_step*(k+1);
-            float forcast_y = lidar_object_arr.obj(match_idx[i].nlidar).centroid().y() +
-                    lidar_object_arr.obj(match_idx[i].nlidar).velocity_linear_x()*time_step*(k+1);
-            point_forcaste.set_x(forcast_x);
-            point_forcaste.set_y(forcast_y);
-            point_forcaste.set_z(lidar_object_arr.obj(match_idx[i].nlidar).centroid().z());
-            point_forcaste_ = fusion_object.add_point_forecast();
-            point_forcaste_->CopyFrom(point_forcaste);
-
-            iv::fusion::NomalForecast forcast_normal;
-            iv::fusion::NomalForecast *forcast_normal_;
-            forcast_normal.set_forecast_index(i);
-            //            std::cout<<"fusion     end"<<std::endl;
-            if((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()>0)&&
-                    (lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()>0))
-            {
-                int xp = (int)((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()/0.2)/2.0);
-                if(xp == 0)xp=1;
-                int yp = (int)((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()/0.2)/2.0);
-                if(yp == 0)yp=1;
-                int ix,iy;
-                for(ix = 0; ix<(xp*2); ix++)
-                {
-                    for(iy = 0; iy<(yp*2); iy++)
-                    {
-                        iv::fusion::NomalXYZ nomal_forcast;
-                        iv::fusion::NomalXYZ *nomal_forcast_;
-                        float nomal_x = ix*0.2 - xp*0.2;
-                        float nomal_y = iy*0.2 - yp*0.2;
-                        float nomal_z = lidar_object_arr.obj(match_idx[i].nlidar).centroid().z();
-                        float s = nomal_x*cos(lidar_object_arr.obj(match_idx[i].nlidar).tyaw()) -
-                                nomal_y*sin(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
-                        float t = nomal_x*sin(lidar_object_arr.obj(match_idx[i].nlidar).tyaw()) +
-                                nomal_y*cos(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
-                        nomal_forcast.set_nomal_x(forcast_x + s);
-                        nomal_forcast.set_nomal_y(forcast_y + t);
-                        nomal_forcast_ = forcast_normal.add_forecast_nomal();
-                        nomal_forcast_->CopyFrom(nomal_forcast);
-                    }
-                }
-            }
-            forcast_normal_=fusion_object.add_forecast_nomals();
-            forcast_normal_->CopyFrom(forcast_normal);
-        }
 
         iv::fusion::fusionobject *pobj = lidar_radar_fusion_object_array.add_obj();
         pobj->CopyFrom(fusion_object);
@@ -288,53 +238,6 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
             }
         }
 
-        for(int k = 0; k<10; k++)
-        {
-            //                std::cout<<"fusion    begin"<<std::endl;
-            iv::fusion::PointXYZ point_forcaste;
-            iv::fusion::PointXYZ *point_forcaste_;
-            float forcast_x = radar_object_array.obj(radar_idx[j]).x()
-                    + radar_object_array.obj(radar_idx[j]).vx()*time_step*(k+1);
-            float forcast_y = radar_object_array.obj(radar_idx[j]).y()
-                    + radar_object_array.obj(radar_idx[j]).vy()*time_step*(k+1);
-            point_forcaste.set_x(forcast_x);
-            point_forcaste.set_y(forcast_y);
-            point_forcaste.set_z(1.0);
-            point_forcaste_ = fusion_obj.add_point_forecast();
-            point_forcaste_->CopyFrom(point_forcaste);
-
-            iv::fusion::NomalForecast forcast_normal;
-            iv::fusion::NomalForecast *forcast_normal_;
-            forcast_normal.set_forecast_index(k);
-
-            int xp = (int)((0.5/0.2)/2.0);
-            if(xp == 0)xp=1;
-            int yp = (int)((0.5/0.2)/2.0);
-            if(yp == 0)yp=1;
-            int ix,iy;
-            for(ix = 0; ix<(xp*2); ix++)
-            {
-                for(iy = 0; iy<(yp*2); iy++)
-                {
-                    iv::fusion::NomalXYZ nomal_forcast;
-                    iv::fusion::NomalXYZ *nomal_forcast_;
-                    float nomal_x = ix*0.2 - xp*0.2;
-                    float nomal_y = iy*0.2 - yp*0.2;
-                    float nomal_z = 1.0;
-                    float s = nomal_x*cos(radar_object_array.obj(radar_idx[j]).angle())
-                            - nomal_y*sin(radar_object_array.obj(radar_idx[j]).angle());
-                    float t = nomal_x*sin(radar_object_array.obj(radar_idx[j]).angle())
-                            + nomal_y*cos(radar_object_array.obj(radar_idx[j]).angle());
-                    nomal_forcast.set_nomal_x(forcast_x + s);
-                    nomal_forcast.set_nomal_y(forcast_y + t);
-                    nomal_forcast_ = forcast_normal.add_forecast_nomal();
-                    nomal_forcast_->CopyFrom(nomal_forcast);
-                }
-            }
-            forcast_normal_=fusion_obj.add_forecast_nomals();
-            forcast_normal_->CopyFrom(forcast_normal);
-        }
-
         iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
         obj->CopyFrom(fusion_obj);
     }
@@ -345,7 +248,7 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
 void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array, iv::mobileye::mobileye& xobs_info)
 {
     int time_step = 0.3;
-    lidar_radar_fusion_object_array.Clear();
+//    lidar_radar_fusion_object_array.Clear();
     for(int j = 0; j< xobs_info.xobj_size() ; j++){
         iv::fusion::fusionobject fusion_obj;
         fusion_obj.set_yaw(xobs_info.xobj(j).obsang());

src/fusion/lidar_radar_fusion_cnn/lidar_radar_fusion_cnn.pro

@@ -1,7 +1,7 @@
 QT -= gui
 
-#CONFIG += c++14 console
-CONFIG += c++14
+CONFIG += c++14 console
+#CONFIG += c++14
 CONFIG -= app_bundle
 
 # The following define makes your compiler emit warnings if you use

src/fusion/lidar_radar_fusion_cnn/main.cpp

@@ -65,6 +65,7 @@ void Listenesrfront(const char * strdata,const unsigned int nSize,const unsigned
 void Listenlidarcnndetect(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
     iv::lidar::objectarray lidarobj;
+
     if(nSize<1)return;
     if(false == lidarobj.ParseFromArray(strdata,nSize))
     {
@@ -73,6 +74,7 @@ void Listenlidarcnndetect(const char * strdata,const unsigned int nSize,const un
     }
 //    std::cout<<"  lidar  is  ok   "<<lidarobj.obj_size()<<std::endl;
     gMutex.lock();
+//    std::cout<<"   obj size  "<<lidarobj.obj_size()<<std::endl;
     datafusion(lidarobj,radarobjvec,li_ra_fusion);
     gMutex.unlock();
 }
@@ -87,7 +89,7 @@ void Listenmobileye(const char * strdata,const unsigned int nSize,const unsigned
         std::cout<<"PecerptionShow Listenesrfront fail."<<std::endl;
         return;
     }
-    std::cout<< " obj size   "<<mobileye.xobj_size()<<std::endl;
+//    std::cout<< " obj size   "<<mobileye.xobj_size()<<std::endl;
 //    for (int m_index = 0; m_index< mobileye.xobj_size(); m_index++)
 //    {
 //        std::cout<<" p_x  p_y  v_x  v_y   "<<mobileye.xobj(m_index).pos_y()<< "  "<<mobileye.xobj(m_index).pos_x()
@@ -106,12 +108,19 @@ void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& r
 {
 //    gMutex.lock();
 //    Transformation::RadarPross(radarobjvec);
-//    RLfusion(lidar_obj,radarobjvec,li_ra_fusion);
-    li_ra_fusion.Clear();
+
+    RLfusion(lidar_obj,radarobjvec,li_ra_fusion);
+
+
+//    li_ra_fusion.Clear();
+//      std::cout<<" RLFusion     begin    "<<std::endl;
     AddMobileye(li_ra_fusion,mobileye_info);
+//    std::cout<<" RLFusion     end      "<<std::endl;
     mobileye_info.clear_xobj();
+//        std::cout<< "     fusion size  "<<li_ra_fusion.obj_size()<<std::endl;
 //    for(int i=0;i<li_ra_fusion.obj_size();i++)
 //    {
+//        std::cout<<"  is  ok  "<<std::endl;
 //        std::cout<<"x    y      vx    vy   w "<<li_ra_fusion.obj(i).centroid().x()<<"          "
 //                <<li_ra_fusion.obj(i).centroid().y()<<"         "
 //                <<li_ra_fusion.obj(i).vel_relative().x()<<"     "
@@ -186,7 +195,7 @@ int main(int argc, char *argv[])
 
     void *gpa;
     gpa = iv::modulecomm::RegisterRecv("radar",Listenesrfront);
-    gpa = iv::modulecomm::RegisterRecv("lidar_track",Listenlidarcnndetect);
+    gpa = iv::modulecomm::RegisterRecv("lidar_pointpillar",Listenlidarcnndetect);
     gpa = iv::modulecomm::RegisterRecv("mobileye",Listenmobileye);
     return a.exec();
 }

src/tool/PerceptionShow/ivlidarobjview.cpp

@@ -101,7 +101,7 @@ void IVLidarObjView::paint()
 //        la.append(QString::number(lo.id()).toStdString());
 //        la.append(" vel:");
 //        la.append(QString::number(lo.velocity_linear_x()).toStdString());
-        painter->drawText(QPoint(lo.centroid().x()*10.0,lo.centroid().y()*(-10.0)),la.data());
+//        painter->drawText(QPoint(lo.centroid().x()*10.0,lo.centroid().y()*(-10.0)),la.data());
 
         double x[4],y[4];
         double s,t;

src/tool/PerceptionShow/mainwindow.cpp

@@ -258,14 +258,14 @@ MainWindow::MainWindow(QWidget *parent) :
 
     pa = iv::modulecomm::RegisterRecv("image00",Listenpics0);
 
-    pa = iv::modulecomm::RegisterRecv("lidarpc_center",ListenPointCloud);
-//    pa = iv::modulecomm::RegisterRecv("lidar_pc",ListenPointCloud);
+//    pa = iv::modulecomm::RegisterRecv("lidarpc_center",ListenPointCloud);
+    pa = iv::modulecomm::RegisterRecv("lidar_pc",ListenPointCloud);
 
     pa = iv::modulecomm::RegisterRecv("esr_front",Listenesrfront);
 
 //   pa = iv::modulecomm::RegisterRecv("can0",Listenesrfront);
 
-    pa = iv::modulecomm::RegisterRecv("lidar_track",Listenlidarcnndectect);
+    pa = iv::modulecomm::RegisterRecv("lidar_pointpillar",Listenlidarcnndectect);
 
     pa = iv::modulecomm::RegisterRecv("fusion",ListenFusion);
 

src/tool/bqev_lidar_cnn_detect_view/mainwindow.cpp

@@ -191,9 +191,9 @@ MainWindow::MainWindow(QWidget *parent) :
 //    myview->scale(0.3,0.3);
 //    myview->show();
 
-    mpa = iv::modulecomm::RegisterRecv("lidarpc_center",ListenPointCloud);
+    mpa = iv::modulecomm::RegisterRecv("lidar_pc",ListenPointCloud);
 
-    mpdetect = iv::modulecomm::RegisterRecv("lidar_cnn_dectect",Listenlidarcnndectect);
+    mpdetect = iv::modulecomm::RegisterRecv("lidar_pointpillar",Listenlidarcnndectect);
 
 
     connect(&mTimer,SIGNAL(timeout()),this,SLOT(onTimer()));