From 971c96f566eb1b843ac7e6aaced19e56f3619a5d Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Fri, 20 May 2016 17:25:52 -0400
Subject: [PATCH] MainWindow projected map: fixed bad occupancy from wrong
 normals after voxel filtering. util3d::segmentObstaclesFromGround(): added
 flatObstacles argument.

---
 .../rtabmap/core/impl/util3d_mapping.hpp      | 21 ++++++++++++++++---
 corelib/include/rtabmap/core/util3d_mapping.h |  6 ++++--
 guilib/src/MainWindow.cpp                     |  7 +++++--
 3 files changed, 27 insertions(+), 7 deletions(-)

diff --git a/corelib/include/rtabmap/core/impl/util3d_mapping.hpp b/corelib/include/rtabmap/core/impl/util3d_mapping.hpp
index 5864d44e..a05e858e 100644
--- a/corelib/include/rtabmap/core/impl/util3d_mapping.hpp
+++ b/corelib/include/rtabmap/core/impl/util3d_mapping.hpp
@@ -28,10 +28,15 @@ void segmentObstaclesFromGround(
 		float clusterRadius,
 		int minClusterSize,
 		bool segmentFlatObstacles,
-		float maxGroundHeight)
+		float maxGroundHeight,
+		pcl::IndicesPtr * flatObstacles)
 {
 	ground.reset(new std::vector<int>);
 	obstacles.reset(new std::vector<int>);
+	if(flatObstacles)
+	{
+		flatObstacles->reset(new std::vector<int>);
+	}
 
 	if(cloud->size())
 	{
@@ -75,6 +80,10 @@ void segmentObstaclesFromGround(
 							{
 								ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
 							}
+							else if(flatObstacles)
+							{
+								*flatObstacles = util3d::concatenate(*flatObstacles, clusteredFlatSurfaces.at(i));
+							}
 						}
 					}
 				}
@@ -82,6 +91,10 @@ void segmentObstaclesFromGround(
 				{
 					// reject ground!
 					ground.reset(new std::vector<int>);
+					if(flatObstacles)
+					{
+						*flatObstacles = flatSurfaces;
+					}
 				}
 			}
 		}
@@ -118,7 +131,8 @@ void segmentObstaclesFromGround(
 		float clusterRadius,
 		int minClusterSize,
 		bool segmentFlatObstacles,
-		float maxGroundHeight)
+		float maxGroundHeight,
+		pcl::IndicesPtr * flatObstacles)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
 	segmentObstaclesFromGround<PointT>(
@@ -131,7 +145,8 @@ void segmentObstaclesFromGround(
 			clusterRadius,
 			minClusterSize,
 			segmentFlatObstacles,
-			maxGroundHeight);
+			maxGroundHeight,
+			flatObstacles);
 }
 
 template<typename PointT>
diff --git a/corelib/include/rtabmap/core/util3d_mapping.h b/corelib/include/rtabmap/core/util3d_mapping.h
index 53676d65..a6c5dc25 100644
--- a/corelib/include/rtabmap/core/util3d_mapping.h
+++ b/corelib/include/rtabmap/core/util3d_mapping.h
@@ -91,7 +91,8 @@ void segmentObstaclesFromGround(
 		float clusterRadius,
 		int minClusterSize,
 		bool segmentFlatObstacles = false,
-		float maxGroundHeight = 0.0f);
+		float maxGroundHeight = 0.0f,
+		pcl::IndicesPtr * flatObstacles = 0);
 template<typename PointT>
 void segmentObstaclesFromGround(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -102,7 +103,8 @@ void segmentObstaclesFromGround(
 		float clusterRadius,
 		int minClusterSize,
 		bool segmentFlatObstacles = false,
-		float maxGroundHeight = 0.0f);
+		float maxGroundHeight = 0.0f,
+		pcl::IndicesPtr * flatObstacles = 0);
 
 template<typename PointT>
 void occupancy2DFromCloud3D(
diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp
index da2fbadc..eb4b9300 100644
--- a/guilib/src/MainWindow.cpp
+++ b/guilib/src/MainWindow.cpp
@@ -2230,14 +2230,17 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
 			float groundNormalMaxAngle = M_PI_4;
 			int minClusterSize = 20;
 			cv::Mat ground, obstacles;
-			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelCloud = util3d::voxelize(cloud, indices, cellSize);
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelCloud = util3d::voxelize(cloudWithoutNormals, indices, cellSize);
 
 			// add pose rotation without yaw
 			float roll, pitch, yaw;
 			pose.getEulerAngles(roll, pitch, yaw);
 			voxelCloud = util3d::transformPointCloud(voxelCloud, Transform(0,0,0, roll, pitch, 0));
 
-			util3d::occupancy2DFromCloud3D<pcl::PointXYZRGBNormal>(
+			pcl::io::savePCDFile("cloud.pcd", *voxelCloud);
+			UWARN("saved cloud.pcd");
+
+			util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(
 					voxelCloud,
 					ground,
 					obstacles,