change yaw

src/detection/CenterPoint-master/CMakeLists.txt

@@ -5,6 +5,7 @@ CONFIG -= app_bundle
 
 QMAKE_CXXFLAGS += -std=gnu++17
 QMAKE_LFLAGS += -no-pie  -Wl,--no-as-needed
+
 # The following define makes your compiler emit warnings if you use
 # any feature of Qt which as been marked deprecated (the exact warnings
 # depend on your compiler). Please consult the documentation of the
@@ -15,8 +16,6 @@ DEFINES += QT_DEPRECATED_WARNINGS
 # 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.
 #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
-TARGET = CenterPoint
-TEMPLATE = app
 
 SOURCES += src/main.cpp \
     src/centerpoint.cpp \

src/detection/CenterPoint-master/detection_centerpoint.xml

@@ -0,0 +1,10 @@
+<xml>	
+	<node name="detection_centerpoint.xml">
+		<param name="pfeOnnxFilePath" value="/media/nvidia/Lexar/onnx/pfe_self2.onnx" />
+		<param name="rpnOnnxFilePath" value="/media/nvidia/Lexar/onnx/rpn_self2.onnx" />
+		<param name="pfeEnginePath" value="/media/nvidia/Lexar/onnx/pfe_fp_self2.engine" />
+		<param name="rpnEnginePath" value="/media/nvidia/Lexar/onnx/rpn_fp_self2.engine" />
+                <param name="threshold" value="0.5" />
+		<param name="load_engine" value="true" />  #if does not have engine,value=false,then use onnx get engine
+	</node>
+</xml>

src/detection/CenterPoint-master/include/config.h

@@ -6,7 +6,7 @@
 #define MAX_POINTS 220000
 #define POINT_DIM 5
 
-// pillar size
+// pillar size 
 #define X_STEP 0.32f
 #define Y_STEP 0.32f
 #define X_MIN -74.88f
@@ -16,18 +16,12 @@
 #define Z_MIN -2.0f
 #define Z_MAX 4.0f
 
-//#define X_CENTER_MIN -80.0f
-//#define X_CENTER_MAX 80.0f
-//#define Y_CENTER_MIN -80.0f
-//#define Y_CENTER_MAX 80.0f
-//#define Z_CENTER_MIN -10.0f
-//#define Z_CENTER_MAX 10.0f
-#define X_CENTER_MIN -10.0f
-#define X_CENTER_MAX 10.0f
-#define Y_CENTER_MIN -10.0f
-#define Y_CENTER_MAX 10.0f
-#define Z_CENTER_MIN -5.0f
-#define Z_CENTER_MAX 5.0f
+#define X_CENTER_MIN -80.0f
+#define X_CENTER_MAX 80.0f
+#define Y_CENTER_MIN -80.0f
+#define Y_CENTER_MAX 80.0f
+#define Z_CENTER_MIN -10.0f
+#define Z_CENTER_MAX 10.0f
 
 #define PI 3.141592653f
 // paramerters for preprocess
@@ -40,15 +34,14 @@
 #define THREAD_NUM 4
 // paramerters for postprocess
 #define SCORE_THREAHOLD 0.1f
-#define NMS_THREAHOLD 0.1f
-//#define INPUT_NMS_MAX_SIZE 4096
-#define INPUT_NMS_MAX_SIZE 1000
+#define NMS_THREAHOLD 0.7f
+#define INPUT_NMS_MAX_SIZE 4096
 #define OUTPUT_NMS_MAX_SIZE 500
 // #define THREADS_PER_BLOCK_NMS  sizeof(unsigned long long) * 8
 const int THREADS_PER_BLOCK_NMS = sizeof(unsigned long long) * 8;
 
 // OUT_SIZE_FACTOR * OUTPUT_H  * Y_STEP = Y_MAX - Y_MIN
-#define OUT_SIZE_FACTOR 1.0f
+#define OUT_SIZE_FACTOR 1.0f    
 
 #define TASK_NUM 1
 #define REG_CHANNEL 2
@@ -57,8 +50,8 @@ const int THREADS_PER_BLOCK_NMS = sizeof(unsigned long long) * 8;
 // #define VEL_CHANNEL 2 //don't defined in waymo
 #define DIM_CHANNEL 3
 
-// spatial output size of rpn
-#define OUTPUT_H 468
+// spatial output size of rpn 
+#define OUTPUT_H 468  
 #define OUTPUT_W 468
 #endif
 
@@ -69,7 +62,7 @@ const int THREADS_PER_BLOCK_NMS = sizeof(unsigned long long) * 8;
 
 // ========================================NUSCENES CENTERPOINT CONFIG======================================== 
 
- // pillar size
+// // pillar size 
 // #define X_STEP 0.2f
 // #define Y_STEP 0.2f
 // #define X_MIN -51.2f

src/detection/CenterPoint-master/src/main.cpp

@@ -14,7 +14,7 @@
  * limitations under the License.
  */
 
-// close lidar_ufk_pda
+#include <QCoreApplication>
 #include "argsParser.h"
 #include "buffers.h"
 #include "common.h"
@@ -48,8 +48,11 @@
 #include <thread>
 #include "objectarray.pb.h"
 
+#define M_PI       3.14159265358979323846
+
 const std::string gSampleName = "TensorRT.sample_onnx_centerpoint";
 
+float threshold = 0.5;
 
 CenterPoint * centerpoint = nullptr ;
 
@@ -63,26 +66,24 @@ void * gpa;
 void * gpdetect;
 
 
-string lidarname = "lidar_pointpillar";
-//string lidarname = "lidar_pc";
+string lidarname = "lidar_pc";
 string detectname = "lidar_track";
 
+
+//change lidarpoints to waymo type,x-->forward,y-->left
 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 + 0] = point.y;
+        out_points_array[i * 5 + 1] = -point.x;
         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;
 
-//        std::cout<<"xyz="<<point.x<<point.y<<point.z<<std::endl;
-
-
-        std::cout<<"the intensity = "<< out_points_array[i * 5 + 3]<< std::endl;
+        //std::cout<<"the intensity = "<< out_points_array[i * 5 + 3]<< std::endl;
     }
 }
 
@@ -96,15 +97,20 @@ void GetLidarObj(std::vector<Box> &predResult,iv::lidar::objectarray & lidarobjv
 
         Box result = predResult[idx];
 
-        if (result.score < 0.2) continue;
+        if (result.score < threshold) continue;
 
         std::cout<<" The scores = "<<result.score<<std::endl;
 
-        lidarobj.set_tyaw(result.theta);
+        lidarobj.set_tyaw(-result.theta);
+
+        std::cout<<" The theta = "<<result.theta<<std::endl;
+        std::cout<<" The xyz = "<<result.cls<<" "<<result.w<<" "<<result.l<<" "<<result.h<<std::endl;
+
+
         iv::lidar::PointXYZ centroid;
         iv::lidar::PointXYZ * _centroid;
-        centroid.set_x(result.x);
-        centroid.set_y(result.y);
+        centroid.set_x(-result.y);
+        centroid.set_y(result.x);
         centroid.set_z(result.z);
         _centroid = lidarobj.mutable_centroid();
         _centroid->CopyFrom(centroid);
@@ -127,8 +133,8 @@ void GetLidarObj(std::vector<Box> &predResult,iv::lidar::objectarray & lidarobjv
 
         iv::lidar::PointXYZ position;
         iv::lidar::PointXYZ * _position;
-        position.set_x(result.x);
-        position.set_y(result.y);
+        position.set_x(-result.y);
+        position.set_y(result.x);
         position.set_z(result.z);
         _position = lidarobj.mutable_position();
         _position->CopyFrom(position);
@@ -140,9 +146,11 @@ void GetLidarObj(std::vector<Box> &predResult,iv::lidar::objectarray & lidarobjv
 
         iv::lidar::PointXYZI point_cloud;
         iv::lidar::PointXYZI * _point_cloud;
-        point_cloud.set_x(result.x);
-        point_cloud.set_y(result.y);
+
+        point_cloud.set_x(-result.y);
+        point_cloud.set_y(result.x);
         point_cloud.set_z(result.z);
+
         point_cloud.set_i(0);
 
         _point_cloud = lidarobj.add_cloud();
@@ -150,12 +158,20 @@ void GetLidarObj(std::vector<Box> &predResult,iv::lidar::objectarray & lidarobjv
 
         iv::lidar::Dimension ld;
         iv::lidar::Dimension * pld;
-        ld.set_x(result.l);
-        ld.set_y(result.w);
-        ld.set_z(result.h);
+
+
+        ld.set_x(result.l);// w
+        ld.set_y(result.w);// l
+        ld.set_z(result.h);// h
+
+
+//        ld.set_x(result.h);// w
+//        ld.set_y(result.l);// l
+//        ld.set_z(result.w);// h
+
+
         pld = lidarobj.mutable_dimensions();
         pld->CopyFrom(ld);
-
         iv::lidar::lidarobject * po = lidarobjvec.add_obj();
         po->CopyFrom(lidarobj);
     }
@@ -195,78 +211,48 @@ void DectectOnePCD(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr)
     std::cout<<"points num is "<<pointsnum<<std::endl;
 }
 
- // read source lidar_pointcloud
-//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<<"ListenPointCloud is  ok  ------------  "<<std::endl;
-
-//    if(nSize <=16)return;
-//    unsigned int * pHeadSize = (unsigned int *)strdata;
-//    if(*pHeadSize > nSize)
-//    {
-//        givlog->verbose("ListenPointCloud data is small headsize = %d, data size is %d", *pHeadSize, nSize);
-//        std::cout<<"ListenPointCloud data is small headsize ="<<*pHeadSize<<"  data size is"<<nSize<<std::endl;
-//    }
-
-//    gnothavedatatime = 0;
-//    QTime xTime;
-//    xTime.start();
-
-//    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
-//                new pcl::PointCloud<pcl::PointXYZI>());
-//    int nNameSize;
-//    nNameSize = *pHeadSize - 4-4-8;
-//    char * strName = new char[nNameSize+1];strName[nNameSize] = 0;
-//    std::shared_ptr<char> str_ptr;
-//    str_ptr.reset(strName);
-//    memcpy(strName,(char *)((char *)strdata +4),nNameSize);
-//    point_cloud->header.frame_id = strName;
-//    memcpy(&point_cloud->header.seq,(char *)strdata+4+nNameSize,4);
-//    memcpy(&point_cloud->header.stamp,(char *)strdata+4+nNameSize+4,8);
-//    int nPCount = (nSize - *pHeadSize)/sizeof(pcl::PointXYZI);
-//    int i;
-//    pcl::PointXYZI * p;
-//    p = (pcl::PointXYZI *)((char *)strdata + *pHeadSize);
-//    for(i=0;i<nPCount;i++)
-//    {
-//        pcl::PointXYZI xp;
-//        memcpy(&xp,p,sizeof(pcl::PointXYZI));
-//        xp.z = xp.z;
-//        point_cloud->push_back(xp);
-//        p++;
-//    }
+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;
 
-//    DectectOnePCD(point_cloud);
+    std::cout<<"ListenPointCloud is  ok  ------------  "<<std::endl;
 
-//    std::cout<<"ListenPointCloud is  end  ------------  "<<std::endl;
-//}
+    if(nSize <=16)return;
+    unsigned int * pHeadSize = (unsigned int *)strdata;
+    if(*pHeadSize > nSize)
+    {
+        givlog->verbose("ListenPointCloud data is small headsize = %d, data size is %d", *pHeadSize, nSize);
+        std::cout<<"ListenPointCloud data is small headsize ="<<*pHeadSize<<"  data size is"<<nSize<<std::endl;
+    }
 
-// read segmentaion_cnn output lidar_pointcloud
-void ListenPointCloud(const char *strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-    iv::lidar::objectarray lidarobjvec;
-    std::string in;
-    in.append(strdata,nSize);
-    lidarobjvec.ParseFromString(in);
+    gnothavedatatime = 0;
+    QTime xTime;
+    xTime.start();
 
     pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
                 new pcl::PointCloud<pcl::PointXYZI>());
-    for(int i=0; i<lidarobjvec.obj_size();i++){
-        iv::lidar::lidarobject lidarobj = lidarobjvec.obj(i);
-        if(lidarobj.type_name()!="car" && lidarobj.type_name()!="pedestrian")
-            continue;
-        for(int j=0;j<lidarobj.cloud_size();j++){
-//            iv::lidar::PointXYZI Point = lidarobj.cloud(j);
-            pcl::PointXYZI xp;
-            xp.x = lidarobj.cloud(j).x();
-            xp.y = lidarobj.cloud(j).y();
-            xp.z = lidarobj.cloud(j).z();
-            xp.intensity = lidarobj.cloud(j).i();
-            point_cloud->push_back(xp);
-        }
+    int nNameSize;
+    nNameSize = *pHeadSize - 4-4-8;
+    char * strName = new char[nNameSize+1];strName[nNameSize] = 0;
+    std::shared_ptr<char> str_ptr;
+    str_ptr.reset(strName);
+    memcpy(strName,(char *)((char *)strdata +4),nNameSize);
+    point_cloud->header.frame_id = strName;
+    memcpy(&point_cloud->header.seq,(char *)strdata+4+nNameSize,4);
+    memcpy(&point_cloud->header.stamp,(char *)strdata+4+nNameSize+4,8);
+    int nPCount = (nSize - *pHeadSize)/sizeof(pcl::PointXYZI);
+    int i;
+    pcl::PointXYZI * p;
+    p = (pcl::PointXYZI *)((char *)strdata + *pHeadSize);
+    for(i=0;i<nPCount;i++)
+    {
+        pcl::PointXYZI xp;
+        memcpy(&xp,p,sizeof(pcl::PointXYZI));
+        xp.z = xp.z;
+        point_cloud->push_back(xp);
+        p++;
     }
+
     DectectOnePCD(point_cloud);
 
     std::cout<<"ListenPointCloud is  end  ------------  "<<std::endl;
@@ -363,6 +349,9 @@ void exitfunc()
 
 int main(int argc, char** argv)
 {
+
+    QCoreApplication a(argc, argv);
+
     gfault = new iv::Ivfault("lidar_centerpoint");
     givlog = new iv::Ivlog("lidar_centerpoint");
     gfault->SetFaultState(0,0,"centerpoint initialize. ");
@@ -396,16 +385,23 @@ int main(int argc, char** argv)
 //    params.fp16 = args.runInFp16;
 //    params.load_engine = args.loadEngine;
 
-    params.pfeOnnxFilePath = "/home/nvidia/modularization/src/detection/CenterPoint-master/models/pfe_baseline32000.onnx";
-    params.rpnOnnxFilePath = "/home/nvidia/modularization/src/detection/CenterPoint-master/models/rpn_baseline.onnx";
-    params.pfeSerializedEnginePath = "/home/nvidia/modularization/src/detection/CenterPoint-master/models/pfe_fp.engine";
-    params.rpnSerializedEnginePath = "/home/nvidia/modularization/src/detection/CenterPoint-master/models/rpn_fp.engine";
+    QString strpath = QCoreApplication::applicationDirPath();
+    strpath = strpath + "/detection_centerpoint.xml";
+    std::cout<<strpath.toStdString()<<std::endl;
+
+    iv::xmlparam::Xmlparam xparam(strpath.toStdString());
+    params.pfeOnnxFilePath = xparam.GetParam("pfeOnnxFilePath","./pfe.onnx");
+    params.rpnOnnxFilePath = xparam.GetParam("rpnOnnxFilePath","./rpn.onnx");
+    params.pfeSerializedEnginePath = xparam.GetParam("pfeEnginePath","./pfe.engine");
+    params.rpnSerializedEnginePath = xparam.GetParam("rpnEnginePath","./rpn.engine");
     params.savePath = "/home/nvidia/modularization/src/detection/CenterPoint-master/results";
     string filePath = "/home/nvidia/modularization/src/detection/CenterPoint-master/lidars";
     params.filePaths=glob(filePath + "/seq_*.bin");
     params.fp16 = true;
-    params.load_engine = true;
+    params.load_engine = xparam.GetParam("load_engine",true);
 
+    //检测分数阈值
+    threshold = xparam.GetParam("threshold",0.5);
 
     // Input Output Names, according to TASK_NUM
     params.pfeInputTensorNames.push_back("input.1");
@@ -460,7 +456,7 @@ int main(int argc, char** argv)
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
     }
 
-    return 1;
+    return a.exec();
 }
 
 

src/detection/CenterPoint-master/src/postprocess.cpp

@@ -160,7 +160,7 @@ void postprocess(samplesCommon::BufferManager * buffers, std::vector<Box>& predR
 
     std::vector<std::string> scoreName{ "265"};
     std::vector<std::string> clsName{   "266"};
-    for (int taskIdx=0;taskIdx < TASK_NUM;taskIdx++){
+for (int taskIdx=0;taskIdx < TASK_NUM;taskIdx++){
         std::vector<Box> predBoxs;
         float* reg = static_cast<float*>(buffers->getHostBuffer(regName[taskIdx]));
         float* height = static_cast<float*>(buffers->getHostBuffer(heightName[taskIdx]));
@@ -281,9 +281,16 @@ void postprocessGPU(samplesCommon::BufferManager * buffers,
             box.x = (host_boxes[i  + 0 * boxSizeAft] + xIdx) *OUT_SIZE_FACTOR*X_STEP + X_MIN;
             box.y = (host_boxes[i  + 1 * boxSizeAft] + yIdx) * OUT_SIZE_FACTOR*Y_STEP + Y_MIN;
             box.z = host_boxes[i +  2 * boxSizeAft];
-            box.l = host_boxes[i +  3 * boxSizeAft];
-            box.h = host_boxes[i + 4 * boxSizeAft];
-            box.w = host_boxes[i + 5 * boxSizeAft];
+
+//            box.l = host_boxes[i +  3 * boxSizeAft];
+//            box.h = host_boxes[i + 4 * boxSizeAft];
+//            box.w = host_boxes[i + 5 * boxSizeAft];
+
+
+             box.w = host_boxes[i +  3 * boxSizeAft];
+             box.l = host_boxes[i + 4 * boxSizeAft];
+             box.h = host_boxes[i + 5 * boxSizeAft];
+
             float theta_s =host_boxes[i + 6 * boxSizeAft];
             float theta_c =host_boxes[i + 7 * boxSizeAft];
             box.theta = atan2(theta_s, theta_c);