New option for detecting close objects and other hacks for closer objects

src/nodelets/obstacles_detection.cpp

@@ -169,7 +169,7 @@ private:
 		}
 
 
-		originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform);
+
 
 
 		/////////////////////////////////////////////////////////////////////////////
@@ -181,10 +181,12 @@ private:
 
 		ros::Time lasttime = ros::Time::now();
 
-		hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits<int>::min(), maxFloorHeight_);
 
+/*
 		if (!simpleSegmentation_){
 
+			originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform);
+			hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits<int>::min(), maxFloorHeight_);
 
 			rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(hypotheticalGroundCloud,
 					ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_);
@@ -203,8 +205,58 @@ private:
 				*obstaclesCloud += *obstaclesNearFloorCloud;
 			}
 
+		}*/
+		if (!simpleSegmentation_){
+
+			originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform);
+			pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud_front = rtabmap::util3d::passThrough(originalCloud, "x", std::numeric_limits<int>::min(), 1.);
+			pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud_back = rtabmap::util3d::passThrough(originalCloud, "x", 1., std::numeric_limits<int>::max());
+
+			pcl::PointCloud<pcl::PointXYZ>::Ptr hypotheticalGroundCloud_front = rtabmap::util3d::passThrough(originalCloud_front, "z", std::numeric_limits<int>::min(), maxFloorHeight_);
+			pcl::PointCloud<pcl::PointXYZ>::Ptr hypotheticalGroundCloud_back = rtabmap::util3d::passThrough(originalCloud_back, "z", std::numeric_limits<int>::min(), maxFloorHeight_);
+
+			obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_);
+
+			//STEP 1.
+			rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(hypotheticalGroundCloud_front,
+								ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_);
+
+			if(ground.get() && ground->size())
+			{
+				pcl::copyPointCloud(*hypotheticalGroundCloud_front, *ground, *groundCloud);
+			}
+
+
+			if(obstacles.get() && obstacles->size())
+			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesNearFloorCloud(new pcl::PointCloud<pcl::PointXYZ>);
+				pcl::copyPointCloud(*hypotheticalGroundCloud_front, *obstacles, *obstaclesNearFloorCloud);
+				*obstaclesCloud += *obstaclesNearFloorCloud;
+			}
+
+			//STEP 2.
+			rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(hypotheticalGroundCloud_back,
+											ground, obstacles, 3.*normalEstimationRadius_, 2.*groundNormalAngle_, minClusterSize_);
+
+			if(ground.get() && ground->size())
+			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud2 (new pcl::PointCloud<pcl::PointXYZ>);
+				pcl::copyPointCloud(*hypotheticalGroundCloud_back, *ground, *groundCloud2);
+				*groundCloud += *groundCloud2;
+			}
+
+
+			if(obstacles.get() && obstacles->size())
+			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesNearFloorCloud(new pcl::PointCloud<pcl::PointXYZ>);
+				pcl::copyPointCloud(*hypotheticalGroundCloud_back, *obstacles, *obstaclesNearFloorCloud);
+				*obstaclesCloud += *obstaclesNearFloorCloud;
+			}
+
 		}
 		else{
+			originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform);
+			hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits<int>::min(), maxFloorHeight_);
 			obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_);
 		}
 

src/nodelets/point_cloud_xyz.cpp

@@ -71,7 +71,8 @@ public:
 		exactSyncDepth_(0),
 		exactSyncDisparity_(0),
 		cut_right_(0),
-		cut_left_(0)
+		cut_left_(0),
+		special_filter_close_object_(false)
 	{}
 
 	virtual ~PointCloudXYZ()
@@ -103,6 +104,8 @@ private:
 		pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
 		pnh.param("cut_left", cut_left_, cut_left_);
 		pnh.param("cut_right", cut_right_, cut_right_);
+		pnh.param("special_filter_close_object", special_filter_close_object_, special_filter_close_object_);
+
 		ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
 
 		if(approxSync)
@@ -164,6 +167,29 @@ private:
 				pRoi.setTo(cv::Scalar(0.));
 			}
 
+			if (special_filter_close_object_){
+				cv::Mat pRoi = image(cv::Rect(int(0.05*(float(cols))),int(0.05*(float(rows))),int(0.9*(float(cols))),int(0.9*float(rows))));
+				//cv::GaussianBlur(pRoi, pRoi, cv::Size(3, 3), 0, 0);
+				cv::medianBlur(pRoi, pRoi, 3);
+
+				//Do filter of close objects
+				pRoi = image(cv::Rect(int(cols/10),int(0.8*(float(rows))),int(0.8*(float(cols))),int(0.15*float(rows))));
+				cv::Mat bluredImage=pRoi.clone();
+
+				//Working Ok with 15 / 15
+				cv::GaussianBlur(pRoi, bluredImage, cv::Size(5, 5), 0, 0);
+
+				for(int y = 0; y < bluredImage.cols; y++)
+					for(int x = 0; x < bluredImage.rows; x++){
+						if (bluredImage.at<unsigned short>(x,y) == 0){
+							pRoi.at<unsigned short>(x,y) = 400;
+						}
+					}
+			}
+
+
+
+
 			image_geometry::PinholeCameraModel model;
 			model.fromCameraInfo(*cameraInfo);
 			float fx = model.fx();
@@ -262,6 +288,7 @@ private:
 	int noiseFilterMinNeighbors_;
 	int cut_left_;
 	int cut_right_;
+	bool special_filter_close_object_;
 
 	ros::Publisher cloudPub_;