util3d::laserScanFromPointCloud(...): ignore NaN points and normals. RegistrationIcp: ignoring NaNs on conversion to PD format to fix some registration fails caused by NaNs.

corelib/src/RegistrationIcp.cpp

@@ -195,22 +195,30 @@ DP laserScanToDP(const rtabmap::LaserScan & scan)
 	View viewNormalY(nx!=-1?cloud.getDescriptorRowViewByName("normals",1):view);
 	View viewNormalZ(nx!=-1?cloud.getDescriptorRowViewByName("normals",2):view);
 	View viewIntensity(offsetI!=-1?cloud.getDescriptorRowViewByName("intensity",0):view);
+	int oi = 0;
 	for(int i=0; i<scan.size(); ++i)
 	{
 		const float * ptr = scan.data().ptr<float>(0, i);
 
+		if(uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && (scan.is2d() || uIsFinite(ptr[2])))
+		{
 			if(hasLocalTransform)
 			{
 				if(nx == -1)
 				{
 					cv::Point3f pt(ptr[0], ptr[1], scan.is2d()?0:ptr[2]);
 					pt = rtabmap::util3d::transformPoint(pt, scan.localTransform());
-				view(0, i) = pt.x;
+					view(0, oi) = pt.x;
-				view(1, i) = pt.y;
+					view(1, oi) = pt.y;
 					if(!scan.is2d())
 					{
-					view(2, i) = pt.z;
+						view(2, oi) = pt.z;
 					}
+					if(offsetI!=-1)
+					{
+						viewIntensity(0, oi) = ptr[offsetI];
+					}
+					++oi;
 				}
 				else if(uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz]))
 				{
@@ -222,37 +230,62 @@ DP laserScanToDP(const rtabmap::LaserScan & scan)
 					pt.normal_y=ptr[ny];
 					pt.normal_z=ptr[nz];
 					pt = rtabmap::util3d::transformPoint(pt, scan.localTransform());
-				view(0, i) = pt.x;
+					view(0, oi) = pt.x;
-				view(1, i) = pt.y;
+					view(1, oi) = pt.y;
 					if(!scan.is2d())
 					{
-					view(2, i) = pt.z;
+						view(2, oi) = pt.z;
-				}
-				viewNormalX(0, i) = pt.normal_x;
-				viewNormalY(0, i) = pt.normal_y;
-				viewNormalZ(0, i) = pt.normal_z;
-			}
-		}
-		else if(nx!=-1 || (uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz])))
-		{
-			view(0, i) = ptr[0];
-			view(1, i) = ptr[1];
-			if(!scan.is2d())
-			{
-				view(2, i) = ptr[2];
-			}
-			if(nx!=-1)
-			{
-				viewNormalX(0, i) = ptr[nx];
-				viewNormalY(0, i) = ptr[ny];
-				viewNormalZ(0, i) = ptr[nz];
-			}
 					}
+					viewNormalX(0, oi) = pt.normal_x;
+					viewNormalY(0, oi) = pt.normal_y;
+					viewNormalZ(0, oi) = pt.normal_z;
 
 					if(offsetI!=-1)
 					{
-			viewIntensity(0, i) = ptr[offsetI];
+						viewIntensity(0, oi) = ptr[offsetI];
 					}
+
+					++oi;
+				}
+				else
+				{
+					UWARN("Ignoring point %d with invalid data: pos=%f %f %f, normal=%f %f %f", i, ptr[0], ptr[1], scan.is2d()?0:ptr[3], ptr[nx], ptr[ny], ptr[nz]);
+				}
+			}
+			else if(nx==-1 || (uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz])))
+			{
+				view(0, oi) = ptr[0];
+				view(1, oi) = ptr[1];
+				if(!scan.is2d())
+				{
+					view(2, oi) = ptr[2];
+				}
+				if(nx!=-1)
+				{
+					viewNormalX(0, oi) = ptr[nx];
+					viewNormalY(0, oi) = ptr[ny];
+					viewNormalZ(0, oi) = ptr[nz];
+				}
+				if(offsetI!=-1)
+				{
+					viewIntensity(0, oi) = ptr[offsetI];
+				}
+				++oi;
+			}
+			else
+			{
+				UWARN("Ignoring point %d with invalid data: pos=%f %f %f, normal=%f %f %f", i, ptr[0], ptr[1], scan.is2d()?0:ptr[3], ptr[nx], ptr[ny], ptr[nz]);
+			}
+		}
+		else
+		{
+			UWARN("Ignoring point %d with invalid data: pos=%f %f %f", i, ptr[0], ptr[1], scan.is2d()?0:ptr[3]);
+		}
+
+	}
+	if(oi != scan.size())
+	{
+		cloud.conservativeResize(oi);
 	}
 
 	return cloud;

corelib/src/util3d.cpp

@@ -1371,7 +1371,7 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud)
 	}
 
 	UASSERT(cloud.data.size()/cloud.point_step == cloud.height*cloud.width);
-	cv::Mat laserScan(1, (int)cloud.data.size()/cloud.point_step, CV_32FC(LaserScan::channels(format)));
+	cv::Mat laserScan = cv::Mat(1, (int)cloud.data.size()/cloud.point_step, CV_32FC(LaserScan::channels(format)));
 
 	int oi=0;
 	for (uint32_t row = 0; row < cloud.height; ++row)
@@ -1383,10 +1383,12 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud)
 
 			float * ptr = laserScan.ptr<float>(0, oi);
 
+			bool valid = true;
 			if(laserScan.channels() == 2)
 			{
 				ptr[0] = *(float*)(msg_data + fieldOffsets[0]);
 				ptr[1] = *(float*)(msg_data + fieldOffsets[1]);
+				valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]);
 			}
 			else if(laserScan.channels() == 3)
 			{
@@ -1399,6 +1401,7 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud)
 				else // XYZ
 				{
 					ptr[2] = *(float*)(msg_data + fieldOffsets[2]);
+					valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]);
 				}
 			}
 			else if(laserScan.channels() == 4)
@@ -1418,6 +1421,7 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud)
 					int * ptrInt = (int*)ptr;
 					ptrInt[3] = int(b) | (int(g) << 8) | (int(r) << 16);
 				}
+				valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]);
 			}
 			else if(laserScan.channels() == 5)
 			{
@@ -1426,6 +1430,7 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud)
 				ptr[2] = *(float*)(msg_data + fieldOffsets[3]);
 				ptr[3] = *(float*)(msg_data + fieldOffsets[4]);
 				ptr[4] = *(float*)(msg_data + fieldOffsets[5]);
+				valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[3]) && uIsFinite(ptr[4]);
 			}
 			else if(laserScan.channels() == 6)
 			{
@@ -1442,6 +1447,7 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud)
 				ptr[3] = *(float*)(msg_data + fieldOffsets[3]);
 				ptr[4] = *(float*)(msg_data + fieldOffsets[4]);
 				ptr[5] = *(float*)(msg_data + fieldOffsets[5]);
+				valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[3]) && uIsFinite(ptr[4]) &&  uIsFinite(ptr[5]);
 			}
 			else if(laserScan.channels() == 7)
 			{
@@ -1463,26 +1469,24 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud)
 				ptr[4] = *(float*)(msg_data + fieldOffsets[3]);
 				ptr[5] = *(float*)(msg_data + fieldOffsets[4]);
 				ptr[6] = *(float*)(msg_data + fieldOffsets[5]);
+				valid = uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && uIsFinite(ptr[2]) && uIsFinite(ptr[4]) && uIsFinite(ptr[5]) &&  uIsFinite(ptr[6]);
 			}
 			else
 			{
 				UFATAL("Cannot handle as many channels (%d)!", laserScan.channels());
 			}
 
-			if(uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && (is3D || uIsFinite(ptr[1])))
+			if(valid)
 			{
 				++oi;
 			}
 		}
 	}
-	if(laserScan.cols == oi)
+	if(oi == 0)
 	{
-		return LaserScan(laserScan, 0, 0, format);
+		return LaserScan();
 	}
-	else
-	{
 	return  LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0, format);
-	}
 }
 
 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform)
@@ -1494,13 +1498,16 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, co
 	cv::Mat laserScan;
 	bool nullTransform = transform.isNull() || transform.isIdentity();
 	Eigen::Affine3f transform3f = transform.toEigen3f();
+	int oi = 0;
 	if(indices.get())
 	{
 		laserScan = cv::Mat(1, (int)indices->size(), CV_32FC3);
 		for(unsigned int i=0; i<indices->size(); ++i)
 		{
 			int index = indices->at(i);
-			float * ptr = laserScan.ptr<float>(0, i);
+			if(pcl::isFinite(cloud.at(index)))
+			{
+				float * ptr = laserScan.ptr<float>(0, oi++);
 				if(!nullTransform)
 				{
 					pcl::PointXYZ pt = pcl::transformPoint(cloud.at(index), transform3f);
@@ -1516,12 +1523,15 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, co
 				}
 			}
 		}
+	}
 	else
 	{
 		laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC3);
 		for(unsigned int i=0; i<cloud.size(); ++i)
 		{
-			float * ptr = laserScan.ptr<float>(0, i);
+			if(pcl::isFinite(cloud.at(i)))
+			{
+				float * ptr = laserScan.ptr<float>(0, oi++);
 				if(!nullTransform)
 				{
 					pcl::PointXYZ pt = pcl::transformPoint(cloud.at(i), transform3f);
@@ -1537,7 +1547,12 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, co
 				}
 			}
 		}
-	return laserScan;
+	}
+	if(oi == 0)
+	{
+		return cv::Mat();
+	}
+	return laserScan(cv::Range::all(), cv::Range(0,oi));
 }
 
 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform)
@@ -1548,13 +1563,19 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud,
 {
 	cv::Mat laserScan;
 	bool nullTransform = transform.isNull() || transform.isIdentity();
+	int oi=0;
 	if(indices.get())
 	{
 		laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(6));
 		for(unsigned int i=0; i<indices->size(); ++i)
 		{
 			int index = indices->at(i);
-			float * ptr = laserScan.ptr<float>(0, i);
+			if(pcl::isFinite(cloud.at(index)) &&
+					uIsFinite(cloud.at(index).normal_x) &&
+					uIsFinite(cloud.at(index).normal_y) &&
+					uIsFinite(cloud.at(index).normal_z))
+			{
+				float * ptr = laserScan.ptr<float>(0, oi++);
 				if(!nullTransform)
 				{
 					pcl::PointNormal pt = util3d::transformPoint(cloud.at(index), transform);
@@ -1576,12 +1597,18 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud,
 				}
 			}
 		}
+	}
 	else
 	{
 		laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(6));
 		for(unsigned int i=0; i<cloud.size(); ++i)
 		{
-			float * ptr = laserScan.ptr<float>(0, i);
+			if(pcl::isFinite(cloud.at(i)) &&
+					uIsFinite(cloud.at(i).normal_x) &&
+					uIsFinite(cloud.at(i).normal_y) &&
+					uIsFinite(cloud.at(i).normal_z))
+			{
+				float * ptr = laserScan.ptr<float>(0, oi++);
 				if(!nullTransform)
 				{
 					pcl::PointNormal pt = util3d::transformPoint(cloud.at(i), transform);
@@ -1603,17 +1630,25 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud,
 				}
 			}
 		}
-	return laserScan;
+	}
+	if(oi == 0)
+	{
+		return cv::Mat();
+	}
+	return laserScan(cv::Range::all(), cv::Range(0,oi));
 }
 
 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform)
 {
 	UASSERT(cloud.size() == normals.size());
-	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6));
+	cv::Mat laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(6));
 	bool nullTransform = transform.isNull() || transform.isIdentity();
+	int oi =0;
 	for(unsigned int i=0; i<cloud.size(); ++i)
 	{
-		float * ptr = laserScan.ptr<float>(0, i);
+		if(pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i)))
+		{
+			float * ptr = laserScan.ptr<float>(0, oi++);
 			if(!nullTransform)
 			{
 				pcl::PointNormal pt;
@@ -1641,7 +1676,12 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, co
 				ptr[5] = normals.at(i).normal_z;
 			}
 		}
-	return laserScan;
+	}
+	if(oi == 0)
+	{
+		return cv::Mat();
+	}
+	return laserScan(cv::Range::all(), cv::Range(0,oi));
 }
 
 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform)
@@ -1654,13 +1694,16 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
 	cv::Mat laserScan;
 	bool nullTransform = transform.isNull() || transform.isIdentity();
 	Eigen::Affine3f transform3f = transform.toEigen3f();
+	int oi=0;
 	if(indices.get())
 	{
 		laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(4));
 		for(unsigned int i=0; i<indices->size(); ++i)
 		{
-			float * ptr = laserScan.ptr<float>(0, i);
 			int index = indices->at(i);
+			if(pcl::isFinite(cloud.at(index)))
+			{
+				float * ptr = laserScan.ptr<float>(0, oi++);
 				if(!nullTransform)
 				{
 					pcl::PointXYZRGB pt = pcl::transformPoint(cloud.at(index), transform3f);
@@ -1678,12 +1721,15 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
 				ptrInt[3] = int(cloud.at(index).b) | (int(cloud.at(index).g) << 8) | (int(cloud.at(index).r) << 16);
 			}
 		}
+	}
 	else
 	{
 		laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(4));
 		for(unsigned int i=0; i<cloud.size(); ++i)
 		{
-			float * ptr = laserScan.ptr<float>(0, i);
+			if(pcl::isFinite(cloud.at(i)))
+			{
+				float * ptr = laserScan.ptr<float>(0, oi++);
 				if(!nullTransform)
 				{
 					pcl::PointXYZRGB pt = pcl::transformPoint(cloud.at(i), transform3f);
@@ -1701,7 +1747,12 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
 				ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
 			}
 		}
-	return laserScan;
+	}
+	if(oi == 0)
+	{
+		return cv::Mat();
+	}
+	return laserScan(cv::Range::all(), cv::Range(0,oi));
 }
 
 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform)
@@ -1714,13 +1765,16 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, c
 	cv::Mat laserScan;
 	bool nullTransform = transform.isNull() || transform.isIdentity();
 	Eigen::Affine3f transform3f = transform.toEigen3f();
+	int oi=0;
 	if(indices.get())
 	{
 		laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(4));
 		for(unsigned int i=0; i<indices->size(); ++i)
 		{
-			float * ptr = laserScan.ptr<float>(0, i);
 			int index = indices->at(i);
+			if(pcl::isFinite(cloud.at(index)))
+			{
+				float * ptr = laserScan.ptr<float>(0, oi++);
 				if(!nullTransform)
 				{
 					pcl::PointXYZI pt = pcl::transformPoint(cloud.at(index), transform3f);
@@ -1737,12 +1791,15 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, c
 				ptr[3] = cloud.at(index).intensity;
 			}
 		}
+	}
 	else
 	{
 		laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(4));
 		for(unsigned int i=0; i<cloud.size(); ++i)
 		{
-			float * ptr = laserScan.ptr<float>(0, i);
+			if(pcl::isFinite(cloud.at(i)))
+			{
+				float * ptr = laserScan.ptr<float>(0, oi++);
 				if(!nullTransform)
 				{
 					pcl::PointXYZI pt = pcl::transformPoint(cloud.at(i), transform3f);
@@ -1759,7 +1816,12 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, c
 				ptr[3] = cloud.at(i).intensity;
 			}
 		}
-	return laserScan;
+	}
+	if(oi == 0)
+	{
+		return cv::Mat();
+	}
+	return laserScan(cv::Range::all(), cv::Range(0,oi));
 }
 
 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform)
@@ -1767,9 +1829,12 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
 	UASSERT(cloud.size() == normals.size());
 	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
 	bool nullTransform = transform.isNull() || transform.isIdentity();
+	int oi = 0;
 	for(unsigned int i=0; i<cloud.size(); ++i)
 	{
-		float * ptr = laserScan.ptr<float>(0, i);
+		if(pcl::isFinite(cloud.at(i)))
+		{
+			float * ptr = laserScan.ptr<float>(0, oi++);
 			if(!nullTransform)
 			{
 				pcl::PointXYZRGBNormal pt;
@@ -1799,7 +1864,12 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
 			int * ptrInt = (int*)ptr;
 			ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
 		}
-	return laserScan;
+	}
+	if(oi == 0)
+	{
+		return cv::Mat();
+	}
+	return laserScan(cv::Range::all(), cv::Range(0,oi));
 }
 
 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform)
@@ -1810,13 +1880,19 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> &
 {
 	cv::Mat laserScan;
 	bool nullTransform = transform.isNull() || transform.isIdentity();
+	int oi = 0;
 	if(indices.get())
 	{
 		laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(7));
 		for(unsigned int i=0; i<indices->size(); ++i)
 		{
 			int index = indices->at(i);
-			float * ptr = laserScan.ptr<float>(0, i);
+			if(pcl::isFinite(cloud.at(index)) &&
+					uIsFinite(cloud.at(index).normal_x) &&
+					uIsFinite(cloud.at(index).normal_y) &&
+					uIsFinite(cloud.at(index).normal_z))
+			{
+				float * ptr = laserScan.ptr<float>(0, oi++);
 				if(!nullTransform)
 				{
 					pcl::PointXYZRGBNormal pt = util3d::transformPoint(cloud.at(index), transform);
@@ -1840,12 +1916,18 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> &
 				ptrInt[3] = int(cloud.at(index).b) | (int(cloud.at(index).g) << 8) | (int(cloud.at(index).r) << 16);
 			}
 		}
+	}
 	else
 	{
 		laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(7));
 		for(unsigned int i=0; i<cloud.size(); ++i)
 		{
-			float * ptr = laserScan.ptr<float>(0, i);
+			if(pcl::isFinite(cloud.at(i)) &&
+					uIsFinite(cloud.at(i).normal_x) &&
+					uIsFinite(cloud.at(i).normal_y) &&
+					uIsFinite(cloud.at(i).normal_z))
+			{
+				float * ptr = laserScan.ptr<float>(0, oi++);
 				if(!nullTransform)
 				{
 					pcl::PointXYZRGBNormal pt = util3d::transformPoint(cloud.at(i), transform);
@@ -1869,7 +1951,12 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> &
 				ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
 			}
 		}
-	return laserScan;
+	}
+	if(oi == 0)
+	{
+		return cv::Mat();
+	}
+	return laserScan(cv::Range::all(), cv::Range(0,oi));
 }
 
 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform)
@@ -1877,9 +1964,12 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, c
 	UASSERT(cloud.size() == normals.size());
 	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
 	bool nullTransform = transform.isNull() || transform.isIdentity();
+	int oi=0;
 	for(unsigned int i=0; i<cloud.size(); ++i)
 	{
-		float * ptr = laserScan.ptr<float>(0, i);
+		if(pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i)))
+		{
+			float * ptr = laserScan.ptr<float>(0, oi++);
 			if(!nullTransform)
 			{
 				pcl::PointXYZINormal pt;
@@ -1908,15 +1998,26 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, c
 			}
 			ptr[3] = cloud.at(i).intensity;
 		}
-	return laserScan;
+	}
+	if(oi == 0)
+	{
+		return cv::Mat();
+	}
+	return laserScan(cv::Range::all(), cv::Range(0,oi));
 }
 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform)
 {
 	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
 	bool nullTransform = transform.isNull() || transform.isIdentity();
+	int oi = 0;
 	for(unsigned int i=0; i<cloud.size(); ++i)
 	{
-		float * ptr = laserScan.ptr<float>(0, i);
+		if(pcl::isFinite(cloud.at(i)) &&
+				uIsFinite(cloud.at(i).normal_x) &&
+				uIsFinite(cloud.at(i).normal_y) &&
+				uIsFinite(cloud.at(i).normal_z))
+		{
+			float * ptr = laserScan.ptr<float>(0, oi++);
 			if(!nullTransform)
 			{
 				pcl::PointXYZINormal pt = util3d::transformPoint(cloud.at(i), transform);
@@ -1938,7 +2039,12 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cl
 			}
 			ptr[3] = cloud.at(i).intensity;
 		}
-	return laserScan;
+	}
+	if(oi == 0)
+	{
+		return cv::Mat();
+	}
+	return laserScan(cv::Range::all(), cv::Range(0,oi));
 }
 
 cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform)
@@ -1946,9 +2052,12 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud,
 	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
 	bool nullTransform = transform.isNull();
 	Eigen::Affine3f transform3f = transform.toEigen3f();
+	int oi=0;
 	for(unsigned int i=0; i<cloud.size(); ++i)
 	{
-		float * ptr = laserScan.ptr<float>(0, i);
+		if(pcl::isFinite(cloud.at(i)))
+		{
+			float * ptr = laserScan.ptr<float>(0, oi++);
 			if(!nullTransform)
 			{
 				pcl::PointXYZ pt = pcl::transformPoint(cloud.at(i), transform3f);
@@ -1960,9 +2069,14 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud,
 				ptr[0] = cloud.at(i).x;
 				ptr[1] = cloud.at(i).y;
 			}
+		}
 
 	}
-	return laserScan;
+	if(oi == 0)
+	{
+		return cv::Mat();
+	}
+	return laserScan(cv::Range::all(), cv::Range(0,oi));
 }
 
 cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform)
@@ -1970,9 +2084,12 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud,
 	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC3);
 	bool nullTransform = transform.isNull();
 	Eigen::Affine3f transform3f = transform.toEigen3f();
+	int oi=0;
 	for(unsigned int i=0; i<cloud.size(); ++i)
 	{
-		float * ptr = laserScan.ptr<float>(0, i);
+		if(pcl::isFinite(cloud.at(i)))
+		{
+			float * ptr = laserScan.ptr<float>(0, oi++);
 			if(!nullTransform)
 			{
 				pcl::PointXYZI pt = pcl::transformPoint(cloud.at(i), transform3f);
@@ -1986,18 +2103,29 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud,
 				ptr[1] = cloud.at(i).y;
 				ptr[2] = cloud.at(i).intensity;
 			}
+		}
 
 	}
-	return laserScan;
+	if(oi == 0)
+	{
+		return cv::Mat();
+	}
+	return laserScan(cv::Range::all(), cv::Range(0,oi));
 }
 
 cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform)
 {
 	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5));
 	bool nullTransform = transform.isNull();
+	int oi=0;
 	for(unsigned int i=0; i<cloud.size(); ++i)
 	{
-		float * ptr = laserScan.ptr<float>(0, i);
+		if(pcl::isFinite(cloud.at(i)) &&
+				uIsFinite(cloud.at(i).normal_x) &&
+				uIsFinite(cloud.at(i).normal_y) &&
+				uIsFinite(cloud.at(i).normal_z))
+		{
+			float * ptr = laserScan.ptr<float>(0, oi++);
 			if(!nullTransform)
 			{
 				pcl::PointNormal pt = util3d::transformPoint(cloud.at(i), transform);
@@ -2016,9 +2144,13 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & clou
 				ptr[3] = pt.normal_y;
 				ptr[4] = pt.normal_z;
 			}
-
 		}
-	return laserScan;
+	}
+	if(oi == 0)
+	{
+		return cv::Mat();
+	}
+	return laserScan(cv::Range::all(), cv::Range(0,oi));
 }
 
 cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform)
@@ -2026,9 +2158,12 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud,
 	UASSERT(cloud.size() == normals.size());
 	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5));
 	bool nullTransform = transform.isNull() || transform.isIdentity();
+	int oi=0;
 	for(unsigned int i=0; i<cloud.size(); ++i)
 	{
-		float * ptr = laserScan.ptr<float>(0, i);
+		if(pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i)))
+		{
+			float * ptr = laserScan.ptr<float>(0, oi++);
 			if(!nullTransform)
 			{
 				pcl::PointNormal pt;
@@ -2054,16 +2189,27 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud,
 				ptr[4] = normals.at(i).normal_z;
 			}
 		}
-	return laserScan;
+	}
+	if(oi == 0)
+	{
+		return cv::Mat();
+	}
+	return laserScan(cv::Range::all(), cv::Range(0,oi));
 }
 
 cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform)
 {
 	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6));
 	bool nullTransform = transform.isNull();
+	int oi=0;
 	for(unsigned int i=0; i<cloud.size(); ++i)
 	{
-		float * ptr = laserScan.ptr<float>(0, i);
+		if(pcl::isFinite(cloud.at(i)) &&
+				uIsFinite(cloud.at(i).normal_x) &&
+				uIsFinite(cloud.at(i).normal_y) &&
+				uIsFinite(cloud.at(i).normal_z))
+		{
+			float * ptr = laserScan.ptr<float>(0, oi++);
 			if(!nullTransform)
 			{
 				pcl::PointXYZINormal pt = util3d::transformPoint(cloud.at(i), transform);
@@ -2084,9 +2230,13 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> &
 				ptr[4] = pt.normal_y;
 				ptr[5] = pt.normal_z;
 			}
-
 		}
-	return laserScan;
+	}
+	if(oi == 0)
+	{
+		return cv::Mat();
+	}
+	return laserScan(cv::Range::all(), cv::Range(0,oi));
 }
 
 cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform)
@@ -2094,9 +2244,12 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud,
 	UASSERT(cloud.size() == normals.size());
 	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6));
 	bool nullTransform = transform.isNull() || transform.isIdentity();
+	int oi=0;
 	for(unsigned int i=0; i<cloud.size(); ++i)
 	{
-		float * ptr = laserScan.ptr<float>(0, i);
+		if(pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i)))
+		{
+			float * ptr = laserScan.ptr<float>(0, oi++);
 			if(!nullTransform)
 			{
 				pcl::PointXYZINormal pt;
@@ -2124,7 +2277,12 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud,
 				ptr[5] = normals.at(i).normal_z;
 			}
 		}
-	return laserScan;
+	}
+	if(oi == 0)
+	{
+		return cv::Mat();
+	}
+	return laserScan(cv::Range::all(), cv::Range(0,oi));
 }
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const LaserScan & laserScan, const Transform & transform)