点云处理——去除重采样后的无效点

对彩色点云重采样后进行快速三角化时报错

控制台显示：
Assertion failed: point_representation_->isValid (point) && “Invalid (NaN, Inf) point coordinates given to nearestKSearch!”, file F:\PCLdon\pcl-master\kdtree\include\pcl/kdtree/impl/kdtree_flann.hpp, line 136

看控制台显示的错误说明，发现是由于输入点云具有无效点造成的，排查错误发现错误出现在法线估计的compute处。

对输入点云使用removeNaNFromPointCloud函数去除无效点，发现去除前后点个数不变。

对调用输入点云的is_dense成员函数，发现输出为True（即不存在无效点）。

至此该错误很诡异，使用pcl自带的无效点去除函数和判断是否具有无效点的成员函数都显示其不具有无效点，但却因具有无效点造成了法线估计上的错误。

继续排查解决：

根据控制台显示查看kdtree_flann.hpp, line 136，对应该错误的代码为：assert (point_representation_->isValid (point) && “Invalid (NaN, Inf) point coordinates given to nearestKSearch!”);

我在程序中的输入点云为PointXYZRGB类型，没有isValid成员函数，该代码中调用isValid的point_representation为PointRepresentationConstPtr类型。

重试中断程序定位出错行，定位到point_representation.h文件，定位到行!pcl_isfinite (temp[i])，其附近代码如下：

 for (int i = 0; i < nr_dimensions_; ++i)
 {
    if (!pcl_isfinite (temp[i]))
     {
         is_valid = false;
          break;
      }
  }

考虑使用其中的pcl_isfinite检测去除无效点。

首先定义输入点云类型的迭代器，分别取出成员x,y,z,rgb，使用pcl_isfinite判断，其中任意一个无效都erase该点。

最后使用该方法成功，代码片如下：

PointCloud<PointXYZRGB>::Ptr cloud(new PointCloud<PointXYZRGB>);
PointCloud<PointXYZRGB>::iterator it = cloud->points.begin();
while (it != cloud->points.end())
{
	float x, y, z, rgb;
	x = it->x;
	y = it->y;
	z = it->z;
	rgb =it->rgb;  
	//cout << "x: " << x << "  y: " << y << "  z: " << z << "  rgb: " << rgb << endl;
	if (!pcl_isfinite(x) || !pcl_isfinite(y) || !pcl_isfinite(z) || !pcl_isfinite(rgb))
	{
		it = cloud->points.erase(it);
	}
	else
		++it;
}