From 24ae844bc49f310c10837b4cd13df51409472827 Mon Sep 17 00:00:00 2001
From: Fan Weng <fanweng02@gmail.com>
Date: Mon, 7 Dec 2020 22:00:51 -0500
Subject: [PATCH] Segment the plane with PCL and separate the obstacles.

---
 Lidar_Obstacle_Detection/src/environment.cpp  | 12 ++++--
 .../src/processPointClouds.cpp                | 41 ++++++++++++++++---
 2 files changed, 44 insertions(+), 9 deletions(-)

diff --git a/Lidar_Obstacle_Detection/src/environment.cpp b/Lidar_Obstacle_Detection/src/environment.cpp
index a0abf80..83a3817 100644
--- a/Lidar_Obstacle_Detection/src/environment.cpp
+++ b/Lidar_Obstacle_Detection/src/environment.cpp
@@ -49,10 +49,14 @@ void simpleHighway(pcl::visualization::PCLVisualizer::Ptr& viewer)
     Lidar* lidar = new Lidar(cars, 0);
     pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud = lidar->scan();
     // renderRays(viewer, lidar->position, pointCloud);
-    renderPointCloud(viewer, pointCloud, "pointCloud");
-
-    // TODO:: Create point processor
-
+    // renderPointCloud(viewer, pointCloud, "pointCloud");
+
+    // Create point processor
+    ProcessPointClouds<pcl::PointXYZ>* pointProcessor = new ProcessPointClouds<pcl::PointXYZ>();
+    // Segment the point cloud
+    std::pair<pcl::PointCloud<pcl::PointXYZ>::Ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentCloud = pointProcessor->SegmentPlane(pointCloud, 100, 0.2);
+    renderPointCloud(viewer, segmentCloud.first, "obstacleCloud", Color(1,0,0));
+    renderPointCloud(viewer, segmentCloud.second, "planeCloud", Color(0,1,0));
 }
 
 
diff --git a/Lidar_Obstacle_Detection/src/processPointClouds.cpp b/Lidar_Obstacle_Detection/src/processPointClouds.cpp
index a9cda9a..713bb42 100644
--- a/Lidar_Obstacle_Detection/src/processPointClouds.cpp
+++ b/Lidar_Obstacle_Detection/src/processPointClouds.cpp
@@ -41,9 +41,23 @@ typename pcl::PointCloud<PointT>::Ptr ProcessPointClouds<PointT>::FilterCloud(ty
 template<typename PointT>
 std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> ProcessPointClouds<PointT>::SeparateClouds(pcl::PointIndices::Ptr inliers, typename pcl::PointCloud<PointT>::Ptr cloud)
 {
-  // TODO: Create two new point clouds, one cloud with obstacles and other with segmented plane
-
-    std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> segResult(cloud, cloud);
+    // Create two new point clouds, one cloud with obstacles and other with segmented plane
+    typename pcl::PointCloud<PointT>::Ptr obstacleCloud{new pcl::PointCloud<PointT>()};
+    typename pcl::PointCloud<PointT>::Ptr planeCloud{new pcl::PointCloud<PointT>()};
+
+    // Construct the plane point cloud
+    for (int index : inliers->indices)
+        planeCloud->points.push_back(cloud->points[index]);
+
+    // Create the filtering object
+    pcl::ExtractIndices<PointT> extract;
+    // Extract the points not belong to the plane
+    extract.setInputCloud(cloud);
+    extract.setIndices(inliers);
+    extract.setNegative(true);
+    extract.filter(*obstacleCloud);
+
+    std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> segResult(obstacleCloud, planeCloud);
     return segResult;
 }
 
@@ -53,8 +67,25 @@ std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT
 {
     // Time segmentation process
     auto startTime = std::chrono::steady_clock::now();
-    pcl::PointIndices::Ptr inliers;
-    // TODO:: Fill in this function to find inliers for the cloud.
+
+    pcl::PointIndices::Ptr inliers{new pcl::PointIndices()};
+    pcl::ModelCoefficients::Ptr coefficients{new pcl::ModelCoefficients()};
+    pcl::SACSegmentation<PointT> seg;
+
+    // Set the segmentation parameters
+    seg.setOptimizeCoefficients(true);
+    seg.setModelType(pcl::SACMODEL_PLANE);
+    seg.setMethodType(pcl::SAC_RANSAC);
+    seg.setMaxIterations(maxIterations);
+    seg.setDistanceThreshold(distanceThreshold);
+
+    // Segment the largest planar component from the remaining cloud
+    seg.setInputCloud(cloud);
+    seg.segment(*inliers, *coefficients);
+    if (inliers->indices.size() == 0)
+    {
+      std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
+    }
 
     auto endTime = std::chrono::steady_clock::now();
     auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);