change detection_lidar_cnn_segmentation.

src/detection/detection_lidar_cnn_segmentation/cluster2d.cpp

@@ -44,6 +44,186 @@ void Cluster2D::traverse(Node *x)
     }
 }
 
+void Cluster2D::cluster(const float * category_pt_data,
+             const float *instance_pt_x_data,
+             const float *instance_pt_y_data,
+             const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr,
+             const pcl::PointIndices &valid_indices,
+             float objectness_thresh, bool use_all_grids_for_clustering)
+{
+    pc_ptr_ = pc_ptr;
+    std::vector<std::vector<Node>> nodes(rows_,
+                                         std::vector<Node>(cols_, Node()));
+
+    // map points into grids
+    size_t tot_point_num = pc_ptr_->size();
+    valid_indices_in_pc_ = &(valid_indices.indices);
+    CHECK_LE(valid_indices_in_pc_->size(), tot_point_num);
+    point2grid_.assign(valid_indices_in_pc_->size(), -1);
+
+    int64 time1 = std::chrono::system_clock::now().time_since_epoch().count();
+    for (size_t i = 0; i < valid_indices_in_pc_->size(); ++i)
+    {
+        int point_id = valid_indices_in_pc_->at(i);
+        CHECK_GE(point_id, 0);
+        CHECK_LT(point_id, static_cast<int>(tot_point_num));
+        const auto &point = pc_ptr_->points[point_id];
+        // * the coordinates of x and y have been exchanged in feature generation
+        // step,
+        // so we swap them back here.
+        int pos_x = F2I(point.y, range_, inv_res_x_);  // col
+        int pos_y = F2I(point.x, range_, inv_res_y_);  // row
+        if (IsValidRowCol(pos_y, pos_x))
+        {
+            // get grid index and count point number for corresponding node
+            point2grid_[i] = RowCol2Grid(pos_y, pos_x);
+            nodes[pos_y][pos_x].point_num++;
+        }
+    }
+
+    int64 time2 = std::chrono::system_clock::now().time_since_epoch().count();
+    // construct graph with center offset prediction and objectness
+    for (int row = 0; row < rows_; ++row)
+    {
+        for (int col = 0; col < cols_; ++col)
+        {
+            int grid = RowCol2Grid(row, col);
+            Node *node = &nodes[row][col];
+            DisjointSetMakeSet(node);
+            node->is_object =
+                    (use_all_grids_for_clustering || nodes[row][col].point_num > 0) &&
+                    (*(category_pt_data + grid) >= objectness_thresh);
+            int center_row = std::round(row + instance_pt_x_data[grid] * scale_);
+            int center_col = std::round(col + instance_pt_y_data[grid] * scale_);
+            center_row = std::min(std::max(center_row, 0), rows_ - 1);
+            center_col = std::min(std::max(center_col, 0), cols_ - 1);
+            node->center_node = &nodes[center_row][center_col];
+        }
+    }
+
+    int64 time3 = std::chrono::system_clock::now().time_since_epoch().count();
+    // traverse nodes
+    for (int row = 0; row < rows_; ++row)
+    {
+        for (int col = 0; col < cols_; ++col)
+        {
+            Node *node = &nodes[row][col];
+            if (node->is_object && node->traversed == 0)
+            {
+                traverse(node);
+            }
+        }
+    }
+
+    int64 time4 = std::chrono::system_clock::now().time_since_epoch().count();
+    for (int row = 0; row < rows_; ++row)
+    {
+        for (int col = 0; col < cols_; ++col)
+        {
+            Node *node = &nodes[row][col];
+            if (!node->is_center)
+            {
+                continue;
+            }
+            for (int row2 = row - 1; row2 <= row + 1; ++row2)
+            {
+                for (int col2 = col - 1; col2 <= col + 1; ++col2)
+                {
+                    if ((row2 == row || col2 == col) && IsValidRowCol(row2, col2))
+                    {
+                        Node *node2 = &nodes[row2][col2];
+                        if (node2->is_center)
+                        {
+                            DisjointSetUnion(node, node2);
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    int64 time5 = std::chrono::system_clock::now().time_since_epoch().count();
+    int count_obstacles = 0;
+    obstacles_.clear();
+    id_img_.assign(grids_, -1);
+    for (int row = 0; row < rows_; ++row)
+    {
+        for (int col = 0; col < cols_; ++col)
+        {
+            Node *node = &nodes[row][col];
+            if (!node->is_object)
+            {
+                continue;
+            }
+            Node *root = DisjointSetFind(node);
+            if (root->obstacle_id < 0)
+            {
+                root->obstacle_id = count_obstacles++;
+                CHECK_EQ(static_cast<int>(obstacles_.size()), count_obstacles - 1);
+                obstacles_.push_back(Obstacle());
+            }
+            int grid = RowCol2Grid(row, col);
+            CHECK_GE(root->obstacle_id, 0);
+            id_img_[grid] = root->obstacle_id;
+            obstacles_[root->obstacle_id].grids.push_back(grid);
+        }
+    }
+    CHECK_EQ(static_cast<size_t>(count_obstacles), obstacles_.size());
+    int64 time6 = std::chrono::system_clock::now().time_since_epoch().count();
+
+//    std::cout<<" t1: "<<(time2 - time1)/1000<<" t2: "<<(time3 - time2)/1000<<" t3: "<<(time4 - time3)/1000
+//            <<" t4: "<<(time5 - time4)/1000<<" t5: "<<(time6 - time5)/1000<<std::endl;
+}
+
+void Cluster2D::filter(const float *confidence_pt_data,
+            const float *height_pt_data)
+{
+    for (size_t obstacle_id = 0; obstacle_id < obstacles_.size();
+         obstacle_id++)
+    {
+        Obstacle *obs = &obstacles_[obstacle_id];
+        CHECK_GT(obs->grids.size(), 0);
+        double score = 0.0;
+        double height = 0.0;
+        for (int grid : obs->grids)
+        {
+            score += static_cast<double>(confidence_pt_data[grid]);
+            height += static_cast<double>(height_pt_data[grid]);
+        }
+        obs->score = score / static_cast<double>(obs->grids.size());
+        obs->height = height / static_cast<double>(obs->grids.size());
+        obs->cloud_ptr.reset(new pcl::PointCloud<pcl::PointXYZI>);
+    }
+}
+
+void Cluster2D::classify(const float *classify_pt_data,int num_classes )
+{
+    CHECK_EQ(num_classes, MAX_META_TYPE);
+    for (size_t obs_id = 0; obs_id < obstacles_.size(); obs_id++)
+    {
+        Obstacle *obs = &obstacles_[obs_id];
+        for (size_t grid_id = 0; grid_id < obs->grids.size(); grid_id++)
+        {
+            int grid = obs->grids[grid_id];
+            for (int k = 0; k < num_classes; k++)
+            {
+                obs->meta_type_probs[k] += classify_pt_data[k * grids_ + grid];
+            }
+        }
+        int meta_type_id = 0;
+        for (int k = 0; k < num_classes; k++)
+        {
+            obs->meta_type_probs[k] /= obs->grids.size();
+            if (obs->meta_type_probs[k] > obs->meta_type_probs[meta_type_id])
+            {
+                meta_type_id = k;
+            }
+        }
+        obs->meta_type = static_cast<MetaType>(meta_type_id);
+    }
+}
+
+#include <chrono>
 void Cluster2D::cluster(const caffe::Blob<float> &category_pt_blob,
                         const caffe::Blob<float> &instance_pt_blob,
                         const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr,
@@ -55,6 +235,7 @@ void Cluster2D::cluster(const caffe::Blob<float> &category_pt_blob,
     const float *instance_pt_y_data =
             instance_pt_blob.cpu_data() + instance_pt_blob.offset(0, 1);
 
+
     pc_ptr_ = pc_ptr;
     std::vector<std::vector<Node>> nodes(rows_,
                                          std::vector<Node>(cols_, Node()));
@@ -65,6 +246,7 @@ void Cluster2D::cluster(const caffe::Blob<float> &category_pt_blob,
     CHECK_LE(valid_indices_in_pc_->size(), tot_point_num);
     point2grid_.assign(valid_indices_in_pc_->size(), -1);
 
+    int64 time1 = std::chrono::system_clock::now().time_since_epoch().count();
     for (size_t i = 0; i < valid_indices_in_pc_->size(); ++i)
     {
         int point_id = valid_indices_in_pc_->at(i);
@@ -84,6 +266,7 @@ void Cluster2D::cluster(const caffe::Blob<float> &category_pt_blob,
         }
     }
 
+    int64 time2 = std::chrono::system_clock::now().time_since_epoch().count();
     // construct graph with center offset prediction and objectness
     for (int row = 0; row < rows_; ++row)
     {
@@ -103,6 +286,7 @@ void Cluster2D::cluster(const caffe::Blob<float> &category_pt_blob,
         }
     }
 
+    int64 time3 = std::chrono::system_clock::now().time_since_epoch().count();
     // traverse nodes
     for (int row = 0; row < rows_; ++row)
     {
@@ -115,6 +299,8 @@ void Cluster2D::cluster(const caffe::Blob<float> &category_pt_blob,
             }
         }
     }
+
+    int64 time4 = std::chrono::system_clock::now().time_since_epoch().count();
     for (int row = 0; row < rows_; ++row)
     {
         for (int col = 0; col < cols_; ++col)
@@ -141,6 +327,7 @@ void Cluster2D::cluster(const caffe::Blob<float> &category_pt_blob,
         }
     }
 
+    int64 time5 = std::chrono::system_clock::now().time_since_epoch().count();
     int count_obstacles = 0;
     obstacles_.clear();
     id_img_.assign(grids_, -1);
@@ -167,6 +354,10 @@ void Cluster2D::cluster(const caffe::Blob<float> &category_pt_blob,
         }
     }
     CHECK_EQ(static_cast<size_t>(count_obstacles), obstacles_.size());
+    int64 time6 = std::chrono::system_clock::now().time_since_epoch().count();
+
+    std::cout<<" t1: "<<(time2 - time1)/1000<<" t2: "<<(time3 - time2)/1000<<" t3: "<<(time4 - time3)/1000
+            <<" t4: "<<(time5 - time4)/1000<<" t5: "<<(time6 - time5)/1000<<std::endl;
 }
 
 void Cluster2D::filter(const caffe::Blob<float> &confidence_pt_blob,
@@ -194,7 +385,10 @@ void Cluster2D::filter(const caffe::Blob<float> &confidence_pt_blob,
 
 void Cluster2D::classify(const caffe::Blob<float> &classify_pt_blob)
 {
+//    int64_t time1 = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
     const float *classify_pt_data = classify_pt_blob.cpu_data();
+//    int64_t time2 = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+//    std::cout<<"cpu data use: "<<(time2-time1)<<std::endl;
     int num_classes = classify_pt_blob.channels();
     CHECK_EQ(num_classes, MAX_META_TYPE);
     for (size_t obs_id = 0; obs_id < obstacles_.size(); obs_id++)

src/detection/detection_lidar_cnn_segmentation/cluster2d.h

@@ -93,6 +93,10 @@ public:
                const pcl::PointIndices &valid_indices,
                float objectness_thresh, bool use_all_grids_for_clustering);
 
+
+
+
+
   void filter(const caffe::Blob<float> &confidence_pt_blob,
               const caffe::Blob<float> &height_pt_blob);
 
@@ -102,6 +106,18 @@ public:
                       const float height_thresh,
                       const int min_pts_num,std::vector<Obstacle> & objvec);
 
+ void cluster(const float * category_pt_data,
+              const float *instance_pt_x_data,
+              const float *instance_pt_y_data,
+              const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr,
+              const pcl::PointIndices &valid_indices,
+              float objectness_thresh, bool use_all_grids_for_clustering);
+
+ void filter(const float *confidence_pt_data,
+             const float *height_pt_data);
+
+ void classify(const float *classify_pt_data,int num_classes );
+
 //  void getObjects(const float confidence_thresh,
 //                  const float height_thresh,
 //                  const int min_pts_num,

src/detection/detection_lidar_cnn_segmentation/cnn_segmentation.cpp

@@ -1,7 +1,14 @@
 #include "cnn_segmentation.h"
 
+
+
 CNNSegmentation::CNNSegmentation()
 {
+
+    minferbuffer.mbNew = false;
+    mbThreadRun = true;
+    mcnnres.mbNew = false;
+
 }
 
 
@@ -80,15 +87,99 @@ bool CNNSegmentation::init(std::string proto_file,std::string weight_file,double
      }
 
      feature_generator_.reset(new FeatureGenerator());
-     if (!feature_generator_->init(feature_blob_.get(), use_constant_feature_))
+     if (!feature_generator_->init(feature_blob_.get(), use_constant_feature_,range_,width_,height_))
      {
   //     ROS_ERROR("[%s] Fail to Initialize feature generator for CNNSegmentation", __APP_NAME__);
        return false;
      }
 
+
+ int i;
+ for(i=0;i<3;i++)
+ {
+     std::thread * pt = new std::thread(&CNNSegmentation::threadCluster,this);
+     mpthreadv.push_back(pt);
+ }
+
   return true;
 }
 
+void CNNSegmentation::threadCluster()
+{
+    std::shared_ptr<Cluster2D> xcluster2d;
+
+    xcluster2d.reset(new Cluster2D());
+    if (!xcluster2d->init(width_,height_,range_))
+//    if (!cluster2d_->init(512,512,60))
+    {
+        std::cout<<" CNNSegmentation::threadCluster "<<" cluster2d init fail."<<std::endl;
+ //     ROS_ERROR("[%s] Fail to Initialize cluster2d for CNNSegmentation", __APP_NAME__);
+      return ;
+    }
+    float objectness_thresh = 0.5;
+    bool use_all_grids_for_clustering = true;
+    float confidence_thresh = score_threshold_;
+    float height_thresh = 3.5;// 0.5;//3.5;//0.5;
+    int min_pts_num = 3;
+    while(mbThreadRun)
+    {
+        bool bNew = false;
+        iv::Cluster2DData xCluster2DData;
+        mmutexbuffer.lock();
+        if(minferbuffer.mbNew)
+        {
+            xCluster2DData = minferbuffer.mData;
+            minferbuffer.mbNew = false;
+            bNew = true;
+        }
+        mmutexbuffer.unlock();
+        if(bNew == false)
+        {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+            continue;
+        }
+
+        xcluster2d->cluster(xCluster2DData.mcategory_pt_data_ptr.get(),
+                             xCluster2DData.minstance_pt_x_data_ptr.get(),
+                             xCluster2DData.minstance_pt_y_data_ptr.get(),
+                             xCluster2DData.mpointcloud,
+                             *xCluster2DData.mvalid_idx_ptr, objectness_thresh,
+                             use_all_grids_for_clustering);
+        xcluster2d->filter(xCluster2DData.mconfidence_pt_data_ptr.get(),xCluster2DData.mheight_pt_data_ptr.get());
+        xcluster2d->classify(xCluster2DData.mclassify_pt_data_ptr.get(),xCluster2DData.mnum_classes);
+
+        int64 nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+        std::cout<<"time "<<xCluster2DData.mPointCloudUpdateTime<<" infer use: "<<(xCluster2DData.mInferUpdateTime - xCluster2DData.mPointCloudUpdateTime)
+                <<" cluster use: "<<(nnow - xCluster2DData.mInferUpdateTime)<<" total use:"<<(nnow - xCluster2DData.mPointCloudUpdateTime)<<std::endl;
+//        xcluster2d_->filter(*confidence_pt_blob_, *height_pt_blob_);
+//        xcluster2d_->classify(*class_pt_blob_);
+
+        std::vector<Obstacle> objvec;
+        xcluster2d->getObjectVector(confidence_thresh, height_thresh, min_pts_num,objvec);
+
+        mmutexcnnres.lock();
+        mcnnres.mobjvec = objvec;
+        mcnnres.mcluster2ddata = xCluster2DData;
+        mcnnres.mbNew = true;
+        mmutexcnnres.unlock();
+
+    }
+}
+
+int CNNSegmentation::GetRes(iv::cnnres & xres)
+{
+    int nrtn = 0;
+    mmutexcnnres.lock();
+    if(mcnnres.mbNew)
+    {
+        xres = mcnnres;
+        mcnnres.mbNew = false;
+        nrtn = 1;
+    }
+    mmutexcnnres.unlock();
+    return nrtn;
+}
+
 bool CNNSegmentation::segment(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr,
                               const pcl::PointIndices &valid_idx,
                               std::vector<Obstacle> & objvec)
@@ -110,8 +201,13 @@ bool CNNSegmentation::segment(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr
 
     int64_t time3 = std::chrono::system_clock::now().time_since_epoch().count();
 
+
+
     float objectness_thresh = 0.5;
     bool use_all_grids_for_clustering = true;
+
+//    boost::shared_ptr<caffe::Blob<float>> xinstance_pt_blob = boost::shared_ptr<caffe::Blob<float>>(new caffe::Blob<float>());
+//    xinstance_pt_blob->CopyFrom(*instance_pt_blob_);
     cluster2d_->cluster(*category_pt_blob_, *instance_pt_blob_, pc_ptr,
                         valid_idx, objectness_thresh,
                         use_all_grids_for_clustering);
@@ -120,10 +216,13 @@ bool CNNSegmentation::segment(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr
     float confidence_thresh = score_threshold_;
     float height_thresh = 3.5;// 0.5;//3.5;//0.5;
     int min_pts_num = 3;
+    int64_t time4 = std::chrono::system_clock::now().time_since_epoch().count();
 
     cluster2d_->getObjectVector(confidence_thresh, height_thresh, min_pts_num,objvec);
 
-    int64_t time4 = std::chrono::system_clock::now().time_since_epoch().count();
+
+
+
 
 
     std::cout<<"infer time: "<<(time3-time2)/1000000<<" generate: "<<(time2-time1)/1000000<<" cluster: "<<(time4-time3)/1000000<<std::endl;
@@ -137,6 +236,94 @@ bool CNNSegmentation::segment(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr
 
 }
 
+
+bool CNNSegmentation::segment(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr,
+                              pcl::PointIndicesPtr & valid_idx_ptr)
+{
+
+    int64 npctime = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+    if(use_gpu_)
+    {
+        caffe::Caffe::SetDevice(gpu_device_id_);
+        caffe::Caffe::set_mode(caffe::Caffe::GPU);
+    }
+
+    int64_t time1 = std::chrono::system_clock::now().time_since_epoch().count();
+
+    feature_generator_->generate(pc_ptr);
+
+    int64_t time2 = std::chrono::system_clock::now().time_since_epoch().count();
+
+    caffe_net_->Forward();
+
+    int64_t time3 = std::chrono::system_clock::now().time_since_epoch().count();
+
+
+
+    float objectness_thresh = 0.5;
+    bool use_all_grids_for_clustering = true;
+
+
+    iv::Cluster2DData xCluster2DData;
+
+    int nSize = width_*height_;
+    xCluster2DData.mcategory_pt_data_ptr = std::shared_ptr<float>(new float[nSize]);
+    xCluster2DData.minstance_pt_x_data_ptr = std::shared_ptr<float>(new float[nSize]);
+    xCluster2DData.minstance_pt_y_data_ptr = std::shared_ptr<float>(new float[nSize]);
+    xCluster2DData.mconfidence_pt_data_ptr = std::shared_ptr<float>(new float[nSize]);
+    xCluster2DData.mheight_pt_data_ptr = std::shared_ptr<float>(new float[nSize]);
+    xCluster2DData.mnum_classes = class_pt_blob_->channels();
+    xCluster2DData.mclassify_pt_data_ptr = std::shared_ptr<float>(new float[nSize * xCluster2DData.mnum_classes]);
+
+    int64_t time4 = std::chrono::system_clock::now().time_since_epoch().count();
+
+    memcpy(xCluster2DData.mcategory_pt_data_ptr.get(),category_pt_blob_->cpu_data(),nSize * sizeof(float) );
+    memcpy(xCluster2DData.minstance_pt_x_data_ptr.get(),instance_pt_blob_->cpu_data(),nSize* sizeof(float));
+    memcpy(xCluster2DData.minstance_pt_y_data_ptr.get(),instance_pt_blob_->cpu_data()+instance_pt_blob_->offset(0, 1),nSize* sizeof(float));
+    memcpy(xCluster2DData.mconfidence_pt_data_ptr.get(),confidence_pt_blob_->cpu_data(),nSize* sizeof(float));
+    memcpy(xCluster2DData.mheight_pt_data_ptr.get(),height_pt_blob_->cpu_data(),nSize* sizeof(float));
+    memcpy(xCluster2DData.mclassify_pt_data_ptr.get(),class_pt_blob_->cpu_data(),nSize*xCluster2DData.mnum_classes* sizeof(float));
+    xCluster2DData.mInferUpdateTime = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+    xCluster2DData.mpointcloud = pc_ptr;
+    xCluster2DData.mvalid_idx_ptr = valid_idx_ptr;
+    xCluster2DData.mPointCloudUpdateTime = npctime;
+
+    int64_t time5 = std::chrono::system_clock::now().time_since_epoch().count();
+
+    mmutexbuffer.lock();
+    minferbuffer.mData = xCluster2DData;
+    minferbuffer.mbNew = true;
+    mmutexbuffer.unlock();
+
+    std::cout<<" feature use: "<<(time2-time1)/1000000<<" infer use: "<<(time3 - time2)/1000000
+            <<" copy use:"<<(time5-time4)/1000000<<std::endl;
+
+//    cluster2d_->cluster(*category_pt_blob_, *instance_pt_blob_, pc_ptr,
+//                        valid_idx, objectness_thresh,
+//                        use_all_grids_for_clustering);
+//    cluster2d_->filter(*confidence_pt_blob_, *height_pt_blob_);
+//    cluster2d_->classify(*class_pt_blob_);
+//    float confidence_thresh = score_threshold_;
+//    float height_thresh = 3.5;// 0.5;//3.5;//0.5;
+//    int min_pts_num = 3;
+
+//    cluster2d_->getObjectVector(confidence_thresh, height_thresh, min_pts_num,objvec);
+
+//    int64_t time4 = std::chrono::system_clock::now().time_since_epoch().count();
+
+
+ //   std::cout<<"infer time: "<<(time3-time2)/1000000<<" generate: "<<(time2-time1)/1000000<<" cluster: "<<(time4-time3)/1000000<<std::endl;
+
+//    cluster2d_->getObjects(confidence_thresh, height_thresh, min_pts_num,
+//                           objects, message_header_);
+
+
+
+    return true;
+
+}
+
+
 //bool CNNSegmentation::segment(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr,
 //                              const pcl::PointIndices &valid_idx,
 //                              rockauto_msgs::DetectedObjectArray &objects)

src/detection/detection_lidar_cnn_segmentation/cnn_segmentation.h

@@ -26,6 +26,9 @@
 
 #define __APP_NAME__ "lidar_cnn_seg_detect"
 
+
+#include <thread>
+
 struct CellStat
 {
   CellStat() : point_num(0), valid_point_num(0)
@@ -36,6 +39,47 @@ struct CellStat
   int valid_point_num;
 };
 
+
+namespace iv {
+struct Cluster2DData
+{
+    std::shared_ptr<float>  mcategory_pt_data_ptr;
+    std::shared_ptr<float>  minstance_pt_x_data_ptr;
+    std::shared_ptr<float>  minstance_pt_y_data_ptr;
+    std::shared_ptr<float>  mconfidence_pt_data_ptr;
+    std::shared_ptr<float>  mheight_pt_data_ptr;
+    std::shared_ptr<float>  mclassify_pt_data_ptr;
+    int mnum_classes;
+    int64 mPointCloudUpdateTime; //ms since epoch
+    int64 mInferUpdateTime; //ms since epoch
+    pcl::PointCloud<pcl::PointXYZI>::Ptr mpointcloud;
+    pcl::PointIndicesPtr mvalid_idx_ptr;
+
+};
+
+}
+
+namespace iv {
+struct pcbuffer
+{
+    pcl::PointCloud<pcl::PointXYZI>::Ptr mpointcloud;
+    bool mbNew;
+    int64 mPointCloudUpdateTime; //ms since epoch
+};
+struct inferresbuffer
+{
+    Cluster2DData mData;
+    bool mbNew;
+};
+
+struct cnnres
+{
+    std::vector<Obstacle> mobjvec;
+    Cluster2DData mcluster2ddata;
+    bool mbNew;
+};
+}
+
 class CNNSegmentation
 {
 public:
@@ -78,6 +122,20 @@ private:
   // bird-view raw feature generator
   std::shared_ptr<FeatureGenerator> feature_generator_;
 
+private:
+  void threadCluster();
+
+  iv::inferresbuffer minferbuffer;
+  std::mutex mmutexbuffer;
+
+  iv::cnnres mcnnres;
+  std::mutex mmutexcnnres;
+
+  bool mbThreadRun;
+
+  std::vector<std::thread *> mpthreadv;
+
+
 
 public:
   bool init(  std::string proto_file,std::string weight_file,double rangemax,double score,
@@ -86,6 +144,10 @@ public:
   bool segment(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr,
                const pcl::PointIndices &valid_idx,
                std::vector<Obstacle> & objvec);
+  bool segment(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr,
+               pcl::PointIndicesPtr & valid_idx_ptr);
+
+  int GetRes(iv::cnnres & xres);
 
 
 //  bool segment(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr,

src/detection/detection_lidar_cnn_segmentation/detection_lidar_cnn_segmentation.xml

@@ -0,0 +1,11 @@
+<xml>	
+	<node name="detection_ndt_matching_gpu_multi.xml">
+		<param name="prototxt" value="/home/nvidia/models/lidar/model.prototxt" />
+		<param name="caffemodel" value="/home/nvidia/models/lidar/model.caffemodel" />
+		<param name="input" value="lidarpc_center" />
+		<param name="output" value="lidar_track" />
+		<param name="range" value="30.0" />
+		<param name="width" value="512" />
+		<param name="height" value="512" />
+	</node>
+</xml>

src/detection/detection_lidar_cnn_segmentation/feature_generator.cpp

@@ -1,14 +1,14 @@
 #include "feature_generator.h"
 
 
-bool FeatureGenerator::init(caffe::Blob<float>* out_blob, bool use_constant_feature)
+bool FeatureGenerator::init(caffe::Blob<float>* out_blob, bool use_constant_feature,double range,int width,int height)
 {
   out_blob_ = out_blob;
 
   // raw feature parameters
-  range_ = 40;
-  width_ = 512;
-  height_ = 512;
+  range_ = range;
+  width_ = width;
+  height_ = height;
   min_height_ = -5.0;
   max_height_ = 5.0;
   CHECK_EQ(width_, height_)

src/detection/detection_lidar_cnn_segmentation/feature_generator.h

@@ -16,7 +16,7 @@ public:
   FeatureGenerator(){}
   ~FeatureGenerator(){}
 
-  bool init(caffe::Blob<float>* out_blob, bool use_constant_feature);
+  bool init(caffe::Blob<float>* out_blob, bool use_constant_feature,double range,int width,int height);
   void generate(
       const pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_ptr);
 private:

src/detection/detection_lidar_cnn_segmentation/main.cpp

@@ -19,6 +19,12 @@
 //#include "alog.h"
 
 CNNSegmentation gcnn;
+CNNSegmentation gcnnv[10];
+
+
+
+iv::pcbuffer gpcbuffer;
+std::mutex gmutexpcbuffer;
 
 void * gpa;
 void * gpdetect;
@@ -30,6 +36,8 @@ std::thread * gpthread;
 std::string gstrinput;
 std::string gstroutput;
 
+bool gbUseMultiCNN = false;
+
 static iv::lidar::PointXYZ pcltoprotopoint(pcl::PointXYZ x)
 {
     iv::lidar::PointXYZ lx;
@@ -245,48 +253,117 @@ static void GetLidarObj(std::vector<Obstacle> objvec,iv::lidar::objectarray & li
 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;
 
-    gcnn.segment(pc_ptr, valid_idx, objvec);
-    givlog->verbose("obj size is %d", objvec.size());
-    std::cout<<"obj size is "<<objvec.size()<<std::endl;
+    bool bUseMutliCluster = true;
 
-//    std::vector<iv::lidar::lidarobject> lidarobjvec;
-    iv::lidar::objectarray lidarobjvec;
-    GetLidarObj(objvec,lidarobjvec);
+    if(bUseMutliCluster == false)
+    {
+        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;
+        gcnn.segment(pc_ptr, valid_idx, objvec);
+        givlog->verbose("obj size is %d", objvec.size());
+        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());
+        givlog->verbose("lenth is %d",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;
+        //    }
 
-    double timex = pc_ptr->header.stamp;
-    timex = timex/1000.0;
-    lidarobjvec.set_timestamp(pc_ptr->header.stamp);
+    }
+    else
+    {
+        pcl::PointIndicesPtr valid_idx_ptr = boost::shared_ptr<pcl::PointIndices>(new pcl::PointIndices());
+        auto &indices = valid_idx_ptr->indices;
+        indices.resize(pc_ptr->size());
+        std::iota(indices.begin(), indices.end(), 0);
+        gcnn.segment(pc_ptr, valid_idx_ptr);
+    }
+}
 
-    int ntlen;
-    std::string out = lidarobjvec.SerializeAsString();
- //   char * strout = lidarobjtostr(lidarobjvec,ntlen);
-    iv::modulecomm::ModuleSendMsg(gpdetect,out.data(),out.length());
-    givlog->verbose("lenth is %d",out.length());
-  //  delete strout;
 
-//    int i;
-//    for(i=0;i<objvec.size();i++)
-//    {
-//        Obstacle x;
-//        x = objvec.at(i);
+void threadgetres()
+{
+    while(1)
+    {
+        iv::cnnres xcnnres;
+        int nrtn = gcnn.GetRes(xcnnres);
+        if(nrtn == 1)
+        {
+            iv::lidar::objectarray lidarobjvec;
+            GetLidarObj(xcnnres.mobjvec,lidarobjvec);
+            std::string out = lidarobjvec.SerializeAsString();
+            //   char * strout = lidarobjtostr(lidarobjvec,ntlen);
+            iv::modulecomm::ModuleSendMsg(gpdetect,out.data(),out.length());
+
+            int64 now =  std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+            std::cout<<" pc time : "<<xcnnres.mcluster2ddata.mPointCloudUpdateTime<<" cnn use: "<<(now - xcnnres.mcluster2ddata.mPointCloudUpdateTime)
+                    <<" obj size: "<<xcnnres.mobjvec.size()<<std::endl;
+        }
+        else
+        {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+    }
+}
 
-//        std::cout<<"obj "<<i<<" x is "<<x.GetTypeString()<<std::endl;
+void DectectOnePCD(const pcl::PointCloud<pcl::PointXYZI>::Ptr &pc_ptr,CNNSegmentation * pcnn)
+{
+    std::cout<<"pcd size is "<<pc_ptr->size()<<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;
-//    }
-}
 
 
+        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;
+        pcnn->segment(pc_ptr, valid_idx, objvec);
+        givlog->verbose("obj size is %d", objvec.size());
+        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());
+        givlog->verbose("lenth is %d",out.length());
+}
+
 int gnothavedatatime = 0;
 
 void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
@@ -341,12 +418,62 @@ void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsign
 
 //    std::cout<<"pcl time is "<<xTime.elapsed()<<std::endl;
 
-    DectectOnePCD(point_cloud);
-    std::cout<<"time is "<<(QDateTime::currentMSecsSinceEpoch() % 1000)<<" "<<xTime.elapsed()<<std::endl;
+    bool bUseMultiCNN = gbUseMultiCNN;
+    if(bUseMultiCNN)
+    {
+        gmutexpcbuffer.lock();
+        gpcbuffer.mbNew = true;
+        gpcbuffer.mpointcloud = point_cloud;
+        gpcbuffer.mPointCloudUpdateTime = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+        gmutexpcbuffer.unlock();
+    }
+    else
+    {
+        DectectOnePCD(point_cloud);
+    }
+
+//    std::cout<<"time is "<<(QDateTime::currentMSecsSinceEpoch() % 1000)<<" "<<xTime.elapsed()<<std::endl;
     gfault->SetFaultState(0, 0, "ok");
 
 }
 
+void threadcnn(std::string protofile,std::string weightfile,double range,int width,int height)
+{
+    CNNSegmentation cnn;
+    cnn.init(protofile,weightfile,60.0,0.6,1024,1024,false,true,0);
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud;
+
+    while(1)
+    {
+        bool bNew = false;
+        int64 npctime;
+        gmutexpcbuffer.lock();
+        if(gpcbuffer.mbNew)
+        {
+            gpcbuffer.mbNew = false;
+            bNew = true;
+            point_cloud = gpcbuffer.mpointcloud;
+            npctime = gpcbuffer.mPointCloudUpdateTime;
+        }
+        gmutexpcbuffer.unlock();
+
+        if(bNew == false)
+        {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+            continue;
+        }
+
+
+        DectectOnePCD(point_cloud,&cnn);
+        std::cout<<"pc time: is "<<npctime<<" res use time: "<<(std::chrono::system_clock::now().time_since_epoch().count()/1000000 - npctime)<<std::endl;
+
+
+    }
+
+
+}
+
 bool gbstate = true;
 void statethread()
 {
@@ -449,9 +576,37 @@ int main(int argc, char *argv[])
     gstrinput = xparam.GetParam("input","lidar_pc");
     gstroutput = xparam.GetParam("output","lidar_cnn_dectect");
 
+    double range,width,height;
+    range = xparam.GetParam("range",60.0);
+    width = xparam.GetParam("width",1024);
+    height = xparam.GetParam("height",1024);
+    int ncnn = xparam.GetParam("ncnn",2);
+
 //    std::string protofile = "/home/yuchuli/qt/bq/models/lidar/model.prototxt";
 //    std::string weightfile = "/home/yuchuli/qt/bq/models/lidar/model.caffemodel";
-    gcnn.init(protofile,weightfile,60.0,0.6,512,512,false,true,0);
+    gcnn.init(protofile,weightfile,range,0.6,width,height,false,true,0);
+
+//    int i;
+//    for(i=0;i<3;i++)
+//    {
+//        gcnnv[i].init(protofile,weightfile,60.0,0.6,1024,1024,false,true,0);
+
+//    }
+
+    std::thread * pthreadv[10];
+      if(gbUseMultiCNN)
+      {
+          int i;
+          if(ncnn>10)ncnn = 10;
+          for(i=0;i<ncnn;i++)
+          {
+              pthreadv[i] = new std::thread(threadcnn,protofile,weightfile,range,width,height);
+          }
+      }
+
+
+      std::thread * pthreadsend = new std::thread(threadgetres);
+