Grid/DetectFlatObstacles default true. segmentObstaclesFromGround(): Fixed not used indices still kept in obstacles output

corelib/include/rtabmap/core/Parameters.h

@@ -477,7 +477,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Grid, MaxGroundAngle,          float,  45,      uFormat("[%s=true] Maximum angle (degrees) between point's normal to ground's normal to label it as ground. Points with higher angle difference are considered as obstacles.", kGridNormalsSegmentation().c_str()));
 	RTABMAP_PARAM(Grid, NormalK,                 int,    10,      uFormat("[%s=true] K neighbors to compute normals.", kGridNormalsSegmentation().c_str()))
 	RTABMAP_PARAM(Grid, MinClusterSize,          int,    10,      uFormat("[%s=true] Minimum cluster size to project the points. The distance between clusters is defined by 2*\"%s\".", kGridNormalsSegmentation().c_str(), kGridCellSize().c_str()));
-	RTABMAP_PARAM(Grid, FlatObstacleDetected,    bool,   false,   uFormat("[%s=true] Flat obstacles detected.", kGridNormalsSegmentation().c_str()));
+	RTABMAP_PARAM(Grid, FlatObstacleDetected,    bool,   true,    uFormat("[%s=true] Flat obstacles detected.", kGridNormalsSegmentation().c_str()));
 #ifdef RTABMAP_OCTOMAP
 	RTABMAP_PARAM(Grid, 3D,                      bool,   true,    uFormat("A 3D occupancy grid is required if you want an Octomap. Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is false.", kGridFromDepth().c_str()));
 #else

corelib/include/rtabmap/core/impl/util3d_mapping.hpp

@@ -141,7 +141,13 @@ void segmentObstaclesFromGround(
 		if(ground->size() != cloud->size())
 		{
 			// Remove ground
-			pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, ground, true);
+			pcl::IndicesPtr notObstacles = ground;
+			if(indices->size())
+			{
+				notObstacles = util3d::extractIndices(cloud, indices, true);
+				notObstacles = util3d::concatenate(notObstacles, ground);
+			}
+			pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
 
 			// If ground height is set, remove obstacles under it
 			if(maxGroundHeight > 0.0f)

corelib/src/OccupancyGrid.cpp

@@ -276,6 +276,8 @@ void OccupancyGrid::createLocalMap(const Signature & node, cv::Mat & ground, cv:
 
 			pcl::IndicesPtr groundIndices, obstaclesIndices;
 
+			if(indices->size())
+			{
 				if(normalsSegmentation_)
 				{
 					UDEBUG("normalKSearch=%d", normalKSearch_);
@@ -307,8 +309,8 @@ void OccupancyGrid::createLocalMap(const Signature & node, cv::Mat & ground, cv:
 					// passthrough filter
 					groundIndices = rtabmap::util3d::passThrough(cloud, indices, "z", minGroundHeight_<0.0f?minGroundHeight_:std::numeric_limits<int>::min(), maxGroundHeight_);
 					obstaclesIndices = rtabmap::util3d::extractIndices(cloud, groundIndices, true);
-
 				}
+
 				UDEBUG("groundIndices=%d obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
 
 				// Do radius filtering after voxel filtering ( a lot faster)
@@ -374,6 +376,7 @@ void OccupancyGrid::createLocalMap(const Signature & node, cv::Mat & ground, cv:
 				}
 			}
 		}
+	}
 	UDEBUG("ground=%d obstacles=%d channels=%d", ground.cols, obstacles.cols, ground.cols?ground.channels():obstacles.channels());
 }
 

guilib/src/DatabaseViewer.cpp

@@ -1546,6 +1546,9 @@ void DatabaseViewer::regenerateLocalMaps()
 	UTimer time;
 	OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
 
+	generatedLocalMaps_.clear();
+	generatedLocalMapsInfo_.clear();
+
 	rtabmap::ProgressDialog progressDialog(this);
 	progressDialog.setMaximumSteps(ids_.size());
 	progressDialog.show();
@@ -1571,12 +1574,6 @@ void DatabaseViewer::regenerateLocalMaps()
 			uInsert(generatedLocalMaps_, std::make_pair(data.id(), std::make_pair(ground, obstacles)));
 			uInsert(generatedLocalMapsInfo_, std::make_pair(data.id(), std::make_pair(grid.getCellSize(), viewpoint)));
 			msg = QString("Generated local occupancy grid map %1/%2 (%3s)").arg(i+1).arg((int)ids_.size()).arg(time.ticks());
-			if(i==37)
-			{
-				pcl::io::savePCDFileBinary("ground.pcd", *util3d::laserScanToPointCloud(ground));
-				pcl::io::savePCDFileBinary("obstacles.pcd", *util3d::laserScanToPointCloud(obstacles));
-				UWARN("Saved ground.pcd and obstacles.pcd");
-			}
 		}
 
 		progressDialog.appendText(msg);