pointcloud_to_depthimage: added fill_iterations parameter. point_cloud_xyz[rgb]: added filter_nans parameter. convertScan3dMsg(): remove NaNs if input cloud is not dense.

src/MsgConversion.cpp

@@ -1505,12 +1505,20 @@ bool convertScan3dMsg(
 		{
 			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 			pcl::fromROSMsg(*scan3dMsg, *pclScan);
+			if(!pclScan->is_dense)
+			{
+				pclScan = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScan);
+			}
 			scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
 		}
 		else
 		{
 			pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
 			pcl::fromROSMsg(*scan3dMsg, *pclScan);
+			if(!pclScan->is_dense)
+			{
+				pclScan = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScan);
+			}
 			scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
 		}
 	}
@@ -1520,6 +1528,10 @@ bool convertScan3dMsg(
 		{
 			pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZRGB>);
 			pcl::fromROSMsg(*scan3dMsg, *pclScan);
+			if(!pclScan->is_dense)
+			{
+				pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan);
+			}
 
 			if(scanCloudNormalK > 0 || scanCloudNormalRadius>0.0f)
 			{
@@ -1538,6 +1550,10 @@ bool convertScan3dMsg(
 		{
 			pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 			pcl::fromROSMsg(*scan3dMsg, *pclScan);
+			if(!pclScan->is_dense)
+			{
+				pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan);
+			}
 
 			if(scanCloudNormalK > 0 || scanCloudNormalRadius>0.0f)
 			{

src/nodelets/icp_odometry.cpp

@@ -41,6 +41,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_surface.h>
 #include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d_filtering.h>
 #include <rtabmap/core/util2d.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UConversion.h>
@@ -167,12 +168,20 @@ private:
 		{
 			pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
 			pcl::fromROSMsg(*cloudMsg, *pclScan);
+			if(!pclScan->is_dense)
+			{
+				pclScan = util3d::removeNaNNormalsFromPointCloud(pclScan);
+			}
 			scan = util3d::laserScanFromPointCloud(*pclScan);
 		}
 		else
 		{
 			pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 			pcl::fromROSMsg(*cloudMsg, *pclScan);
+			if(!pclScan->is_dense)
+			{
+				pclScan = util3d::removeNaNFromPointCloud(pclScan);
+			}
 
 			if(scanCloudNormalK_ > 0 || scanCloudNormalRadius_>0.0f)
 			{

src/nodelets/point_cloud_xyz.cpp

@@ -75,6 +75,7 @@ public:
 		noiseFilterMinNeighbors_(5),
 		normalK_(0),
 		normalRadius_(0.0f),
+		filterNaNs_(false),
 		approxSyncDepth_(0),
 		approxSyncDisparity_(0),
 		exactSyncDepth_(0),
@@ -112,6 +113,7 @@ private:
 		pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
 		pnh.param("normal_k", normalK_, normalK_);
 		pnh.param("normal_radius", normalRadius_, normalRadius_);
+		pnh.param("filter_nans", filterNaNs_, filterNaNs_);
 		pnh.param("roi_ratios", roiStr, roiStr);
 
 		// Deprecated
@@ -314,10 +316,18 @@ private:
 			pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_);
 			pcl::PointCloud<pcl::PointNormal>::Ptr pclCloudNormal(new pcl::PointCloud<pcl::PointNormal>);
 			pcl::concatenateFields(*pclCloud, *normals, *pclCloudNormal);
+			if(filterNaNs_)
+			{
+				pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal);
+			}
 			pcl::toROSMsg(*pclCloudNormal, rosCloud);
 		}
 		else
 		{
+			if(filterNaNs_ && !pclCloud->is_dense)
+			{
+				pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
+			}
 			pcl::toROSMsg(*pclCloud, rosCloud);
 		}
 		rosCloud.header.stamp = header.stamp;
@@ -337,6 +347,7 @@ private:
 	int noiseFilterMinNeighbors_;
 	int normalK_;
 	float normalRadius_;
+	bool filterNaNs_;
 	std::vector<float> roiRatios_;
 
 	ros::Publisher cloudPub_;

src/nodelets/point_cloud_xyzrgb.cpp

@@ -77,9 +77,12 @@ public:
 		noiseFilterMinNeighbors_(5),
 		normalK_(0),
 		normalRadius_(0.0f),
+		filterNaNs_(false),
 		approxSyncDepth_(0),
+		approxSyncDisparity_(0),
 		approxSyncStereo_(0),
 		exactSyncDepth_(0),
+		exactSyncDisparity_(0),
 		exactSyncStereo_(0)
 	{}
 
@@ -87,10 +90,14 @@ public:
 	{
 		if(approxSyncDepth_)
 			delete approxSyncDepth_;
+		if(approxSyncDisparity_)
+			delete approxSyncDisparity_;
 		if(approxSyncStereo_)
 			delete approxSyncStereo_;
 		if(exactSyncDepth_)
 			delete exactSyncDepth_;
+		if(exactSyncDisparity_)
+			delete exactSyncDisparity_;
 		if(exactSyncStereo_)
 			delete exactSyncStereo_;
 	}
@@ -114,6 +121,7 @@ private:
 		pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
 		pnh.param("normal_k", normalK_, normalK_);
 		pnh.param("normal_radius", normalRadius_, normalRadius_);
+		pnh.param("filter_nans", filterNaNs_, filterNaNs_);
 		pnh.param("roi_ratios", roiStr, roiStr);
 
 		//parse roi (region of interest)
@@ -459,10 +467,18 @@ private:
 			pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_);
 			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pclCloudNormal(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 			pcl::concatenateFields(*pclCloud, *normals, *pclCloudNormal);
+			if(filterNaNs_)
+			{
+				pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal);
+			}
 			pcl::toROSMsg(*pclCloudNormal, rosCloud);
 		}
 		else
 		{
+			if(filterNaNs_ && !pclCloud->is_dense)
+			{
+				pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
+			}
 			pcl::toROSMsg(*pclCloud, rosCloud);
 		}
 		rosCloud.header.stamp = header.stamp;
@@ -482,6 +498,7 @@ private:
 	int noiseFilterMinNeighbors_;
 	int normalK_;
 	float normalRadius_;
+	bool filterNaNs_;
 	std::vector<float> roiRatios_;
 	rtabmap::ParametersMap stereoBMParameters_;
 

src/nodelets/pointcloud_to_depthimage.cpp

@@ -62,6 +62,7 @@ public:
 		waitForTransform_(0.1),
 		fillHolesSize_ (0),
 		fillHolesError_(0.1),
+		fillIterations_(1),
 		decimation_(1),
 		approxSync_(0),
 		exactSync_(0)
@@ -95,6 +96,7 @@ private:
 		pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
 		pnh.param("fill_holes_size", fillHolesSize_, fillHolesSize_);
 		pnh.param("fill_holes_error", fillHolesError_, fillHolesError_);
+		pnh.param("fill_iterations", fillIterations_, fillIterations_);
 		pnh.param("decimation", decimation_, decimation_);
 		pnh.param("approx", approx, approx);
 
@@ -113,6 +115,7 @@ private:
 		ROS_INFO("  wait_for_transform=%fs", waitForTransform_);
 		ROS_INFO("  fill_holes_size=%d pixels (0=disabled)", fillHolesSize_);
 		ROS_INFO("  fill_holes_error=%f", fillHolesError_);
+		ROS_INFO("  fill_iterations=%d", fillIterations_);
 		ROS_INFO("  decimation=%d", decimation_);
 
 		image_transport::ImageTransport it(nh);
@@ -196,9 +199,12 @@ private:
 			cv_bridge::CvImage depthImage;
 			depthImage.image = rtabmap::util3d::projectCloudToCamera(model.imageSize(), model.K(), cloud, model.localTransform());
 
-			if(fillHolesSize_ > 0)
+			if(fillHolesSize_ > 0 && fillIterations_ > 0)
 			{
-				depthImage.image = rtabmap::util2d::fillDepthHoles(depthImage.image, fillHolesSize_, fillHolesError_);
+				for(int i=0; i<fillIterations_;++i)
+				{
+					depthImage.image = rtabmap::util2d::fillDepthHoles(depthImage.image, fillHolesSize_, fillHolesError_);
+				}
 			}
 
 			depthImage.header = cameraInfoMsg->header;
@@ -228,6 +234,7 @@ private:
 	double waitForTransform_;
 	int fillHolesSize_;
 	double fillHolesError_;
+	int fillIterations_;
 	int decimation_;
 
 	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::CameraInfo> MyApproxSyncPolicy;

src/nodelets/rgbdicp_odometry.cpp

@@ -52,6 +52,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_surface.h>
 #include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d_filtering.h>
 #include <rtabmap/core/util2d.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UConversion.h>
@@ -335,12 +336,20 @@ private:
 					{
 						pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
 						pcl::fromROSMsg(*cloudMsg, *pclScan);
+						if(!pclScan->is_dense)
+						{
+							pclScan = util3d::removeNaNNormalsFromPointCloud(pclScan);
+						}
 						scan = util3d::laserScanFromPointCloud(*pclScan);
 					}
 					else
 					{
 						pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 						pcl::fromROSMsg(*cloudMsg, *pclScan);
+						if(!pclScan->is_dense)
+						{
+							pclScan = util3d::removeNaNFromPointCloud(pclScan);
+						}
 
 						if(scanCloudNormalK_ > 0 || scanCloudNormalRadius_>0.0f)
 						{