modify all

README.md

@@ -14,11 +14,11 @@ driver_gps_hcp2
 driver_can_nvidia_agx
 driver_map_trace
 driver_map_xodrload
-driver_rpc_server
-driver_cloud_grpc_client
-driver_grpc_server
-driver_rpc_server
-driver_group_grpc_client
+#driver_rpc_server
+#driver_cloud_grpc_client
+#driver_grpc_server
+#driver_rpc_server
+#driver_group_grpc_client
 
 ######detection#####
 detection_lidar_PointPillars_MultiHead

autogen.sh

@@ -194,7 +194,7 @@ do
 	cd ../../../
 done
 decition_app_name=(
-#	decition_brain_sf
+	decition_brain_sf
 	#decition_brain_ge3
 	#decition_brain_qingyuan
 	#decition_brain_vv7
@@ -222,11 +222,11 @@ driver_app_name=(
 	driver_can_nvidia_agx
 	driver_map_trace
 	driver_map_xodrload
-	driver_rpc_server
-	driver_cloud_grpc_client
-	driver_grpc_server
-	driver_rpc_server
-	driver_group_grpc_client
+#	driver_rpc_server
+#	driver_cloud_grpc_client
+#	driver_grpc_server
+#	driver_rpc_server
+#	driver_group_grpc_client
 )
 
 for x in ${driver_app_name[@]}

src/common/modulecomm/modulecomm.pro

@@ -16,7 +16,7 @@ unix:system("./../../../include/linuxsystemtest.sh ")
 unix:include(./../../../include/systemdef.pri)
 win32: DEFINES += SYSTEM_WIN
 
-DEFINES += USE_GROUPUDP
+#DEFINES += USE_GROUPUDP
 
 if(contains(DEFINES,USE_GROUPUDP)){
 QT += network

src/decition/common/common/car_status.h

@@ -164,7 +164,7 @@ namespace iv {
         int vehGroupId;
         double mfEPSOff = 0.0;
         float socfDis=5;   //停障触发距离
-        float aocfDis=25;   //避障触发距离//20
+        float aocfDis=23;   //避障触发距离//20
         float aocfTimes=3; //避障触发次数
         float aobzDis=5;   //避障保障距离
         /// traffice light : 0x90
@@ -246,6 +246,11 @@ namespace iv {
 
         int mbNoPathCnt=0;
         bool mbNoPath=false;
+
+
+        //超声波信息
+        //std::vector<iv::ultrasonic::Area> mbUltraArea;
+        float mbUltraDis[4]={200,200,200,200};
     };
     typedef boost::serialization::singleton<iv::CarStatus> CarStatusSingleton;//非线程安全，注意多线程加锁，单例模式
 }

src/decition/decition_brain_sf_jsguide/decition/brain.cpp

@@ -69,6 +69,11 @@ void ListenFusion(const char *strdata, const unsigned int nSize, const unsigned
     gbrain->GetFusion(strdata,nSize);
 }
 
+void ListenUltraArea(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    gbrain->UpdateUltra(strdata,nSize); //超声波数据
+}
+
 /*    void ListenMap_change_req(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
         {
             iv::formation_map_index::map map;
@@ -155,6 +160,8 @@ iv::decition::BrainDecition::BrainDecition()
     mpaObsTraceLeft = iv::modulecomm::RegisterSend("obstraceleft",100000,1);
     mpaObsTraceRight = iv::modulecomm::RegisterSend("obstraceright",100000,1);
 
+    mpUltraArea = iv::modulecomm::RegisterRecv("ultra_area",iv::decition::ListenUltraArea);
+
 
     mTime.start();
     mnOldTime = mTime.elapsed();
@@ -470,6 +477,20 @@ void iv::decition::BrainDecition::run() {
         ServiceCarStatus.avoidObs = false;
     }
 
+    std::string SideDrive = xp.GetParam("SideDrive","false");  //If Use MPC set true
+    if(SideDrive == "true")
+    {
+        ServiceCarStatus.msysparam.mbSideDrive = true;
+    }else{
+        ServiceCarStatus.msysparam.mbSideDrive = false;
+    }
+
+
+     std::string SideDis = xp.GetParam("SideDis","1.0");
+
+    ServiceCarStatus.msysparam.mfSideDis = atof(SideDis.data());
+
+
     mstrmemmap_index = xp.GetParam("msg_mapindex","map_index");
 
 
@@ -1311,6 +1332,7 @@ void iv::decition::BrainDecition::SideMove(iv::GPS_INS &x)
     double rel_x,rel_y;
     GaussProjCal(x.gps_lng,x.gps_lat,&rel_x,&rel_y);
     double fmove = 0;
+    double duesidedis=ServiceCarStatus.msysparam.mfSideDis;
     fmove = ServiceCarStatus.msysparam.mfSideDis + ServiceCarStatus.msysparam.vehWidth/2.0 - x.mfLaneWidth +x.mfDisToLaneLeft;
     if(fmove == 0.0)return;
     double rel_x1,rel_y1;
@@ -1596,14 +1618,15 @@ void iv::decition::BrainDecition::UpdateChassis(const char *strdata, const unsig
     ServiceCarStatus.epb = xchassis.epbfault();
     ServiceCarStatus.grade = xchassis.shift();
     ServiceCarStatus.driverMode = xchassis.drivemode();
-    if(xchassis.has_epsmode()){
-        ServiceCarStatus.epsMode = xchassis.epsmode();
-        ServiceCarStatus.receiveEps=true;
-        if(ServiceCarStatus.epsMode == 0)
-        {
-            ServiceCarStatus.mbRunPause = true;
-        }
-    }
+//    if(xchassis.has_epsmode()){
+//        ServiceCarStatus.epsMode = xchassis.epsmode();
+//        ServiceCarStatus.receiveEps=true;
+//        if(ServiceCarStatus.epsMode == 0)
+//        {
+//            ServiceCarStatus.mbRunPause = true;
+//        }
+//    }
+
     if(xchassis.has_torque())
     {
         ServiceCarStatus.torque_st = xchassis.torque();
@@ -1766,7 +1789,28 @@ void iv::decition::BrainDecition::UpdateVbox(const char *pdata, const int ndatas
 void iv::decition::BrainDecition::UpdateSate(){
     decitionGps00->isFirstRun=true;
 }
+void iv::decition::BrainDecition::UpdateUltra(const char *pdata, const int ndatasize)
+{
+    if(ndatasize<1)return;
+    iv::ultrasonic::ultraarea ultra_msg;
+    if(false == ultra_msg.ParseFromArray(pdata,ndatasize))
+    {
+        std::cout<<"Update UltraMsg Parse fail."<<std::endl;
+        return;
+    }
 
+    iv::ultrasonic::Area ultra_area;
+    for(int i=0;i<ultra_msg.area_size();i++)
+    {
+        ultra_area =ultra_msg.area(i);
+        if(ultra_area.valid()==true&&(ultra_area.id()>=1))
+        {
+            ServiceCarStatus.mbUltraDis[ultra_area.id()-1] = ultra_area.dist();
+        }
+    }
+
+
+}
 void iv::decition::BrainDecition::adjuseGpsLidarPosition(){
 
     ServiceCarStatus.msysparam.lidarGpsXiuzheng -=  ServiceCarStatus.msysparam.gpsOffset_y;

src/decition/decition_brain_sf_jsguide/decition/brain.h

@@ -47,6 +47,7 @@
 #include "vbox.pb.h"
 #include "carstate.pb.h"
 #include "groupmsg.pb.h"
+#include "ultraarea.pb.h"
 
 #include "fanyaapi.h"
 
@@ -141,6 +142,7 @@ namespace iv {
             void * mpaObsTraceLeft;
             void * mpaObsTraceRight;
             void * mpaCarstate;
+            void * mpUltraArea;
 
 
             void ShareDecition(iv::decition::Decition decition);
@@ -170,7 +172,7 @@ namespace iv {
             void UpdateVbox(const char * pdata,const int ndatasize);
             void GetFusion(const char* pdata, const int ndatasize);
             void UpdateFusion(std::shared_ptr<iv::fusion::fusionobjectarray> fusion_obs);
-
+            void UpdateUltra(const char * pdata,const int ndatasize);
         private:
             std::shared_ptr<iv::fusion::fusionobjectarray>  mfusion_obs_;
             iv::LidarGridPtr fusion_ptr_ = NULL;

src/decition/decition_brain_sf_jsguide/decition/decide_gps_00.cpp

@@ -2341,11 +2341,43 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }
 
 //均胜加速时速度逐渐增加
-    if(dSpeed-realspeed>1.0)
-    {
-        dSpeed=min(realspeed+0.5,dSpeed);
-    }
-
+//    if(dSpeed-realspeed>1.0)
+//    {
+//        dSpeed=min(realspeed+0.5,dSpeed);
+//    }
+    //超声波逻辑,begin
+     if(ServiceCarStatus.mbUltraDis[0]>=3.0&&ServiceCarStatus.mbUltraDis[0]<=5.0)
+     {
+        dSpeed=min(dSpeed,3.0);
+     }
+     else if(ServiceCarStatus.mbUltraDis[0]>=0.0&&ServiceCarStatus.mbUltraDis[0]<3.0)
+     {
+         minDecelerate=min(-1.0f,minDecelerate);
+     }
+
+     if((ServiceCarStatus.mbUltraDis[1]>=1.0&&ServiceCarStatus.mbUltraDis[1]<5.0)||
+        (ServiceCarStatus.mbUltraDis[3]>=1.0&&ServiceCarStatus.mbUltraDis[3]<5.0))
+     {
+        dSpeed=min(dSpeed,3.0);
+     }
+     else if((ServiceCarStatus.mbUltraDis[1]>=0.0&&ServiceCarStatus.mbUltraDis[1]<1.0)||
+             (ServiceCarStatus.mbUltraDis[3]>=0.0&&ServiceCarStatus.mbUltraDis[3]<1.0))
+     {
+         minDecelerate=min(-0.5f,minDecelerate);
+     }
+
+     if(ServiceCarStatus.daocheMode)
+     {
+         if(ServiceCarStatus.mbUltraDis[2]>=3.0&&ServiceCarStatus.mbUltraDis[2]<=5.0)
+         {
+            dSpeed=min(dSpeed,3.0);
+         }
+         else if(ServiceCarStatus.mbUltraDis[2]>=0.0&&ServiceCarStatus.mbUltraDis[2]<3.0)
+         {
+             minDecelerate=min(-1.0f,minDecelerate);
+         }
+     }
+  //超声波逻辑,end
     if(ServiceCarStatus.group_control){
         dSpeed = ServiceCarStatus.group_comm_speed;
     }

src/decition/decition_brain_sf_jsguide/decition_brain_sf_jsguide.pro

@@ -34,6 +34,7 @@ SOURCES += $$PWD/../common/main.cpp \
     ../../include/msgtype/fusionobject.pb.cc \
     ../../include/msgtype/carstate.pb.cc \
     ../../include/msgtype/groupmsg.pb.cc \
+   ../../include/msgtype/ultraarea.pb.cc \
     ../../include/msgtype/remotectrl.pb.cc
 
 
@@ -99,6 +100,7 @@ HEADERS += \
     ../common/common/sysparam_type.h \
     ../../include/msgtype/carstate.pb.h \
     ../../include/msgtype/groupmsg.pb.h \
+   ../../include/msgtype/ultraarea.pb.h \
     ../../include/msgtype/remotectrl.pb.h
 
 

src/detection/detection_lidar_PointPillars_MultiHead/.gitignore

@@ -1,73 +0,0 @@
-# This file is used to ignore files which are generated
-# ----------------------------------------------------------------------------
-
-*~
-*.autosave
-*.a
-*.core
-*.moc
-*.o
-*.obj
-*.orig
-*.rej
-*.so
-*.so.*
-*_pch.h.cpp
-*_resource.rc
-*.qm
-.#*
-*.*#
-core
-!core/
-tags
-.DS_Store
-.directory
-*.debug
-Makefile*
-*.prl
-*.app
-moc_*.cpp
-ui_*.h
-qrc_*.cpp
-Thumbs.db
-*.res
-*.rc
-/.qmake.cache
-/.qmake.stash
-
-# qtcreator generated files
-*.pro.user*
-
-# xemacs temporary files
-*.flc
-
-# Vim temporary files
-.*.swp
-
-# Visual Studio generated files
-*.ib_pdb_index
-*.idb
-*.ilk
-*.pdb
-*.sln
-*.suo
-*.vcproj
-*vcproj.*.*.user
-*.ncb
-*.sdf
-*.opensdf
-*.vcxproj
-*vcxproj.*
-
-# MinGW generated files
-*.Debug
-*.Release
-
-# Python byte code
-*.pyc
-
-# Binaries
-# --------
-*.dll
-*.exe
-

src/detection/detection_lidar_PointPillars_MultiHead/cfgs/cbgs_pp_multihead.yaml

@@ -1,5 +1,4 @@
-CLASS_NAMES: ['car','truck', 'construction_vehicle', 'bus', 'trailer',
-              'barrier', 'motorcycle', 'bicycle', 'pedestrian', 'traffic_cone']
+CLASS_NAMES: ['car','truck', 'bus','barrier', 'bicycle', 'pedestrian', 'traffic_cone']
 
 DATA_CONFIG:
     _BASE_CONFIG_: cfgs/dataset_configs/nuscenes_dataset.yaml
@@ -76,16 +75,6 @@ MODEL:
                 'matched_threshold': 0.55,
                 'unmatched_threshold': 0.4
             },
-            {
-                'class_name': construction_vehicle,
-                'anchor_sizes': [[6.37, 2.85, 3.19]],
-                'anchor_rotations': [0, 1.57],
-                'anchor_bottom_heights': [-0.225],
-                'align_center': False,
-                'feature_map_stride': 4,
-                'matched_threshold': 0.5,
-                'unmatched_threshold': 0.35
-            },
             {
                 'class_name': bus,
                 'anchor_sizes': [[10.5, 2.94, 3.47]],
@@ -96,16 +85,6 @@ MODEL:
                 'matched_threshold': 0.55,
                 'unmatched_threshold': 0.4
             },
-            {
-                'class_name': trailer,
-                'anchor_sizes': [[12.29, 2.90, 3.87]],
-                'anchor_rotations': [0, 1.57],
-                'anchor_bottom_heights': [0.115],
-                'align_center': False,
-                'feature_map_stride': 4,
-                'matched_threshold': 0.5,
-                'unmatched_threshold': 0.35
-            },
             {
                 'class_name': barrier,
                 'anchor_sizes': [[0.50, 2.53, 0.98]],
@@ -116,16 +95,6 @@ MODEL:
                 'matched_threshold': 0.55,
                 'unmatched_threshold': 0.4
             },
-            {
-                'class_name': motorcycle,
-                'anchor_sizes': [[2.11, 0.77, 1.47]],
-                'anchor_rotations': [0, 1.57],
-                'anchor_bottom_heights': [-1.085],
-                'align_center': False,
-                'feature_map_stride': 4,
-                'matched_threshold': 0.5,
-                'unmatched_threshold': 0.3
-            },
             {
                 'class_name': bicycle,
                 'anchor_sizes': [[1.70, 0.60, 1.28]],
@@ -165,16 +134,16 @@ MODEL:
                 'HEAD_CLS_NAME': ['car'],
             },
             {
-                'HEAD_CLS_NAME': ['truck', 'construction_vehicle'],
+                'HEAD_CLS_NAME': ['truck'],
             },
             {
-                'HEAD_CLS_NAME': ['bus', 'trailer'],
+                'HEAD_CLS_NAME': ['bus'],
             },
             {
                 'HEAD_CLS_NAME': ['barrier'],
             },
             {
-                'HEAD_CLS_NAME': ['motorcycle', 'bicycle'],
+                'HEAD_CLS_NAME': ['bicycle'],
             },
             {
                 'HEAD_CLS_NAME': ['pedestrian', 'traffic_cone'],

src/detection/detection_lidar_PointPillars_MultiHead/detection_lidar_PointPillars_MultiHead.pro

@@ -1,6 +1,6 @@
 QT -= gui
 
-CONFIG += c++11 console
+CONFIG += c++14 console
 CONFIG -= app_bundle
 
 QMAKE_CXXFLAGS += -std=gnu++17
@@ -11,7 +11,7 @@ QMAKE_LFLAGS += -no-pie  -Wl,--no-as-needed
 # depend on your compiler). Please consult the documentation of the
 # deprecated API in order to know how to port your code away from it.
 DEFINES += QT_DEPRECATED_WARNINGS
-
+#DEFINES += DEBUG_SHOW
 # You can also make your code fail to compile if you use deprecated APIs.
 # In order to do so, uncomment the following line.
 # You can also select to disable deprecated APIs only up to a certain version of Qt.
@@ -20,13 +20,18 @@ DEFINES += QT_DEPRECATED_WARNINGS
 SOURCES += main.cpp \
     pointpillars.cc \
     ../../include/msgtype/object.pb.cc \
-    ../../include/msgtype/objectarray.pb.cc
+    ../../include/msgtype/objectarray.pb.cc \
+    Tracker/Ctracker.cpp \
+    Tracker/HungarianAlg.cpp \
+    Tracker/Kalman.cpp \
+    Tracker/track.cpp
 
 DISTFILES += \
     nms.cu \
     postprocess.cu \
     preprocess.cu \
-    scatter.cu
+    scatter.cu \
+    roiaware_pool3d_kernel.cu
 
 HEADERS += \
     common.h \
@@ -36,14 +41,24 @@ HEADERS += \
     preprocess.h \
     scatter.h \
     ../../include/msgtype/object.pb.h \
-    ../../include/msgtype/objectarray.pb.h
-
+    ../../include/msgtype/objectarray.pb.h \
+    Tracker/Ctracker.h \
+    Tracker/defines.h \
+    Tracker/HungarianAlg.h \
+    Tracker/Kalman.h \
+    Tracker/ShortPathCalculator.h \
+    Tracker/track.h \
+    Tracker/Tracking.hpp \
+    roiaware_pool3d.h
+
+INCLUDEPATH+=Tracker
 
 CUDA_SOURCES +=  \
     nms.cu \
     postprocess.cu \
     preprocess.cu \
-    scatter.cu
+    scatter.cu \
+    roiaware_pool3d_kernel.cu
 
 CUDA_SDK = "/usr/local/cuda/"   # cudaSDK路径
 
@@ -164,3 +179,6 @@ unix:LIBS +=  -lpcl_common\
         -lpcl_surface\
         -lpcl_tracking\
         -lpcl_visualization
+
+INCLUDEPATH += /usr/include/opencv4/
+LIBS += /usr/lib/aarch64-linux-gnu/libopencv*.so

src/detection/detection_lidar_PointPillars_MultiHead/main.cpp

@@ -16,6 +16,10 @@
 #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;
@@ -29,33 +33,310 @@ 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);
-  }
+        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) {
-  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 * 5 + 0] = point.x;
-    out_points_array[i * 5 + 1] = point.y;
-    out_points_array[i * 5 + 2] = point.z;
-    out_points_array[i * 5 + 3] =
-        static_cast<float>(point.intensity / normalizing_factor);
-    out_points_array[i * 5 + 4] = 0;
-  }
+        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,
@@ -67,8 +348,7 @@ void GetLidarObj(std::vector<float> out_detections,std::vector<int> out_labels,
     for(i=0;i<obj_size;i++)
     {
         iv::lidar::lidarobject lidarobj;
-        if (out_scores.at(i) < 0.12) continue;
-        if (out_labels.at(i) == 5) continue;
+        if (out_scores.at(i) < 0.10) continue;
 
         lidarobj.set_tyaw(out_detections.at(i*7+6));
         iv::lidar::PointXYZ centroid;
@@ -117,16 +397,16 @@ void GetLidarObj(std::vector<float> out_detections,std::vector<int> out_labels,
         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.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));
-        ld.set_y(out_detections.at(i*7+4));
-        ld.set_z(out_detections.at(i*7+5));
+        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);
 
@@ -134,15 +414,16 @@ void GetLidarObj(std::vector<float> out_detections,std::vector<int> out_labels,
         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];
+    //   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;
+    int in_num_points = pc_ptr->width;
 
     std::vector<float> out_detections;
     std::vector<int> out_labels;
@@ -151,34 +432,146 @@ void DectectOnePCD(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr)
     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();
-    int BoxFeature = 7;
-    int num_objects = out_detections.size() / BoxFeature;
+    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;
 
-//    std::vector<iv::lidar::lidarobject> lidarobjvec;
+
+//    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(out_detections,out_labels,out_scores,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());
+    //    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;
+    //    std::cout<<" is  ok  ------------  "<<std::endl;
     if(nSize <=16)return;
     unsigned int * pHeadSize = (unsigned int *)strdata;
     if(*pHeadSize > nSize)
@@ -210,9 +603,9 @@ void ListenPointCloud(const char *strdata,const unsigned int nSize,const unsigne
     {
         pcl::PointXYZI xp;
         memcpy(&xp,p,sizeof(pcl::PointXYZI));
-        xp.z = xp.z;
+        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);
-        p++;
     }
 
     DectectOnePCD(point_cloud);
@@ -272,10 +665,7 @@ void exitfunc()
     iv::modulecomm::Unregister(gpdetect);
     std::cout<<"exit func complete"<<std::endl;
 }
-
-#include <QDir>
 #include <QFile>
-
 bool trtisexist(std::string strpfe,std::string strbackbone)
 {
     QFile xFile;
@@ -291,13 +681,12 @@ bool trtisexist(std::string strpfe,std::string strbackbone)
     }
     return true;
 }
-
 int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
 
-//    RegisterIVBackTrace();
-
+    //    RegisterIVBackTrace();
+    tracker.setSettings(settings);
     gfault = new iv::Ivfault("lidar_pointpillar");
     givlog = new iv::Ivlog("lidar_pointpillar");
 
@@ -306,13 +695,11 @@ int main(int argc, char *argv[])
     char * strhome = getenv("HOME");
     std::string pfe_file = strhome;
     pfe_file += "/models/lidar/cbgs_pp_multihead_pfe.onnx";
-    std::string pfe_trt_file = strhome;
-    pfe_trt_file += "/models/lidar/cbgs_pp_multihead_pfe.trt";
+    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 = strhome;
-    backbone_trt_file += "/models/lidar/cbgs_pp_multihead_backbone.trt";
+    std::string backbone_trt_file = backbone_file.substr(0, backbone_file.find(".")) + ".trt";
 
     bool btrtexist = trtisexist(pfe_trt_file,backbone_trt_file);
 
@@ -332,33 +719,40 @@ int main(int argc, char *argv[])
     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
-    );
+        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 engine mode."<<std::endl;
-    pPillars = new PointPillars(
-      0.15,
-      0.10,
-      false,
-      pfe_trt_file,
-      backbone_trt_file,
-      pp_config
-    );
+        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.0418, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转 yaw   +
+    Eigen::AngleAxisd r_y ( -0.0242, Eigen::Vector3d ( 0,1,0 ) ); //沿 Y 轴旋转 roll  +
+    Eigen::AngleAxisd r_x ( -0.0137, 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.25, 0.72;
     gpa = iv::modulecomm::RegisterRecv(gstrinput.data(),ListenPointCloud);
     gpdetect = iv::modulecomm::RegisterSend(gstroutput.data(), 10000000,1);
     gpthread = new std::thread(statethread);

src/detection/detection_lidar_PointPillars_MultiHead/pointpillars.cc

@@ -224,13 +224,28 @@ void PointPillars::DeviceMemoryMalloc() {
 
     GPU_CHECK(cudaMalloc(&rpn_buffers_[0],  kRpnInputSize * sizeof(float)));
 
-    GPU_CHECK(cudaMalloc(&rpn_buffers_[1],  kNumAnchorPerCls  * sizeof(float)));  //classes
-    GPU_CHECK(cudaMalloc(&rpn_buffers_[2],  kNumAnchorPerCls  * 2 * 2 * sizeof(float)));
-    GPU_CHECK(cudaMalloc(&rpn_buffers_[3],  kNumAnchorPerCls  * 2 * 2 * sizeof(float)));
-    GPU_CHECK(cudaMalloc(&rpn_buffers_[4],  kNumAnchorPerCls  * sizeof(float)));
-    GPU_CHECK(cudaMalloc(&rpn_buffers_[5],  kNumAnchorPerCls  * 2 * 2 * sizeof(float)));
-    GPU_CHECK(cudaMalloc(&rpn_buffers_[6],  kNumAnchorPerCls  * 2 * 2 * sizeof(float)));
-    
+
+    if(kNumClass == 10)
+    {
+////class = 10
+        GPU_CHECK(cudaMalloc(&rpn_buffers_[1],  kNumAnchorPerCls  * sizeof(float)));  //classes
+        GPU_CHECK(cudaMalloc(&rpn_buffers_[2],  kNumAnchorPerCls  * 2 * 2 * sizeof(float)));
+        GPU_CHECK(cudaMalloc(&rpn_buffers_[3],  kNumAnchorPerCls  * 2 * 2 * sizeof(float)));
+        GPU_CHECK(cudaMalloc(&rpn_buffers_[4],  kNumAnchorPerCls  * sizeof(float)));
+        GPU_CHECK(cudaMalloc(&rpn_buffers_[5],  kNumAnchorPerCls  * 2 * 2 * sizeof(float)));
+        GPU_CHECK(cudaMalloc(&rpn_buffers_[6],  kNumAnchorPerCls  * 2 * 2 * sizeof(float)));
+    }
+    else
+    {
+////class = 7
+        GPU_CHECK(cudaMalloc(&rpn_buffers_[1],  kNumAnchorPerCls  * sizeof(float)));  //classes
+        GPU_CHECK(cudaMalloc(&rpn_buffers_[2],  kNumAnchorPerCls  * sizeof(float)));
+        GPU_CHECK(cudaMalloc(&rpn_buffers_[3],  kNumAnchorPerCls  * sizeof(float)));
+        GPU_CHECK(cudaMalloc(&rpn_buffers_[4],  kNumAnchorPerCls  * sizeof(float)));
+        GPU_CHECK(cudaMalloc(&rpn_buffers_[5],  kNumAnchorPerCls  * sizeof(float)));
+        GPU_CHECK(cudaMalloc(&rpn_buffers_[6],  kNumAnchorPerCls  * 2 * 2 * sizeof(float)));
+    }
+
     GPU_CHECK(cudaMalloc(&rpn_buffers_[7],  kNumAnchorPerCls * kNumClass * kNumOutputBoxFeature * sizeof(float))); //boxes
 
     // for scatter kernel
@@ -283,33 +298,13 @@ void PointPillars::SetDeviceMemoryToZero() {
     GPU_CHECK(cudaMemset(dev_filter_count_,     0, kNumClass * sizeof(int)));
 }
 
-
-#include <fstream>
-#include <iostream>
-#
-
-void PointPillars::SaveEngine(nvinfer1::ICudaEngine* pengine,std::string strpath)
-{
-    nvinfer1::IHostMemory* data = pengine->serialize();
-    std::ofstream file;
-    file.open(strpath,std::ios::binary | std::ios::out);
-    if(!file.is_open())
-    {
-        std::cout << "read create engine file" << strpath <<" failed" << std::endl;
-        return;
-    }
-
-    file.write((const char*)data->data(), data->size());
-    file.close();
-}
-
 void PointPillars::InitTRT(const bool use_onnx) {
   if (use_onnx_) {
     // create a TensorRT model from the onnx model and load it into an engine
     OnnxToTRTModel(pfe_file_, &pfe_engine_);
-    SaveEngine(pfe_engine_,"/home/nvidia/models/lidar/cbgs_pp_multihead_pfe.trt");
+    SaveEngine(pfe_engine_, pfe_file_.substr(0, pfe_file_.find(".")) + ".trt");
     OnnxToTRTModel(backbone_file_, &backbone_engine_);
-    SaveEngine(backbone_engine_,"/home/nvidia/models/lidar/cbgs_pp_multihead_backbone.trt");
+    SaveEngine(backbone_engine_, backbone_file_.substr(0, backbone_file_.find(".")) + ".trt");
   }else {
     EngineToTRTModel(pfe_file_, &pfe_engine_);
     EngineToTRTModel(backbone_file_, &backbone_engine_);
@@ -363,58 +358,19 @@ void PointPillars::OnnxToTRTModel(
     builder->destroy();
 }
 
-void PointPillars::LoadEngineModel(const string &engine_file, nvinfer1::ICudaEngine **engine_ptr)
+void PointPillars::SaveEngine(const nvinfer1::ICudaEngine* engine, const std::string& engine_filepath)
 {
-//    using namespace std;
-//    fstream file;
-
-//    file.open(engine_file,ios::binary | ios::in);
-//    if(!file.is_open())
-//    {
-//        cout << "read engine file" << engine_file <<" failed" << endl;
-//        return;
-//    }
-//    file.seekg(0, ios::end);
-//    int length = file.tellg();
-//    file.seekg(0, ios::beg);
-//    std::unique_ptr<char[]> data(new char[length]);
-//    file.read(data.get(), length);
-
-//    file.close();
-
-//    std::cout << "deserializing" << std::endl;
-//    mTrtRunTime = createInferRuntime(gLogger);
-//    assert(mTrtRunTime != nullptr);
-//    mTrtEngine= mTrtRunTime->deserializeCudaEngine(data.get(), length, &mTrtPluginFactory);
-//    assert(mTrtEngine != nullptr);
-
-//    nvinfer1::IRuntime* runtime = nvinfer1::createInferRuntime(g_logger_);
-
-//    if (runtime == nullptr) {
-//        std::string msg("failed to build runtime parser");
-//        g_logger_.log(nvinfer1::ILogger::Severity::kERROR, msg.c_str());
-//        exit(EXIT_FAILURE);
-//    }
-
-
-
-//    std::cout << "                                                                  "<< std::endl;
-//    std::cout << "------------------------------------------------------------------"<< std::endl;
-//    std::cout << ">>>>                                                          >>>>"<< std::endl;
-//    std::cout << "                                                                  "<< std::endl;
-//    std::cout << "Input filename:   " << engine_file << std::endl;
-//    std::cout << "                                                                  "<< std::endl;
-//    std::cout << ">>>>                                                          >>>>"<< std::endl;
-//    std::cout << "------------------------------------------------------------------"<< std::endl;
-//    std::cout << "                                                                  "<< std::endl;
-
-//    nvinfer1::ICudaEngine* engine = runtime->deserializeCudaEngine(modelMem, modelSize, NULL);
-//    if (engine == nullptr) {
-//        std::string msg("failed to build engine parser");
-//        g_logger_.log(nvinfer1::ILogger::Severity::kERROR, msg.c_str());
-//        exit(EXIT_FAILURE);
-//    }
-//    *engine_ptr = engine;
+    // serialize the engine, then close everything down
+    nvinfer1::IHostMemory& trtModelStream = *(engine->serialize());
+    std::ofstream file;
+    file.open(engine_filepath, std::ios::binary | std::ios::out);
+    if(!file.is_open())
+    {
+        std::cout << "read create engine file" << engine_filepath <<" failed" << std::endl;
+        return;
+    }
+    file.write((const char*)trtModelStream.data(), std::streamsize(trtModelStream.size()));
+    file.close();
 }
 
 void PointPillars::EngineToTRTModel(
@@ -561,8 +517,6 @@ 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/pointpillars.h

@@ -225,6 +225,7 @@ class PointPillars {
      * @details Called in the constructor
      */
     void InitTRT(const bool use_onnx);
+    void SaveEngine(const nvinfer1::ICudaEngine* engine, const std::string& engine_filepath);
     /**
      * @brief Convert ONNX to TensorRT model
      * @param[in] model_file ONNX model file path
@@ -243,9 +244,6 @@ class PointPillars {
     void EngineToTRTModel(const std::string &engine_file ,     
                         nvinfer1::ICudaEngine** engine_ptr) ;
 
-    void LoadEngineModel(const std::string &engine_file ,
-                         nvinfer1::ICudaEngine** engine_ptr) ;
-
     /**
      * @brief Preproces points
      * @param[in] in_points_array Point cloud array
@@ -272,8 +270,6 @@ class PointPillars {
                 const std::string pp_config);
     ~PointPillars();
 
-    void SaveEngine(nvinfer1::ICudaEngine* pengine,std::string strpath);
-
     /**
      * @brief Call PointPillars for the inference
      * @param[in] in_points_array Point cloud array

src/detection/detection_lidar_PointPillars_MultiHead/postprocess.cu

@@ -143,6 +143,7 @@ __global__ void sigmoid_filter_kernel(
     if( cls_score[ threadIdx.x ] > score_threshold) 
     {
         int counter = atomicAdd(&filter_count[blockIdx.z], 1);
+        //printf("counter : %d \n" , counter);
         if ( blockIdx.z == 0) {
             box_decode_warp(0 ,box_pred_0 , tid , num_anchors_per_head , counter , filtered_box);
             filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
@@ -188,38 +189,139 @@ __global__ void sigmoid_filter_kernel(
     __syncthreads();  
     if( cls_score[ threadIdx.x + blockDim.x ] > score_threshold)  {     
             int counter = atomicAdd(&filter_count[blockIdx.z], 1);
-            // printf("counter : %d \n" , counter);
+            //printf("counter : %d \n" , counter);
             if (blockIdx.z == 1) {
                 box_decode_warp(0 ,box_pred_2 , tid , num_anchors_per_head , counter , filtered_box);
-                filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+                filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x + blockDim.x];
             }else 
             if (blockIdx.z == 2) {
                 box_decode_warp(0 ,box_pred_2 , tid , num_anchors_per_head , counter , filtered_box);
-                filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+                filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x + blockDim.x];
             }else 
             if (blockIdx.z == 3) {
                 box_decode_warp(0 ,box_pred_4 , tid , num_anchors_per_head , counter , filtered_box);
-                filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+                filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x + blockDim.x];
             }else 
             if (blockIdx.z == 4) {
                 box_decode_warp(0 ,box_pred_4 , tid , num_anchors_per_head , counter , filtered_box);
-                filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+                filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x + blockDim.x];
             }else 
             if (blockIdx.z == 6) {
                 box_decode_warp(0 ,box_pred_7 , tid , num_anchors_per_head , counter , filtered_box);
-                filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+                filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x + blockDim.x];
             }else 
             if (blockIdx.z == 7) {
                 box_decode_warp(0 ,box_pred_7 , tid , num_anchors_per_head , counter , filtered_box);
-                filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+                filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x + blockDim.x];
             }else 
             if (blockIdx.z == 8) {
                 box_decode_warp(0 ,box_pred_9 , tid , num_anchors_per_head , counter , filtered_box);
-                filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+                filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x + blockDim.x];
             }else 
             if (blockIdx.z == 9) {
                 box_decode_warp(0 ,box_pred_9 , tid , num_anchors_per_head , counter , filtered_box);
-                filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+                filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x + blockDim.x];
+            }
+    }
+}
+
+__global__ void sigmoid_filter_kernel(
+
+    float* cls_pred_0,
+    float* cls_pred_1,
+    float* cls_pred_2,
+    float* cls_pred_3,
+    float* cls_pred_4,
+    float* cls_pred_56,
+
+    const float* box_pred_0,
+
+    const float* box_pred_1,
+
+    const float* box_pred_2,
+
+    const float* box_pred_3,
+
+    const float* box_pred_4,
+
+    const float* box_pred_5,
+    const float* box_pred_6,
+
+
+    float* filtered_box,
+    float* filtered_score,
+    int* filter_count,
+
+    const float score_threshold) {
+
+    // cls_pred_34
+    // 32768*2 , 2
+
+    int num_anchors_per_head = gridDim.x * gridDim.y * blockDim.x;
+    // 16 * 4 * 512 = 32768
+    extern __shared__ float cls_score[];
+    cls_score[threadIdx.x + blockDim.x] = -1.0f;
+
+    int tid = blockIdx.x * gridDim.y * blockDim.x + blockIdx.y *  blockDim.x + threadIdx.x;
+
+
+    if ( blockIdx.z == 0) cls_score[ threadIdx.x ] = 1 / (1 + expf(-cls_pred_0[ tid ]));
+    if ( blockIdx.z == 1) cls_score[ threadIdx.x ] = 1 / (1 + expf(-cls_pred_1[ tid ]));
+    if ( blockIdx.z == 2) cls_score[ threadIdx.x ] = 1 / (1 + expf(-cls_pred_2[ tid ]));
+    if ( blockIdx.z == 3) cls_score[ threadIdx.x ] = 1 / (1 + expf(-cls_pred_3[ tid ]));
+    if ( blockIdx.z == 4) cls_score[ threadIdx.x ] = 1 / (1 + expf(-cls_pred_4[ tid ]));
+    if ( blockIdx.z == 5) {
+        cls_score[ threadIdx.x ] = 1 / (1 + expf(-cls_pred_56[ tid * 2 ]));
+        cls_score[ threadIdx.x + blockDim.x] = 1 / (1 + expf(-cls_pred_56[ (num_anchors_per_head + tid) * 2]));}
+    if ( blockIdx.z == 6) {
+        cls_score[ threadIdx.x ] = 1 / (1 + expf(-cls_pred_56[ tid * 2 + 1 ]));
+        cls_score[ threadIdx.x + blockDim.x] = 1 / (1 + expf(-cls_pred_56[ (num_anchors_per_head + tid) * 2 + 1]));}
+
+    __syncthreads();
+
+    if( cls_score[ threadIdx.x ] > score_threshold)
+    {
+        int counter = atomicAdd(&filter_count[blockIdx.z], 1);
+        if ( blockIdx.z == 0) {
+            box_decode_warp(0 ,box_pred_0 , tid , num_anchors_per_head , counter , filtered_box);
+            filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+        }else
+        if ( blockIdx.z == 1) {
+            box_decode_warp(0 ,box_pred_1 , tid , num_anchors_per_head , counter , filtered_box);
+            filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+        }else
+        if ( blockIdx.z == 2) {
+            box_decode_warp(0 ,box_pred_2 , tid , num_anchors_per_head , counter , filtered_box);
+            filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+        }else
+        if ( blockIdx.z == 3) {
+            box_decode_warp(0 ,box_pred_3 , tid , num_anchors_per_head , counter , filtered_box);
+            filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+        }else
+        if (blockIdx.z == 4) {
+            box_decode_warp(0 ,box_pred_4 , tid , num_anchors_per_head , counter , filtered_box);
+            filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+        }else
+        if ( blockIdx.z == 5) {
+            box_decode_warp(0 ,box_pred_5 , tid , num_anchors_per_head , counter , filtered_box);
+            filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+        }else
+        if ( blockIdx.z == 6) {
+            box_decode_warp(0 ,box_pred_5 , tid , num_anchors_per_head , counter , filtered_box);
+            filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x ];
+        }
+    }
+    __syncthreads();
+    if( cls_score[ threadIdx.x + blockDim.x ] > score_threshold)  {
+            int counter = atomicAdd(&filter_count[blockIdx.z], 1);
+            // printf("counter : %d \n" , counter);
+            if (blockIdx.z == 5) {
+                box_decode_warp(0 ,box_pred_6 , tid , num_anchors_per_head , counter , filtered_box);
+                filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x + blockDim.x];
+            }else
+            if (blockIdx.z == 6) {
+                box_decode_warp(0 ,box_pred_6 , tid , num_anchors_per_head , counter , filtered_box);
+                filtered_score[blockIdx.z * num_anchors_per_head + counter] = cls_score[ threadIdx.x + blockDim.x];
             }
     }
 }
@@ -290,31 +392,62 @@ void PostprocessCuda::DoPostprocessCuda(
     std::vector<float>& out_detection, std::vector<int>& out_label , std::vector<float>& out_score) {
     // 在此之前，先进行rpn_box_output的concat. 
     // 128x128 的feature map， cls_pred 的shape为（32768，1），（32768,1），（32768,1），（65536,2），（32768，1）
-    dim3 gridsize(16, 4 , 10);  //16 *  4  * 512  = 32768 代表一个head的anchors
-    sigmoid_filter_kernel<<< gridsize, 512 , 512 * 2 * sizeof(float)>>>(
-        cls_pred_0,
-        cls_pred_12, 
-        cls_pred_34, 
-        cls_pred_5, 
-        cls_pred_67, 
-        cls_pred_89,
-
-        &box_preds[0 * 32768 * 9],
-        &box_preds[1 * 32768 * 9],
-        &box_preds[2 * 32768 * 9],
-        &box_preds[3 * 32768 * 9],
-        &box_preds[4 * 32768 * 9],
-        &box_preds[5 * 32768 * 9],
-        &box_preds[6 * 32768 * 9],
-        &box_preds[7 * 32768 * 9],
-        &box_preds[8 * 32768 * 9],
-        &box_preds[9 * 32768 * 9],
-
-        dev_filtered_box, 
-        dev_filtered_score,  
-        dev_filter_count, 
-    
-        score_threshold_);
+
+//////class = 10
+    if(num_class_ == 10)
+    {
+        dim3 gridsize(16, 4 , 10);  //16 *  4  * 512  = 32768 代表一个head的anchors
+        sigmoid_filter_kernel<<< gridsize, 512 , 512 * 2 * sizeof(float)>>>(
+            cls_pred_0,
+            cls_pred_12,
+            cls_pred_34,
+            cls_pred_5,
+            cls_pred_67,
+            cls_pred_89,
+
+            &box_preds[0 * 32768 * 9],
+            &box_preds[1 * 32768 * 9],
+            &box_preds[2 * 32768 * 9],
+            &box_preds[3 * 32768 * 9],
+            &box_preds[4 * 32768 * 9],
+            &box_preds[5 * 32768 * 9],
+            &box_preds[6 * 32768 * 9],
+            &box_preds[7 * 32768 * 9],
+            &box_preds[8 * 32768 * 9],
+            &box_preds[9 * 32768 * 9],
+
+            dev_filtered_box,
+            dev_filtered_score,
+            dev_filter_count,
+
+            score_threshold_);
+    }
+    else
+    {
+        ////class = 7
+            dim3 gridsize(16, 4 , 7);  //16 *  4  * 512  = 32768 代表一个head的anchors
+            sigmoid_filter_kernel<<< gridsize, 512 , 512 * 2 * sizeof(float)>>>(
+                 cls_pred_0,
+                 cls_pred_12,
+                 cls_pred_34,
+                 cls_pred_5,
+                 cls_pred_67,
+                 cls_pred_89,
+                 &box_preds[0 * 32768 * 9],
+                 &box_preds[1 * 32768 * 9],
+                 &box_preds[2 * 32768 * 9],
+                 &box_preds[3 * 32768 * 9],
+                 &box_preds[4 * 32768 * 9],
+                 &box_preds[5 * 32768 * 9],
+                 &box_preds[6 * 32768 * 9],
+
+                 dev_filtered_box,
+                 dev_filtered_score,
+                 dev_filter_count,
+
+                 score_threshold_);
+    }
+
     cudaDeviceSynchronize();
     
     int host_filter_count[num_class_] = {0};
@@ -362,17 +495,16 @@ void PostprocessCuda::DoPostprocessCuda(
 
         cudaMemcpy(host_filtered_box, dev_sorted_filtered_box, host_filter_count[i] * num_output_box_feature_ * sizeof(float), cudaMemcpyDeviceToHost);
         cudaMemcpy(host_filtered_scores, dev_sorted_filtered_scores, host_filter_count[i] * sizeof(float), cudaMemcpyDeviceToHost);
-        for (int i = 0; i < num_out; ++i)  {
-            out_detection.emplace_back(host_filtered_box[keep_inds[i] * num_output_box_feature_ + 0]);
-            out_detection.emplace_back(host_filtered_box[keep_inds[i] * num_output_box_feature_ + 1]);
-            out_detection.emplace_back(host_filtered_box[keep_inds[i] * num_output_box_feature_ + 2]);
-            out_detection.emplace_back(host_filtered_box[keep_inds[i] * num_output_box_feature_ + 3]);
-            out_detection.emplace_back(host_filtered_box[keep_inds[i] * num_output_box_feature_ + 4]);
-            out_detection.emplace_back(host_filtered_box[keep_inds[i] * num_output_box_feature_ + 5]);
-            out_detection.emplace_back(host_filtered_box[keep_inds[i] * num_output_box_feature_ + 6]);
-            out_score.emplace_back(host_filtered_scores[keep_inds[i]]);
+        for (int j = 0; j < num_out; ++j)  {
+            out_detection.emplace_back(host_filtered_box[keep_inds[j] * num_output_box_feature_ + 0]);
+            out_detection.emplace_back(host_filtered_box[keep_inds[j] * num_output_box_feature_ + 1]);
+            out_detection.emplace_back(host_filtered_box[keep_inds[j] * num_output_box_feature_ + 2]);
+            out_detection.emplace_back(host_filtered_box[keep_inds[j] * num_output_box_feature_ + 3]);
+            out_detection.emplace_back(host_filtered_box[keep_inds[j] * num_output_box_feature_ + 4]);
+            out_detection.emplace_back(host_filtered_box[keep_inds[j] * num_output_box_feature_ + 5]);
+            out_detection.emplace_back(host_filtered_box[keep_inds[j] * num_output_box_feature_ + 6]);
+            out_score.emplace_back(host_filtered_scores[keep_inds[j]]);
             out_label.emplace_back(i);
-
         }
         delete[] keep_inds;
         delete[] host_filtered_scores;
@@ -380,6 +512,5 @@ void PostprocessCuda::DoPostprocessCuda(
 
         GPU_CHECK(cudaFree(dev_indexes));
         GPU_CHECK(cudaFree(dev_sorted_filtered_box));
-        GPU_CHECK(cudaFree(dev_sorted_filtered_scores));
     }
 }

src/detection/detection_lidar_ukf_pda/imm_ukf_pda.cpp

@@ -543,7 +543,8 @@ void ImmUkfPda::staticClassification()
   for (size_t i = 0; i < targets_.size(); i++)
   {
     // targets_[i].x_merge_(2) is referred for estimated velocity
-    double current_velocity = std::abs(targets_[i].x_merge_(2));
+//    double current_velocity = std::abs(targets_[i].x_merge_(2));
+    double current_velocity = targets_[i].x_merge_(2);
     targets_[i].vel_history_.push_back(current_velocity);
     if (targets_[i].tracking_num_ == TrackingState::Stable && targets_[i].lifetime_ > life_time_thres_)
     {
@@ -696,7 +697,7 @@ void ImmUkfPda::makeOutput(const iv::lidar::objectarray & input,
 
 //    }
 
-    std::cout<<"          v     tyaw :      "<<tv<<"  "   <<tyaw<<std::endl;
+//    std::cout<<"          v     tyaw :      "<<tv<<"  "   <<tyaw<<std::endl;
 
     while (tyaw > M_PI)
       tyaw -= 2. * M_PI;
@@ -711,6 +712,8 @@ void ImmUkfPda::makeOutput(const iv::lidar::objectarray & input,
     dd.set_id(targets_[i].ukf_id_);
 //    dd.id = targets_[i].ukf_id_;
     dd.set_velocity_linear_x(tv);
+//    std::cout<<" -------------------------------------------------------"<<std::endl;
+//    std::cout<<"   vel    linear    "<<tv<<std::endl;
    // dd.velocity_linear_x = tv;
     dd.set_acceleration_linear_y(tyaw_rate);
    // dd.acceleration_linear_y = tyaw_rate;
@@ -901,7 +904,7 @@ void ImmUkfPda::tracker(const iv::lidar::objectarray & input,
       for(j=0;j<detected_objects_output.obj_size();j++)
       {
       iv::lidar::lidarobject obj = detected_objects_output.obj(j);
-      std::cout<<"obj "<<j<< " vel is "<<obj.velocity_linear_x()<<" id is "<<obj.id()<<std::endl;
+//      std::cout<<"obj "<<j<< " vel is "<<obj.velocity_linear_x()<<" id is "<<obj.id()<<std::endl;
 
       }
   }

src/driver/driver_ultrasonic_dauxi_KS136A/canrecv_consumer.cpp

@@ -5,7 +5,7 @@
 extern setupConfig_t setupConfig;
 extern iv::msgunit shmSonar;
 
-//#define RESULT_OUTPUT_ENABLE
+#define RESULT_OUTPUT_ENABLE
 
 CANRecv_Consumer::CANRecv_Consumer(CAN_Producer_Consumer *pBuf)
 {
@@ -151,6 +151,12 @@ void CANRecv_Consumer::run()
 //                        std::cout<<"finish decode sensor "<<(int)decodeSensorID<<" result:"<<QDateTime::currentMSecsSinceEpoch()<<std::endl;
                         decodeTimer.restart();
                     }
+#ifdef RESULT_OUTPUT_ENABLE
+                    else if(tempDist == 0xEEEE)
+                    {
+                        std::cout<<"sensor: "<<(int)(decodeSensorID + 1)<<" may be disturbed."<<std::endl;
+                    }
+#endif
                 }
             }
         }
@@ -191,16 +197,16 @@ void CANRecv_Consumer::UltrasonicData_Ready_Slot(void)
     xmsg.set_sigobjdist_rlside(objDist_Send[11]);
 
     xmsg.set_sigsensor_front_ls(sensorStatus_Send[0]);
-    xmsg.set_sigsensor_front_l(sensorStatus_Send[1]);
+    xmsg.set_sigsensor_front_lc(sensorStatus_Send[1]);
     xmsg.set_sigsensor_front_lm(sensorStatus_Send[2]);
     xmsg.set_sigsensor_front_rm(sensorStatus_Send[3]);
-    xmsg.set_sigsensor_front_r(sensorStatus_Send[4]);
+    xmsg.set_sigsensor_front_rc(sensorStatus_Send[4]);
     xmsg.set_sigsensor_front_rs(sensorStatus_Send[5]);
     xmsg.set_sigsensor_rear_rs(sensorStatus_Send[6]);
-    xmsg.set_sigsensor_rear_r(sensorStatus_Send[7]);
+    xmsg.set_sigsensor_rear_rc(sensorStatus_Send[7]);
     xmsg.set_sigsensor_rear_rm(sensorStatus_Send[8]);
     xmsg.set_sigsensor_rear_lm(sensorStatus_Send[9]);
-    xmsg.set_sigsensor_rear_l(sensorStatus_Send[10]);
+    xmsg.set_sigsensor_rear_lc(sensorStatus_Send[10]);
     xmsg.set_sigsensor_rear_ls(sensorStatus_Send[11]);
 #ifdef RESULT_OUTPUT_ENABLE
     for(int i=0;i<12;i++)

src/driver/driver_ultrasonic_dauxi_KS136A/cansend_producer.cpp

@@ -78,5 +78,5 @@ void CANSend_Producer::Enable_Ask_Slot(bool enableFlag , uint8_t sensorID, uint8
 {
     askEnableFlag = enableFlag;
     askSensorID = sensorID;
-    askDistanceType = distanceType;
+    askDistanceType = 0; //distanceType;
 }

src/driver/driver_ultrasonic_dauxi_KS136A/driver_ultrasonic_dauxi_KS136A.pro

@@ -9,7 +9,7 @@ CONFIG -= app_bundle
 # depend on your compiler). Please consult the documentation of the
 # deprecated API in order to know how to port your code away from it.
 DEFINES += QT_DEPRECATED_WARNINGS
-
+DEFINES += RESULT_OUTPUT_ENABLE
 # You can also make your code fail to compile if it uses deprecated APIs.
 # In order to do so, uncomment the following line.
 # You can also select to disable deprecated APIs only up to a certain version of Qt.

src/fusion/lidar_radar_fusion_cnn/Tracker/Ctracker.cpp

@@ -113,7 +113,7 @@ void CTracker::UpdateTrackingState(
             if (assignment[i] != -1)
             {
 #ifdef DEBUG_SHOW
-                std::cout<<costMatrix[i + assignment[i] * N]<<", ";
+//                std::cout<<costMatrix[i + assignment[i] * N]<<", ";
 #endif
                 if (costMatrix[i + assignment[i] * N] > m_settings.m_distThres)
                 {
@@ -124,14 +124,14 @@ void CTracker::UpdateTrackingState(
             else
             {
 #ifdef DEBUG_SHOW
-                std::cout<<-1<<", ";
+//                std::cout<<-1<<", ";
 #endif
                 // If track have no assigned detect, then increment skipped frames counter.
                 m_tracks[i]->SkippedFrames()++;
             }
         }
 #ifdef DEBUG_SHOW
-                std::cout<<std::endl;
+//                std::cout<<std::endl;
 #endif
         // If track didn't get detects long time, remove it.
         for (size_t i = 0; i < m_tracks.size();)

src/fusion/lidar_radar_fusion_cnn/Tracker/Tracking.hpp

@@ -37,7 +37,7 @@ bool InitTracker(CTracker& tracker)
 iv::fusion::fusionobjectarray Tracking(iv::fusion::fusionobjectarray& fusionobjvec, CTracker& tracker)
 {
 #ifdef DEBUG_SHOW
-    std::cout<<"-------------------------------------------------"<<std::endl;
+//    std::cout<<"-------------------------------------------------"<<std::endl;
 #endif
     iv::fusion::fusionobjectarray trackedobjvec;
     trackedobjvec.clear_obj();
@@ -58,15 +58,15 @@ iv::fusion::fusionobjectarray Tracking(iv::fusion::fusionobjectarray& fusionobjv
         CRegion region = CRegion(rect,fusionobjvec.obj(i).type(),fusionobjvec.obj(i).prob());
         regions.push_back(region);
 #ifdef DEBUG_SHOW
-        std::cout<<"old id:"<<i<<std::endl;
-        std::cout<<"old type:"<<fusionobjvec.obj(i).type()<<std::endl;
-        std::cout<<"old x,y,z,w,h,l,yaw:"<<rect.center.x<<","<<rect.center.y<<","<<rect.center.z<<"  "<<rect.size.width<<","<<rect.size.height<<","<<rect.size.length<<"  "<<rect.yaw<<std::endl;
+//        std::cout<<"old id:"<<i<<std::endl;
+//        std::cout<<"old type:"<<fusionobjvec.obj(i).type()<<std::endl;
+//        std::cout<<"old x,y,z,w,h,l,yaw:"<<rect.center.x<<","<<rect.center.y<<","<<rect.center.z<<"  "<<rect.size.width<<","<<rect.size.height<<","<<rect.size.length<<"  "<<rect.yaw<<std::endl;
 #endif
     }
     tracker.Update(regions, cv::UMat(), 30);
     auto tracks = tracker.GetTracks();
 #ifdef DEBUG_SHOW
-    std::cout<<"fusion size, tracker size:"<<regions.size()<<","<<tracks.size()<<std::endl;
+//    std::cout<<"fusion size, tracker size:"<<regions.size()<<","<<tracks.size()<<std::endl;
 #endif
     for (size_t i = 0; i < tracks.size(); i++)
     {
@@ -159,13 +159,13 @@ iv::fusion::fusionobjectarray Tracking(iv::fusion::fusionobjectarray& fusionobjv
             pe->CopyFrom(fusion_object);
         }
 #ifdef DEBUG_SHOW
-        std::cout<<"id:"<<fusion_object.id()<<"  "<<obj_id<<std::endl;
-        std::cout<<"type:"<<fusion_object.type()<<std::endl;
-        std::cout<<"update x,y,z,w,h,l,yaw,vx,vy:"<<fusion_object.centroid().x()<<","<<fusion_object.centroid().y()<<","<<fusion_object.centroid().z()<<"  "<<fusion_object.dimensions().x()<<","<<fusion_object.dimensions().y()<<","<<fusion_object.dimensions().z()<<"  "<<fusion_object.yaw()<<"  "<<fusion_object.vel_relative().x()<<","<<fusion_object.vel_relative().y()<<std::endl;
+//        std::cout<<"id:"<<fusion_object.id()<<"  "<<obj_id<<std::endl;
+//        std::cout<<"type:"<<fusion_object.type()<<std::endl;
+//        std::cout<<"update x,y,z,w,h,l,yaw,vx,vy:"<<fusion_object.centroid().x()<<","<<fusion_object.centroid().y()<<","<<fusion_object.centroid().z()<<"  "<<fusion_object.dimensions().x()<<","<<fusion_object.dimensions().y()<<","<<fusion_object.dimensions().z()<<"  "<<fusion_object.yaw()<<"  "<<fusion_object.vel_relative().x()<<","<<fusion_object.vel_relative().y()<<std::endl;
 #endif
     }
 #ifdef DEBUG_SHOW
-    std::cout<<"trackedobjvec size:"<<trackedobjvec.obj_size()<<std::endl;
+//    std::cout<<"trackedobjvec size:"<<trackedobjvec.obj_size()<<std::endl;
 #endif
 //    for (size_t i = 0; i < trackedobjvec.obj_size(); i++)
 //    {

src/fusion/lidar_radar_fusion_cnn/fusion.hpp

@@ -128,7 +128,7 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
             centroid.set_z(lidar_object_arr.obj(match_idx[i].nlidar).centroid().z());
             centroid_ = fusion_object.mutable_centroid();
             centroid_->CopyFrom(centroid);
-            std::cout<<"lidar: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;
+//            std::cout<<"lidar: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;
 
         }else {
 //             std::cout<<"   fusion    is    ok  "<<std::endl;
@@ -171,7 +171,7 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
         dimension_ = fusion_object.mutable_dimensions();
         dimension_->CopyFrom(dimension);
 
-        std::cout<<" x     y    z:   "<<lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()<<"     "<<lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()<<std::endl;
+//        std::cout<<" x     y    z:   "<<lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()<<"     "<<lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()<<std::endl;
 
 
 
@@ -211,7 +211,6 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
 //    std::cout<<"   fusion   end    "<<std::endl;
 }
 
-
 void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array, iv::mobileye::mobileye& xobs_info)
 {
     int time_step = 0.3;
@@ -241,7 +240,7 @@ void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array,
         centroid_ = fusion_obj.mutable_centroid();
         centroid_->CopyFrom(centroid);
 #ifdef DEBUG_SHOW
-        std::cout<<"mobileye: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;
+//        std::cout<<"mobileye: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;
 #endif
 
         iv::fusion::Dimension dimension;
@@ -257,12 +256,48 @@ void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array,
 
 }
 
+void AddRadarBack(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array, iv::radar::radarobjectarray& radar_info)
+{
+    for(int j = 0; j< radar_info.obj_size() ; j++){
+        iv::fusion::fusionobject fusion_obj;
+        fusion_obj.set_sensor_type(1);
+        fusion_obj.set_yaw(0);
+        iv::fusion::VelXY vel_relative;
+        iv::fusion::VelXY *vel_relative_;
+        vel_relative.set_x(-radar_info.obj(j).vx());
+        vel_relative.set_y(-radar_info.obj(j).vy());
+        vel_relative_ = fusion_obj.mutable_vel_relative();
+        vel_relative_->CopyFrom(vel_relative);
+
+        iv::fusion::PointXYZ centroid;
+        iv::fusion::PointXYZ *centroid_;
+        centroid.set_x(-radar_info.obj(j).x());
+        centroid.set_y(-radar_info.obj(j).y());
+        centroid.set_z(1.0);
+        centroid_ = fusion_obj.mutable_centroid();
+        centroid_->CopyFrom(centroid);
+#ifdef DEBUG_SHOW
+//        std::cout<<"radar_back: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;
+#endif
+
+        iv::fusion::Dimension dimension;
+        iv::fusion::Dimension *dimension_;
+        dimension.set_x(0.5);
+        dimension.set_y(0.5);
+        dimension.set_z(0.5);
+        dimension_ = fusion_obj.mutable_dimensions();
+        dimension_->CopyFrom(dimension);
+        iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
+        obj->CopyFrom(fusion_obj);
+}
+}
+
 void ObsToNormal(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
 {
     for(int i = 0; i < lidar_radar_fusion_object_array.obj_size(); i++)
     {
-        if((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0) &&(lidar_radar_fusion_object_array.obj(i).centroid().y() > 20 || abs(lidar_radar_fusion_object_array.obj(i).centroid().x())>10) ) continue;
-        if ((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0)&&(lidar_radar_fusion_object_array.obj(i).centroid().y()>10 && abs(lidar_radar_fusion_object_array.obj(i).centroid().x())<1.3)) continue;
+//        if((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0) &&(lidar_radar_fusion_object_array.obj(i).centroid().y() > 20 || abs(lidar_radar_fusion_object_array.obj(i).centroid().x())>10) ) continue;
+//        if ((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0)&&(lidar_radar_fusion_object_array.obj(i).centroid().y()>10 && abs(lidar_radar_fusion_object_array.obj(i).centroid().x())<1.3)) continue;
         if((lidar_radar_fusion_object_array.obj(i).sensor_type() == 0)&&(lidar_radar_fusion_object_array.obj(i).centroid().y() <1.0  && abs(lidar_radar_fusion_object_array.obj(i).centroid().x())<1.3)) continue;
 
         if((lidar_radar_fusion_object_array.obj(i).dimensions().x()>0)&&
@@ -289,7 +324,8 @@ void ObsToNormal(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
                     nomal_centroid.set_nomal_x(lidar_radar_fusion_object_array.obj(i).centroid().x() + s);
                     nomal_centroid.set_nomal_y(lidar_radar_fusion_object_array.obj(i).centroid().y() + t);
                     if(abs(lidar_radar_fusion_object_array.obj(i).centroid().x() + s) <1.3 &&
-                            lidar_radar_fusion_object_array.obj(i).centroid().y() + t <1.0) continue;
+                            (lidar_radar_fusion_object_array.obj(i).centroid().y() + t) <1.0 ||
+                            (lidar_radar_fusion_object_array.obj(i).centroid().y() + t) <-1.0 ) continue;
                     else{
                         nomal_centroid.set_nomal_x(lidar_radar_fusion_object_array.obj(i).centroid().x() + s);
                         nomal_centroid.set_nomal_y(lidar_radar_fusion_object_array.obj(i).centroid().y() + t);
@@ -301,8 +337,8 @@ void ObsToNormal(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
             }
 
     }
-}}
-
+}
+}
 
 }
 }

src/fusion/lidar_radar_fusion_cnn/lidar_radar_fusion_cnn.pro

@@ -9,7 +9,7 @@ CONFIG -= app_bundle
 # depend on your compiler). Please consult the documentation of the
 # deprecated API in order to know how to port your code away from it.
 DEFINES += QT_DEPRECATED_WARNINGS
-DEFINES += DEBUG_SHOW
+#DEFINES += DEBUG_SHOW
 INCLUDEPATH +=Tracker
 
 HEADERS += \

src/fusion/lidar_radar_fusion_cnn/main.cpp

@@ -25,7 +25,8 @@ typedef  iv::radar::radarobjectarray RadarDataType;
 typedef  iv::lidar::objectarray LidarDataType;
 typedef  std::chrono::system_clock::time_point TimeType;
 
-iv::radar::radarobjectarray radarobjvec;
+iv::radar::radarobjectarray radar_front;
+iv::radar::radarobjectarray radar_back;
 iv::lidar::objectarray lidar_obj;
 iv::mobileye::mobileye mobileye_info;
 iv::mobileye::obs obs_info;
@@ -37,7 +38,7 @@ QTime gTime;
 using namespace std;
 int gntemp = 0;
 iv::fusion::fusionobjectarray li_ra_fusion;
-void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& radarobjvec, iv::fusion::fusionobjectarray& li_ra_fusion);
+void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& radar_front, iv::fusion::fusionobjectarray& li_ra_fusion);
 TrackerSettings settings;
 CTracker tracker(settings);
 bool m_isTrackerInitialized = false;
@@ -71,7 +72,7 @@ void Listenesrfront(const char * strdata,const unsigned int nSize,const unsigned
 
     //    }
 
-    radarobjvec.CopyFrom(radarobj);
+    radar_front.CopyFrom(radarobj);
     gMutex.unlock();
 }
 
@@ -88,7 +89,23 @@ 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);
+    datafusion(lidarobj,radar_front,li_ra_fusion);
+    gMutex.unlock();
+}
+
+void Listenradarback(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    iv::radar::radarobjectarray radarobj;
+    if(nSize<1)return;
+    if(false == radarobj.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<" Listenesrfront fail."<<std::endl;
+        return;
+    }
+    else{
+    }
+    gMutex.lock();
+    radar_back.CopyFrom(radarobj);
     gMutex.unlock();
 }
 
@@ -127,37 +144,11 @@ void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& r
 
     //    li_ra_fusion.Clear();
     //      std::cout<<" RLFusion     begin    "<<std::endl;
-    AddMobileye(li_ra_fusion,mobileye_info);
+//    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++)
-    //    {
-    //        if(abs(li_ra_fusion.obj(i).vel_relative().y())>10 )
-    //        {
-    //            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()<<"     "
-    //                    <<li_ra_fusion.obj(i).vel_relative().y()<<"     "
-    //                    <<li_ra_fusion.obj(i).dimensions().x()<<std::endl;
-
-    //        }
-
-    //    }
-
-
-    //    int nsize =0;
-    //    for(int nradar = 0; nradar<radarobjvec.obj_size(); nradar++)
-    //    {
-    //        if(radarobjvec.obj(nradar).bvalid() == false) continue;
-    //        if(sqrt(abs(radarobjvec.obj(nradar).x()))< 4 && radarobjvec.obj(nradar).y()< 20 ) nsize = nsize +1;
-    //    }
-
-    //        std::cout<<"   fusion_obj_size     radar_obj_size   :   "<<li_ra_fusion.obj_size()<<"     "<<nsize<<std::endl;
-
-
-    //    gMutex.lock();
-    //    std::cout<<" track   begin"<<std::endl;
+//    mobileye_info.clear_xobj();
+      AddRadarBack(li_ra_fusion,radar_back);
+      radar_back.clear_obj();
     //---------------------------------------------  init tracker  -------------------------------------------------
     if (!m_isTrackerInitialized)
     {
@@ -214,6 +205,7 @@ int main(int argc, char *argv[])
 
     void *gpa;
     gpa = iv::modulecomm::RegisterRecv("radar",Listenesrfront);
+    gpa = iv::modulecomm::RegisterRecv("radar_back",Listenradarback);
     gpa = iv::modulecomm::RegisterRecv("lidar_pointpillar",Listenlidarcnndetect);
     gpa = iv::modulecomm::RegisterRecv("mobileye",Listenmobileye);
     return a.exec();

src/include/proto/ultrasonic.proto

@@ -1,16 +1,16 @@
 /*
 		 2  3   4  5
  		/o--o---o--o\
-		|			|
- 	 1 o|			|o 6
- 		|			|
-		|			|
-		|			|
-		|			|
-    12 o|			|o 7
-		|			|
+		|	    |
+ 	     1 o|	    |o 6
+ 		|	    |
+		|	    |
+		|	    |
+		|	    |
+            12 o|	    |o 7
+		|	    |
 		\o--o---o--o/
-		11	10  9  8	
+	        11  10  9  8	
  */
 
 syntax = "proto2";
@@ -25,25 +25,25 @@ message ultrasonic
 	optional uint32 sigObjDist_FRMiddle = 4; //前中右
 	optional uint32 sigObjDist_FRCorner = 5; //前右角
 	optional uint32 sigObjDist_FRSide   = 6; //前右侧
-	optional uint32 sigObjDist_RLSide   = 7; //后左侧
-	optional uint32 sigObjDist_RLCorner = 8; //后左角
-	optional uint32 sigObjDist_RLMiddle = 9; //后中左
-	optional uint32 sigObjDist_RRMiddle = 10;//后中右
-	optional uint32 sigObjDist_RRCorner = 11;//后右角
-	optional uint32 sigObjDist_RRSide   = 12;//后右侧
+        optional uint32 sigObjDist_RRSide   = 7;//后右侧
+	optional uint32 sigObjDist_RRCorner = 8;//后右角
+	optional uint32 sigObjDist_RRMiddle = 9;//后中右
+	optional uint32 sigObjDist_RLMiddle = 10; //后中左
+	optional uint32 sigObjDist_RLCorner = 11; //后左角
+	optional uint32 sigObjDist_RLSide   = 12; //后左侧
 
-	optional uint32 sigSensor_Front_LS  = 13;//前左侧运行状态显示
-	optional uint32 sigSensor_Front_L   = 14;//前左角运行状态显示
-	optional uint32 sigSensor_Front_LM  = 15;//前中左
-	optional uint32 sigSensor_Front_RM  = 16;//前中右
-	optional uint32 sigSensor_Front_R   = 17;//前右角
-	optional uint32 sigSensor_Front_RS  = 18;//前右侧
-	optional uint32 sigSensor_Rear_L	= 19;//后左角
-	optional uint32 sigSensor_Rear_LS   = 20;//后左侧
-	optional uint32 sigSensor_Rear_LM   = 21;//后左中
-	optional uint32 sigSensor_Rear_R    = 22;//后右角
-	optional uint32 sigSensor_Rear_RS   = 23;//后右侧
-	optional uint32 sigSensor_Rear_RM   = 24;//后右中
+	optional uint32 sigSensor_Front_LS  = 13;//前左侧运行状态显示 
+        optional uint32 sigSensor_Front_LC  = 14;//前左角运行状态显示  
+        optional uint32 sigSensor_Front_LM  = 15;//前中左 
+        optional uint32 sigSensor_Front_RM  = 16;//前中右 
+        optional uint32 sigSensor_Front_RC  = 17;//前右角 
+        optional uint32 sigSensor_Front_RS  = 18;//前右侧
+        optional uint32 sigSensor_Rear_RS   = 19;//后右侧
+        optional uint32 sigSensor_Rear_RC   = 20;//后右角 
+        optional uint32 sigSensor_Rear_RM   = 21;//后右中 
+        optional uint32 sigSensor_Rear_LM   = 22;//后左中
+        optional uint32 sigSensor_Rear_LC   = 23;//后左角 
+        optional uint32 sigSensor_Rear_LS   = 24;//后左侧 
 
 	optional uint32 sigCANVoltageSt = 25;
 	optional uint32 sigPAVoltageSt = 26;

src/tool/adcndtmultimapping/main.cpp

@@ -28,8 +28,8 @@ int main(int argc, char *argv[])
    std::cout<<strpath.toStdString()<<std::endl;
 
    iv::xmlparam::Xmlparam xp(strpath.toStdString());
-   gstrlidarmsg = xp.GetParam("msg","lidarpc_center");
-   gstrimumsg = xp.GetParam("gpsmsg","ins550d_gpsimu");
+   gstrlidarmsg = xp.GetParam("msg","lidar_pc");
+   gstrimumsg = xp.GetParam("gpsmsg","hcp2_gpsimu");
     MainWindow w;
     w.show();
 

src/tool/view_pointcloud/view_pointcloud.pro

@@ -42,9 +42,17 @@ unix:LIBS +=  -lpcl_common\
         -lpcl_tracking\
         -lpcl_visualization
 
-INCLUDEPATH += $$PWD/../../../include/
-LIBS += -L$$PWD/../../../bin/ -lxmlparam -lmodulecomm -livlog -livfault
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
 
+!include(../../../include/ivboost.pri ) {
+    error( "Couldn't find the ivboost.pri file!" )
+}
+
+!include(../../../include/ivpcl.pri ) {
+    error( "Couldn't find the ivpcl.pri file!" )
+}
 
 LIBS += -lboost_system -lvtkCommonExecutionModel-6.3 -lvtkCommonCore-6.3 -lvtkRenderingLOD-6.3 -lvtkRenderingCore-6.3 \
         -lvtkFiltersSources-6.3