#10 feat: add guassian process

CMakeLists.txt

@@ -24,6 +24,13 @@ find_package(catkin REQUIRED COMPONENTS
     object_builders_lib
 )
 
+find_package(OpenMP)
+if (OPENMP_FOUND)
+    set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+    set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+    set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
+endif()
+
 ## System dependencies are found with CMake's conventions
 ### PCL
 find_package(PCL 1.7 REQUIRED COMPONENTS
@@ -75,6 +82,7 @@ add_library(${PROJECT_NAME}
     src/region_euclidean_segmenter.cpp
     src/region_growing_segmenter.cpp
     src/don_segmenter.cpp
+    src/GuassianProcess.cpp
 )
 
 ## http://mariobadr.com/creating-a-header-only-library-with-cmake.html

config/detection.yaml

@@ -1,7 +1,8 @@
 detect: {
   frame_id: "velodyne",
 
-  sub_pc_topic: "/kitti/points_raw",
+  #sub_pc_topic: "/kitti/points_raw",
+  sub_pc_topic: "/kitti/velo/pointcloud",
   sub_pc_queue_size: 1,
 
   pub_pc_ground_topic: "/segmenter/points_ground",
@@ -10,7 +11,7 @@ detect: {
 
   ## Important to use roi filter for "Ground remover"
   #use_roi_filter: false,
-  use_roi_filter: true,
+  use_roi_filter: false,
 
   ## Ground Segmenter
   # type: string

include/segmenters/GuassianProcess.h

@@ -0,0 +1,47 @@
+#ifndef _GUASSIANPROCESS_
+#define _GUASSIANPROCESS_
+
+#include <iostream>
+#include <vector>
+#include <math.h>
+#include <algorithm>
+#include <Eigen/Dense>
+#include <pcl/point_types.h>
+#include <malloc.h>
+#include "iterator"
+#include <omp.h>
+using namespace std;
+using namespace Eigen;
+
+#define mypi 3.14159
+#define GP_M 72//72
+#define GP_N 80//160
+#define divider 1
+#define R_limit 80
+#define binlen 1//(R_limit/GP_N-1)
+
+namespace autosense {
+namespace segmenter {
+
+typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::DontAlign> Mat;
+
+struct MyPoint
+{
+	float x;
+	float y;
+	float z;
+	short intesity;
+	short type = -1;
+	int Index;
+	short Isit = -1;
+};
+
+struct VEC{
+	vector <MyPoint> Point;
+};
+
+void GP_INSAC(const pcl::PointCloud<pcl::PointXYZI> &After, pcl::PointCloud<pcl::PointXYZI> &Ob, pcl::PointCloud<pcl::PointXYZI> &Gr);
+
+}
+}
+#endif

samples/detection_node.cpp

@@ -18,6 +18,7 @@
 #include "object_builders/object_builder_manager.hpp"
 #include "roi_filters/roi.hpp"               // roi::applyROIFilter
 #include "segmenters/segmenter_manager.hpp"  // segmenter::createGroundSegmenter
+#include "segmenters/GuassianProcess.h"  //GPR
 
 using namespace autosense;  // NOLINT
 
@@ -57,10 +58,14 @@ void OnPointCloud(const sensor_msgs::PointCloud2ConstPtr& ros_pc2) {
     std::vector<PointICloudPtr> cloud_clusters;
     PointICloudPtr cloud_ground(new PointICloud);
     PointICloudPtr cloud_nonground(new PointICloud);
-
-    ground_remover_->segment(*cloud, cloud_clusters);
+
+    //GPF ground remove
+    /*ground_remover_->segment(*cloud, cloud_clusters);
     *cloud_ground = *cloud_clusters[0];
-    *cloud_nonground = *cloud_clusters[1];
+    *cloud_nonground = *cloud_clusters[1];*/
+
+    //GPR ground remove
+    segmenter::GP_INSAC(*cloud, *cloud_nonground, *cloud_ground);
 
     // reset clusters
     cloud_clusters.clear();