change detection_lidar_cnn_segmentation+_trt. not complete.

src/detection/detection_lidar_cnn_segmentation_trt/detection_lidar_cnn_segmentation_trt.pro

@@ -15,6 +15,8 @@ DEFINES += QT_DEPRECATED_WARNINGS
 #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
 
 SOURCES += \
+        ../../include/msgtype/object.pb.cc \
+        ../../include/msgtype/objectarray.pb.cc \
         lib/src/TrtNet.cpp \
         main.cpp \
         src/cluster2d.cpp \
@@ -30,6 +32,8 @@ else: unix:!android: target.path = /opt/$${TARGET}/bin
 !isEmpty(target.path): INSTALLS += target
 
 HEADERS += \
+    ../../include/msgtype/object.pb.h \
+    ../../include/msgtype/objectarray.pb.h \
     include/lidar_apollo_instance_segmentation/cluster2d.hpp \
     include/lidar_apollo_instance_segmentation/debugger.hpp \
     include/lidar_apollo_instance_segmentation/detector.hpp \
@@ -77,3 +81,15 @@ unix:LIBS +=  -lpcl_common\
         -lpcl_surface\
         -lpcl_tracking\
         -lpcl_visualization
+
+
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+INCLUDEPATH += $$PWD/../../include/msgtype

src/detection/detection_lidar_cnn_segmentation_trt/main.cpp

@@ -4,8 +4,20 @@
 #include <pcl/io/pcd_io.h>
 
 #include <chrono>
+#include <thread>
 
 #include "lidar_apollo_instance_segmentation/detector.hpp"
+
+#include "object.pb.h"
+#include "objectarray.pb.h"
+
+#include "modulecomm.h"
+#include "xmlparam.h"
+
+
+void * gpa;
+void * gpdetect;
+
 LidarApolloInstanceSegmentation * gseg;
 
 void test()
@@ -30,7 +42,7 @@ void test()
     }
 
     int i;
-    for(i=0;i<1000;i++)
+    for(i=0;i<10;i++)
     {
 
     int64_t time1 =  std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
@@ -42,14 +54,294 @@ void test()
                 xpc,
                 objvec);
 
+
     int64_t time2 =  std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
 
     std::cout<<"complete test. use "<<(time2 - time1)/1000<< std::endl;
 
+//    std::this_thread::sleep_for(std::chrono::milliseconds(80));
+
     }
     std::cout<<"test complete."<<std::endl;
 }
 
+std::string gstrinput;
+std::string gstroutput;
+
+static iv::lidar::PointXYZ pcltoprotopoint(pcl::PointXYZ x)
+{
+    iv::lidar::PointXYZ lx;
+    lx.set_x(x.x);
+    lx.set_y(x.y);
+    lx.set_z(x.z);
+    return lx;
+}
+
+static void GetLidarObj(std::vector<Obstacle> objvec,iv::lidar::objectarray & lidarobjvec)
+{
+    int i;
+
+    for(i=0;i<objvec.size();i++)
+    {
+        Obstacle x;
+        iv::lidar::lidarobject lidarobj;
+  //      iv::lidar::object xo;
+
+        x = objvec.at(i);
+        pcl::PointCloud<pcl::PointXYZI> in_cluster = *x.cloud_ptr;
+
+
+
+        float min_x = std::numeric_limits<float>::max();
+        float max_x = -std::numeric_limits<float>::max();
+        float min_y = std::numeric_limits<float>::max();
+        float max_y = -std::numeric_limits<float>::max();
+        float min_z = std::numeric_limits<float>::max();
+        float max_z = -std::numeric_limits<float>::max();
+        float average_x = 0, average_y = 0, average_z = 0;
+        float length, width, height;
+
+        pcl::PointXYZ min_point;
+        pcl::PointXYZ max_point;
+  //      pcl::PointXYZ average_point;
+        pcl::PointXYZ centroid;
+
+        centroid.x = 0;
+        centroid.y = 0;
+        centroid.z = 0;
+
+        for (auto pit = in_cluster.points.begin(); pit != in_cluster.points.end(); ++pit)
+        {
+            average_x += pit->x;
+            average_y += pit->y;
+            average_z += pit->z;
+            centroid.x += pit->x;
+            centroid.y += pit->y;
+            centroid.z += pit->z;
+
+            if (pit->x < min_x)
+                min_x = pit->x;
+            if (pit->y < min_y)
+                min_y = pit->y;
+            if (pit->z < min_z)
+                min_z = pit->z;
+            if (pit->x > max_x)
+                max_x = pit->x;
+            if (pit->y > max_y)
+                max_y = pit->y;
+            if (pit->z > max_z)
+                max_z = pit->z;
+        }
+
+        // min, max points
+        min_point.x = min_x;
+        min_point.y = min_y;
+        min_point.z = min_z;
+        max_point.x = max_x;
+        max_point.y = max_y;
+        max_point.z = max_z;
+
+        if (in_cluster.points.size() > 0)
+        {
+            centroid.x /= in_cluster.points.size();
+            centroid.y /= in_cluster.points.size();
+            centroid.z /= in_cluster.points.size();
+        }
+        length = max_point.x - min_point.x;
+        width = max_point.y - min_point.y;
+        height = max_point.z - min_point.z;
+
+        iv::lidar::PointXYZ lx;
+        iv::lidar::PointXYZ * plx;
+        lx = pcltoprotopoint(centroid);
+        plx = lidarobj.mutable_centroid();
+        plx->CopyFrom(lx);
+        lx = pcltoprotopoint(min_point);
+        plx = lidarobj.mutable_min_point();
+        plx->CopyFrom(lx);
+        lx = pcltoprotopoint(max_point);
+        plx = lidarobj.mutable_max_point();
+        plx->CopyFrom(lx);
+
+
+
+
+//        lidarobj.centroid = centroid;
+//        lidarobj.min_point = min_point;
+//        lidarobj.max_point = max_point;
+
+
+        lx.set_x(min_point.x + length / 2);
+        lx.set_y(min_point.y + width / 2);
+        lx.set_z(min_point.z + height / 2);
+
+        plx = lidarobj.mutable_position();
+        plx->CopyFrom(lx);
+
+//        lidarobj.position.x = min_point.x + length / 2;
+//        lidarobj.position.y = min_point.y + width / 2;
+//        lidarobj.position.z = min_point.z + height / 2;
+        iv::lidar::Dimension ld;
+        iv::lidar::Dimension * pld;
+        ld.set_x((length < 0) ? -1 * length : length);
+        ld.set_y((width < 0) ? -1 * width : width);
+        ld.set_z((height < 0) ? -1 * height : height);
+//        lidarobj.dimensions.x = ((length < 0) ? -1 * length : length);
+//        lidarobj.dimensions.y = ((width < 0) ? -1 * width : width);
+//        lidarobj.dimensions.z = ((height < 0) ? -1 * height : height);
+        pld = lidarobj.mutable_dimensions();
+        pld->CopyFrom(ld);
+
+        lidarobj.set_mntype(x.meta_type);
+        lidarobj.set_score(x.score);
+
+
+        lidarobj.set_type_name(x.GetTypeString());
+
+        int j;
+        for(j=0;j<x.meta_type_probs.size();j++)
+        {
+            lidarobj.add_type_probs(x.meta_type_probs.at(j));
+        }
+
+        for(j=0;j<x.cloud_ptr->size();j++)
+        {
+            iv::lidar::PointXYZI * pcp = lidarobj.add_cloud();
+            pcl::PointXYZI pp = x.cloud_ptr->at(j);
+            pcp->set_x(pp.x);
+            pcp->set_y(pp.y);
+            pcp->set_z(pp.z);
+            pcp->set_i(pp.intensity);
+        }
+
+        iv::lidar::lidarobject * po = lidarobjvec.add_obj();
+        po->CopyFrom(lidarobj);
+
+//        givlog->verbose("CNNOBJ","    object %d type:%s x:%6.3f y:%6.3f",i,
+//                        lidarobj.type_name().data(),lidarobj.position().x(),
+//                        lidarobj.position().y());
+
+
+//        lidarobj.mnType = x.meta_type;
+
+//        lidarobj.score = x.score;
+//        lidarobj.type_probs = x.meta_type_probs;
+//        lidarobj.cloud_ptr = x.cloud_ptr;
+
+//        int ndatalen;
+//        char * strx = lidarobj.serialize(ndatalen);
+
+//        lidarobj.unserialize(strx,ndatalen);
+//        delete strx;
+
+//        lidarobjvec.push_back(lidarobj);
+
+    }
+}
+
+
+void DectectOnePCD(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr)
+{
+    std::cout<<"pcd size is "<<pc_ptr->size()<<std::endl;
+    pcl::PointIndices valid_idx;
+    auto &indices = valid_idx.indices;
+    indices.resize(pc_ptr->size());
+    std::iota(indices.begin(), indices.end(), 0);
+
+    std::vector<Obstacle> objvec;
+
+    gseg->detectDynamicObjects(
+                pc_ptr,
+                objvec);
+
+    std::cout<<"obj size is "<<objvec.size()<<std::endl;
+
+//    std::vector<iv::lidar::lidarobject> lidarobjvec;
+    iv::lidar::objectarray lidarobjvec;
+    GetLidarObj(objvec,lidarobjvec);
+
+    double timex = pc_ptr->header.stamp;
+    timex = timex/1000.0;
+    lidarobjvec.set_timestamp(pc_ptr->header.stamp);
+
+    int ntlen;
+    std::string out = lidarobjvec.SerializeAsString();
+ //   char * strout = lidarobjtostr(lidarobjvec,ntlen);
+    iv::modulecomm::ModuleSendMsg(gpdetect,out.data(),out.length());
+  //  delete strout;
+
+//    int i;
+//    for(i=0;i<objvec.size();i++)
+//    {
+//        Obstacle x;
+//        x = objvec.at(i);
+
+//        std::cout<<"obj "<<i<<" x is "<<x.GetTypeString()<<std::endl;
+
+//        iv::lidar::lidarobject y;
+//        y = lidarobjvec.at(i);
+//        std::cout<<"     "<<" x is"<<y.position.x<<" y is "<<y.position.y<<" len x is "<<y.dimensions.x<<" len y is "<<y.dimensions.y<<std::endl;
+//        std::cout<<"     "<<" type is "<<y.mnType<<std::endl;
+//    }
+}
+
+void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    if(nSize <=16)return;
+    unsigned int * pHeadSize = (unsigned int *)strdata;
+    if(*pHeadSize > nSize)
+    {
+        std::cout<<"ListenPointCloud data is small headsize ="<<*pHeadSize<<"  data size is"<<nSize<<std::endl;
+    }
+
+    QTime xTime;
+    xTime.start();
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+    int nNameSize;
+    nNameSize = *pHeadSize - 4-4-8;
+    char * strName = new char[nNameSize+1];strName[nNameSize] = 0;
+    memcpy(strName,(char *)((char *)strdata +4),nNameSize);
+    point_cloud->header.frame_id = strName;
+    memcpy(&point_cloud->header.seq,(char *)strdata+4+nNameSize,4);
+    memcpy(&point_cloud->header.stamp,(char *)strdata+4+nNameSize+4,8);
+    int nPCount = (nSize - *pHeadSize)/sizeof(pcl::PointXYZI);
+    int i;
+    pcl::PointXYZI * p;
+    p = (pcl::PointXYZI *)((char *)strdata + *pHeadSize);
+    for(i=0;i<nPCount;i++)
+    {
+        pcl::PointXYZI xp;
+//        if((p->x<-30)||(p->x>30)||(p->y>50)||(p->y<-50))
+//        {
+
+//        }
+//        else
+//        {
+            memcpy(&xp,p,sizeof(pcl::PointXYZI));
+            xp.z = xp.z + (-1.0) ;
+            point_cloud->push_back(xp);
+
+//        }
+        p++;
+//        xp.x = p->x;
+//        xp.y = p->y;
+//        xp.z = p->z;
+//        xp.intensity = p->intensity;
+//        point_cloud->push_back(xp);
+//        p++;
+    }
+
+//    std::cout<<"pcl time is "<<xTime.elapsed()<<std::endl;
+
+    DectectOnePCD(point_cloud);
+    std::cout<<"time is "<<(QDateTime::currentMSecsSinceEpoch() % 1000)<<" "<<xTime.elapsed()<<std::endl;
+
+}
+
+
+
 //LidarApolloInstanceSegmentation
 
 int main(int argc, char *argv[])
@@ -58,7 +350,23 @@ int main(int argc, char *argv[])
 
     gseg = new LidarApolloInstanceSegmentation();
 
-    test();
+ //   test();
+
+    QString strpath = QCoreApplication::applicationDirPath();
+    if(argc < 2)
+        strpath = strpath + "/detection_lidar_cnn_segmentation.xml";
+    else
+        strpath = argv[1];
+    std::cout<<strpath.toStdString()<<std::endl;
+
+    iv::xmlparam::Xmlparam xparam(strpath.toStdString());
+
+    gstrinput = xparam.GetParam("input","lidarpc_center");
+    gstroutput = xparam.GetParam("output","lidar_cnn_dectect");
+
+
+    gpa = iv::modulecomm::RegisterRecv(gstrinput.data(),ListenPointCloud);
+    gpdetect = iv::modulecomm::RegisterSend(gstroutput.data(),10000000,1);
 
     return a.exec();
 }

src/detection/detection_lidar_cnn_segmentation_trt/src/cluster2d.cpp

@@ -254,47 +254,41 @@ void Cluster2D::getObjectVector(const float confidence_thresh,
                                 const float height_thresh,
                                 const int min_pts_num,std::vector<Obstacle> & objvec)
 {
-//    CHECK(valid_indices_in_pc_ != nullptr);
-
-//    for (size_t i = 0; i < point2grid_.size(); ++i)
-//    {
-//        int grid = point2grid_[i];
-//        if (grid < 0)
-//        {
-//            continue;
-//        }
-
-//        CHECK_GE(grid, 0);
-//        CHECK_LT(grid, grids_);
-//        int obstacle_id = id_img_[grid];
-
-//        int point_id = valid_indices_in_pc_->at(i);
-//        CHECK_GE(point_id, 0);
-//        CHECK_LT(point_id, static_cast<int>(pc_ptr_->size()));
-
-//        if (obstacle_id >= 0 &&
-//            obstacles_[obstacle_id].score >= confidence_thresh)
-//        {
-////            if (height_thresh < 0 ||
-////                pc_ptr_->points[point_id].z <=
-////                obstacles_[obstacle_id].height + height_thresh)
-////            {
-//                obstacles_[obstacle_id].cloud_ptr->push_back(pc_ptr_->points[point_id]);
-// //           }
-//        }
-//    }
+    for (size_t i = 0; i < point2grid_.size(); ++i) {
+        int grid = point2grid_[i];
+        if (grid < 0) {
+            continue;
+        }
 
-//    for (size_t obstacle_id = 0; obstacle_id < obstacles_.size();
-//         obstacle_id++)
-//    {
-//        Obstacle *obs = &obstacles_[obstacle_id];
-//        if (static_cast<int>(obs->cloud_ptr->size()) < min_pts_num)
-//        {
-//            continue;
-//        }
+        int obstacle_id = id_img_[grid];
 
-//        objvec.push_back(*obs);
-//    }
+        int point_id = valid_indices_in_pc_->at(i);
+
+        if (obstacle_id >= 0 && obstacles_[obstacle_id].score >= confidence_thresh) {
+ //           std::cout<<obstacle_id<<" type: " <<obstacles_[obstacle_id].GetTypeString()<<std::endl;
+            if (
+                    height_thresh < 0 ||
+                    pc_ptr_->points[point_id].z <= obstacles_[obstacle_id].height + height_thresh) {
+                obstacles_[obstacle_id].cloud_ptr->push_back(pc_ptr_->points[point_id]);
+            }
+        }
+    }
+
+    std::cout<<"obs size: "<<obstacles_.size()<<std::endl;
+    for (size_t obstacle_id = 0; obstacle_id < obstacles_.size(); obstacle_id++) {
+        Obstacle * obs = &obstacles_[obstacle_id];
+
+        if (static_cast<int>(obs->cloud_ptr->size()) < min_pts_num) {
+            continue;
+        }
+        objvec.push_back(*obs);
+        std::cout<<obstacle_id<<" type: " <<obs->GetTypeString()<< "pos:"<<obs->cloud_ptr->points.at(0).x<<" " <<obs->cloud_ptr->points.at(0).y<<std::endl;
+
+
+//        tier4_perception_msgs::msg::DetectedObjectWithFeature out_obj =
+//                obstacleToObject(*obs, in_header);
+//        objects.feature_objects.push_back(out_obj);
+    }
 
 }
 

src/detection/detection_lidar_cnn_segmentation_trt/src/detector.cpp

@@ -18,6 +18,8 @@
 
 #include <NvCaffeParser.h>
 #include <NvInfer.h>
+
+#include <chrono>
 //#include <pcl_conversions/pcl_conversions.h>
 
 LidarApolloInstanceSegmentation::LidarApolloInstanceSegmentation()
@@ -27,10 +29,10 @@ LidarApolloInstanceSegmentation::LidarApolloInstanceSegmentation()
   std::string engine_file;
   std::string prototxt_file;
   std::string caffemodel_file;
-  score_threshold_ = 0.8;//node_->declare_parameter("score_threshold", 0.8);
+  score_threshold_ = 0.4;//node_->declare_parameter("score_threshold", 0.8);
   range = 60;//node_->declare_parameter("range", 60);
-  width = 640;//node_->declare_parameter("width", 640);
-  height = 640;//node_->declare_parameter("height", 640);
+  width = 512;// 640;//node_->declare_parameter("width", 640);
+  height = 512; //640;//node_->declare_parameter("height", 640);
   engine_file =  "deploy.engine";//node_->declare_parameter("engine_file", "vls-128.engine");
   prototxt_file = "hdl-64.prototxt";//node_->declare_parameter("prototxt_file", "vls-128.prototxt");
   caffemodel_file = "deploy.caffemodel";// node_->declare_parameter("caffemodel_file", "vls-128.caffemodel");
@@ -106,14 +108,22 @@ bool LidarApolloInstanceSegmentation::detectDynamicObjects(
    const pcl::PointCloud<pcl::PointXYZI>::Ptr &pcl_pointcloud_raw_ptr,
    std::vector<Obstacle> & objvec)
 {
+
+    int64_t time1 = std::chrono::system_clock::now().time_since_epoch().count();
     // generate feature map
     std::shared_ptr<FeatureMapInterface> feature_map_ptr =
       feature_generator_->generate(pcl_pointcloud_raw_ptr);
 
+   int64_t time2 = std::chrono::system_clock::now().time_since_epoch().count();
     // inference
     std::shared_ptr<float> inferred_data(new float[net_ptr_->getOutputSize() / sizeof(float)]);
     net_ptr_->doInference(feature_map_ptr->map_data.data(), inferred_data.get());
 
+    int64_t time3 = std::chrono::system_clock::now().time_since_epoch().count();
+
+    std::cout<<"feature time:"<<(time2 - time1)/1000000<<" inference time: "<<(time3-time2)/1000000<<std::endl;
+
+ //   return true;
     // post process
     const float objectness_thresh = 0.5;
     pcl::PointIndices valid_idx;
@@ -124,6 +134,9 @@ bool LidarApolloInstanceSegmentation::detectDynamicObjects(
       true /*use all grids for clustering*/);
     const float height_thresh = 0.5;
     const int min_pts_num = 3;
+
+    cluster2d_->getObjectVector(
+                score_threshold_, height_thresh, min_pts_num, objvec);
 //    cluster2d_->getObjects(
 //      score_threshold_, height_thresh, min_pts_num, output, transformed_cloud.header);