run leg commit

src/decition/common/common/car_status.h

@@ -163,8 +163,8 @@ namespace iv {
         iv::roadmode_vel mroadmode_vel;
         int vehGroupId;
         double mfEPSOff = 0.0;
-        float socfDis=15;   //停障触发距离
-        float aocfDis=25;   //避障触发距离//20
+        float socfDis=8;   //停障触发距离
+        float aocfDis=30;   //避障触发距离//20
         float aocfTimes=3; //避障触发次数
         float aobzDis=5;   //避障保障距离
         /// traffice light : 0x90

src/decition/decition_brain_sf_jsrunlegs/decition/adc_adapter/haomo_adapter.cpp

@@ -52,7 +52,7 @@ iv::decition::Decition iv::decition::HaoMoAdapter::getAdapterDeciton(GPS_INS now
     controlSpeed= dSpeed;
     if(controlSpeed>8)
         controlSpeed=8; //调试先限制下速度
-    controlSpeed=max(controlSpeed,10.0f); //对车子进行限速，车子最大速度是,倒车是5*3.6km/h，前进是10*3.6
+   // controlSpeed=min(controlSpeed,8.0f); //对车子进行限速，车子最大速度是,倒车是5*3.6km/h，前进是10*3.6
 
     //0227 10m nei xianzhi shache
     //障碍物距离在3~10米之间，让速度慢慢降到1，当障碍物距离小于3了，直接停车

src/decition/decition_brain_sf_jsrunlegs/decition/brain.cpp

@@ -498,6 +498,11 @@ void iv::decition::BrainDecition::run() {
             ServiceCarStatus.mbRunPause = true;
             ServiceCarStatus.mnBocheComplete--;
         }
+        std::cout<<"*********************************************"<<endl;
+        std::cout<<"mbRunPause==="<<ServiceCarStatus.mbRunPause<<endl;
+        std::cout<<"mapsize==="<<navigation_data.size()<<endl;
+        std::cout<<"boche==="<<ServiceCarStatus.mnBocheComplete<<endl;
+         std::cout<<"*********************************************"<<endl;
         if((ServiceCarStatus.mbRunPause)||(navigation_data.size()<1)||(ServiceCarStatus.mnBocheComplete>0))
         {
             ServiceCarStatus.mbBrainCtring = false;
@@ -721,10 +726,10 @@ void iv::decition::BrainDecition::run() {
             //		decition_gps = decitionGpsUnit->getDecideFromGPS(*gps_data_, navigation_data, obs_lidar_grid_);     // 新的
 
 
-            if ( ServiceCarStatus.epsMode==0 && ServiceCarStatus.receiveEps) {
-                ServiceCarStatus.mbRunPause=true;
+//            if ( ServiceCarStatus.epsMode==0 && ServiceCarStatus.receiveEps) {
+//                ServiceCarStatus.mbRunPause=true;
 
-            }
+//            }//chenxiaowei,20220228
             ServiceCarStatus.receiveEps=false;
 
             lastMode=ServiceCarStatus.epsMode;
@@ -1443,10 +1448,10 @@ void iv::decition::BrainDecition::UpdateChassis(const char *strdata, const unsig
     if(xchassis.has_epsmode()){
         ServiceCarStatus.epsMode = xchassis.epsmode();
         ServiceCarStatus.receiveEps=true;
-        if(ServiceCarStatus.epsMode == 0)
-        {
-            ServiceCarStatus.mbRunPause = true;
-        }
+//        if(ServiceCarStatus.epsMode == 0)
+//        {
+//            ServiceCarStatus.mbRunPause = true;
+//        }  //chenxiaowei,20220228
     }
     if(xchassis.has_torque())
     {
@@ -1534,7 +1539,7 @@ void iv::decition::BrainDecition::UpdateFusion(std::shared_ptr<iv::fusion::fusio
                 fusion_ptr_[dx*(iv::gry) + dy].type = xobs.type();
                 fusion_ptr_[dx*(iv::gry) + dy].high = xobs.dimensions().z();
                 fusion_ptr_[dx*(iv::gry) + dy].speed_x = xobs.vel_relative().x();
-                fusion_ptr_[dx*(iv::gry) + dy].speed_y = xobs.vel_relative().y();
+                fusion_ptr_[dx*(iv::gry) + dy].speed_y = xobs.velocity_linear_x();
                 fusion_ptr_[dx*(iv::gry) + dy].yaw = xobs.yaw();
                 fusion_ptr_[dx*(iv::gry) + dy].pointcount = 5;
                 fusion_ptr_[dx*(iv::gry) + dy].ob = 2;

src/decition/decition_brain_sf_jsrunlegs/decition/decide_gps_00.cpp

@@ -1333,8 +1333,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         controlAng=0-controlAng;
     }
 
-    //2020-0106
-    if(ServiceCarStatus.msysparam.mvehtype !="zhongche"){
+//    //2020-0106
+/*    if(ServiceCarStatus.msysparam.mvehtype !="zhongche"){
 
         if(vehState==avoiding){
             ServiceCarStatus.msysparam.vehWidth=2.4;
@@ -1345,7 +1345,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             controlAng=max(-150.0,controlAng);//35   zj-150
             controlAng=min(150.0,controlAng);//35     zj-150
         }
-    }
+    } */ //20200304,chenxiaowei
 
 //    givlog->debug("brain_decition","vehState: %d,controlAng: %f",
 //            vehState,controlAng);
@@ -1689,8 +1689,9 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             //            useFrenet = false;
             //            useOldAvoid = true;
         }
-        else if (useOldAvoid && avoidDistance>35) {    //zj 2021.06.21   gpsTraceNow[60].x)<0.02
-            // vehState = avoidObs; 0929
+       // else if (useOldAvoid && avoidDistance>35) {    //zj 2021.06.21   gpsTraceNow[60].x)<0.02
+         else if (useOldAvoid && avoidDistance>20) {
+        // vehState = avoidObs; 0929
             vehState = normalRun;
             turnLampFlag=0;
         }
@@ -1723,7 +1724,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             //            useFrenet = false;
             //            useOldAvoid = true;
         }
-        else if (useOldAvoid && backDistance>35 ) {//abs(gpsTraceNow[60].x)<0.05
+       // else if (useOldAvoid && backDistance>35 ) {//abs(gpsTraceNow[60].x)<0.05
+        else if (useOldAvoid && backDistance>20) {//abs(gpsTraceNow[60].x)<0.05
             // vehState = avoidObs; 0929
             vehState = normalRun;
             obstacle_avoid_flag=0;
@@ -2284,7 +2286,22 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                 dSpeed=0;
             }
     }else if(obsDistance>0 && obsDistance<15){   //均胜跑腿车距离可以适当减小
-        dSpeed=0;
+
+        if(ServiceCarStatus.msysparam.mvehtype=="haomo")
+        {
+            if(obsDistance>ServiceCarStatus.socfDis && obsDistance<15)
+            {
+                dSpeed = min(3.0,dSpeed);
+            }
+            else if(obsDistance<ServiceCarStatus.socfDis)
+            {
+               dSpeed=0;
+            }
+
+
+        }
+        else
+            dSpeed=0;
     }
 
     if(ServiceCarStatus.group_control){
@@ -2477,6 +2494,11 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                            <<"Decide_Brake"<< ","  <<gps_decition->brake<< ","
                            <<"Vehicle_RealSpd"<< ","  <<setprecision(2)<<now_gps_ins.speed<< ","
                            <<"OBS_Distance"<< ","<<obsDistance_log<< ","
+                           <<"OBS_Speed"<< ","<<obs_speed_for_avoid<< ","
+                           <<"Vehicle_state"<< ","<<vehState<< ","
+                          <<"avoid_flag"<<","<<obstacle_avoid_flag<<","
+                           <<"avoid_cnt"<<","<<ObsTimeSpan<<","
+                          <<"avoidXY_size"<<","<<gpsTraceAvoidXY.size()<<","
                            <<"Min_Decelation"","<<minDecelerate<< ","
                            <<"Decide_Angle"<< ","  << setprecision(2)<<gps_decition->wheel_angle<< ","
                            <<"Vehicle_GPS_heading"<< ","<< setprecision(10)<<now_gps_ins.ins_heading_angle<< ","
@@ -2484,6 +2506,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                            <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
                            <<"Trace_Point"<< ","<<PathPoint<< ","
                            <<"OBS_Speed"<< ","<<obsSpeed<< ","
+                           <<"OBS_Distance"<< ","<<obsDistance<< ","
                            <<"TTC"<< ","<<ttc<< ","
                           <<endl;
                    outfile.close();

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/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路径
 
@@ -128,7 +143,7 @@ LIBS += -lcudart -lcufft -lyaml-cpp
 
 #LIBS += -L/home/adc/soft/cudnn-10.2-linux-x64-v7.6.5.32/cuda/lib64 -lcudnn
 
-LIBS +=  -lmyelin -lnvinfer -lnvonnxparser -lnvcaffe_parser
+LIBS += -lnvinfer -lnvonnxparser -lnvcaffe_parser
 
 #LIBS += -L/home/nvidia/git/libtorch_gpu-1.6.0-linux-aarch64/lib -ltorch_cuda  -ltorch -lc10 -ltorch_cpu
 
@@ -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,145 @@ 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 +602,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 +664,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 +680,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 +694,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 +718,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.0274, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转 yaw   +
+    Eigen::AngleAxisd r_y ( -0.0426, Eigen::Vector3d ( 0,1,0 ) ); //沿 Y 轴旋转 roll  +
+    Eigen::AngleAxisd r_x ( -0.0059, 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.13, 0.28;
     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

@@ -350,7 +350,7 @@ void PointPillars::OnnxToTRTModel(
 
     // Build the engine
     builder->setMaxBatchSize(kBatchSize);
-    builder->setHalf2Mode(true);
+    //builder->setHalf2Mode(true);
     nvinfer1::IBuilderConfig* config = builder->createBuilderConfig();
     config->setMaxWorkspaceSize(1 << 25);
     nvinfer1::ICudaEngine* engine =

src/detection/detection_lidar_PointPillars_MultiHead/pointpillars.h

@@ -76,7 +76,7 @@ class Logger : public nvinfer1::ILogger {
   explicit Logger(Severity severity = Severity::kWARNING)
       : reportable_severity(severity) {}
 
-  void log(Severity severity, const char* msg) override {
+  void log(Severity severity, nvinfer1::AsciiChar const* msg) noexcept {
     // suppress messages with severity enum value greater than the reportable
     if (severity > reportable_severity) return;
 
@@ -179,8 +179,8 @@ class PointPillars {
     void* pfe_buffers_[2];
     //variable for doPostprocessCudaMultiHead
     void* rpn_buffers_[8];
-    
-    std::vector<float*> rpn_box_output_; 
+
+    std::vector<float*> rpn_box_output_;
     std::vector<float*> rpn_cls_output_;
 
     float* dev_scattered_feature_;
@@ -221,7 +221,7 @@ class PointPillars {
     void InitParams();
     /**
      * @brief Initializing TensorRT instances
-     * @param[in] usr_onnx_ if true, parse ONNX 
+     * @param[in] usr_onnx_ if true, parse ONNX
      * @details Called in the constructor
      */
     void InitTRT(const bool use_onnx);
@@ -237,10 +237,10 @@ class PointPillars {
     /**
      * @brief Convert Engine to TensorRT model
      * @param[in] model_file Engine(TensorRT) model file path
-     * @param[out] engine_ptr TensorRT model engine made 
+     * @param[out] engine_ptr TensorRT model engine made
      * @details Load Engine model, and convert it to TensorRT model
      */
-    void EngineToTRTModel(const std::string &engine_file ,     
+    void EngineToTRTModel(const std::string &engine_file ,
                         nvinfer1::ICudaEngine** engine_ptr) ;
 
     void LoadEngineModel(const std::string &engine_file ,
@@ -288,4 +288,3 @@ class PointPillars {
                     std::vector<int>* out_labels,
                     std::vector<float>* out_scores);
 };
-

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

@@ -696,7 +696,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 +711,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 +903,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/fusion/lidar_radar_fusion_cnn/Tracker/Tracking.hpp

@@ -22,7 +22,7 @@ bool InitTracker(CTracker& tracker)
     settings.m_accelNoiseMag = 0.5f; // 卡尔曼的噪声放大器
     settings.m_distThres = 30.f; // 匹配算法中的距离阈值
     settings.m_minAreaRadiusPix = -1.f;//frame.rows / 20.f; // 目标的最小面积半径（像素）
-    settings.m_maximumAllowedSkippedFrames = 2; // 被跟踪目标允许未匹配到的最大次数，当超过这一数值，该目标的跟踪器将被移除
+    settings.m_maximumAllowedSkippedFrames = 3; // 被跟踪目标允许未匹配到的最大次数，当超过这一数值，该目标的跟踪器将被移除
     settings.m_maxTraceLength = 5; // 最大跟踪长度，即历史轨迹保留的最大长度
 
     tracker.setSettings(settings);
@@ -58,9 +58,9 @@ 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);
@@ -159,9 +159,9 @@ 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

src/fusion/lidar_radar_fusion_cnn/fusion.hpp

@@ -102,6 +102,7 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
         fusion_object.set_sensor_type(0);
         fusion_object.set_type(lidar_object_arr.obj(match_idx[i].nlidar).mntype());
         fusion_object.set_prob(lidar_object_arr.obj(match_idx[i].nlidar).score());
+        fusion_object.set_velocity_linear_x(-lidar_object_arr.obj(match_idx[i].nlidar).velocity_linear_x());
 
         if(match_idx.at(i).nradar == -1000)
         {
@@ -128,8 +129,9 @@ 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);
+#ifdef DEBUG_SHOW
             std::cout<<"lidar: "<<centroid.x()<<","<<centroid.y()<<","<<centroid.z()<<std::endl;
-
+#endif
         }else {
 //             std::cout<<"   fusion    is    ok  "<<std::endl;
             fusion_object.set_yaw(radar_object_array.obj(match_idx.at(i).nradar).angle());
@@ -171,7 +173,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;
 
 
 
@@ -261,8 +263,8 @@ 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)&&

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

@@ -87,7 +87,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;
+        std::cout<<"   obj size  "<<lidarobj.obj_size()<<std::endl;
     datafusion(lidarobj,radarobjvec,li_ra_fusion);
     gMutex.unlock();
 }
@@ -127,9 +127,9 @@ 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();
+//    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++)
     //    {
@@ -172,7 +172,7 @@ void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& r
 
     //--------------------------------------------  end tracking  --------------------------------------------------
     //    gMutex.unlock();
-    iv::fusion::fusionobjectarray out_fusion = trackedobjvec;
+    iv::fusion::fusionobjectarray out_fusion = li_ra_fusion;
     ObsToNormal(out_fusion);
     string out;
 
@@ -214,7 +214,7 @@ int main(int argc, char *argv[])
 
     void *gpa;
     gpa = iv::modulecomm::RegisterRecv("radar",Listenesrfront);
-    gpa = iv::modulecomm::RegisterRecv("lidar_pointpillar",Listenlidarcnndetect);
+    gpa = iv::modulecomm::RegisterRecv("lidar_track",Listenlidarcnndetect);
     gpa = iv::modulecomm::RegisterRecv("mobileye",Listenmobileye);
     return a.exec();
 }

src/fusion/lidar_radar_fusion_cnn_1025/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/tool/PerceptionShow/mainwindow.cpp

@@ -265,7 +265,7 @@ MainWindow::MainWindow(QWidget *parent) :
 
 //   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/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();