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/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_;