PCL::removeNaNFromPointCloud() 不能有效去除 NaN 点

1. 问题描述

pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>); // 建立kdtree来进行近邻点集搜索
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>); // 为kdtree添加点运数据
tree->setInputCloud(cloud);
n.setInputCloud(cloud);
n.setSearchMethod(tree); // 点云法向计算时，需要所搜的近邻点大小
n.setKSearch (20);
// 开始进行法向计算
n.compute(*normals);

在使用 PCL 库计算点云法向量时，出现以下错误：

Assertion failed: point_representation_->isValid (point) && " nvalid (NaN, Inf) point coordinates given to nearestKSearch!"

说明点云数据中存在无效点，需要去除这些 NaN 点

2. removeNaNFromPointCloud() 去除无效点

引入 pcl/filters/filter.h 头文件，调用 filter.h 中的 removeNaNFromPointCloud() 函数去除NaN 点：

# include <pcl/filters/filter.h>

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);
std::vector<int> mapping;
pcl::removeNaNFromPointCloud(*cloud_in, *cloud_out, mapping); // 移除无效点，其中 cloud_in 为初始点云，cloud_out 为去除无效点后的点云

• 如果 cloud_in 是由 pcd 文件导出生成的点云，那么使用 removeNaNFromPointCloud() 函数能解决此问题

pcl::io::loadPCDFile(argv[1],*cloud_in); // 由 pcd 文件导出的点云数据使用removeNaNFromPointCloud()可以解决该问题

• 但若是由 push_back 定义的点云数据，直接使用 removeNaNFromPointCloud() 函数仍会报错

pcl::PointCloudpcl::PointXYZ::Ptr cloud_in (new pcl::PointCloudpcl::PointXYZ);
pcl::PointXYZ p_nan,p_valid;

p_nan.x = p_nan.y = p_nan.z = std::numeric_limits::quiet_NaN();
p_valid.x = p_valid.y = p_valid.z = 1.0;

cloud_in->push_back(p_nan);
cloud_in->push_back(p_valid); // 这种方式创建的点云数据 cloud_in 使用removeNaNFromPointCloud() 函数仍会报错

利用 cout 输出 cloud_in 的数目，发现与使用 removeNaNFromPointCloud() 函数前的点云数目相同，说明实际上没有去除无效点。

3. 解决方法

在使用 removeNaNFromPointCloud() 函数之前，添加语句：

cloud_in->is_dense = false;

“手动”创建的点云数据，默认具有 dense 属性，即不存在无效点（NaN 点），因此需要把输入的点云 cloud_in 的 is_dense 改为 false。

removeNaNFromPointCloud() 函数的定义：

template <typename PointT> void
pcl::removeNaNFromPointCloud (const pcl::PointCloud<PointT> &cloud_in, // 输入点云
                              pcl::PointCloud<PointT> &cloud_out,	   // 输出无NaN的点云
                              std::vector<int> &index)				   // 输出点云的索引
{
  // If the clouds are not the same, prepare the output
  if (&cloud_in != &cloud_out)
  {
    cloud_out.header = cloud_in.header;
    cloud_out.points.resize (cloud_in.points.size ());
  }
  // Reserve enough space for the indices
  index.resize (cloud_in.points.size ());
  size_t j = 0;

  // If the data is dense, we don't need to check for NaN
  if (cloud_in.is_dense) 											// 判断点云中是否是 dense 点云
  {
    // Simply copy the data											// 如果是 dense 点云，则输出点云 = 输入点云
    cloud_out = cloud_in;
    for (j = 0; j < cloud_out.points.size (); ++j)
      index[j] = static_cast<int>(j);
  }
  else
  {
    for (size_t i = 0; i < cloud_in.points.size (); ++i)
    {
      if (!pcl_isfinite (cloud_in.points[i].x) || 					//返回一个布尔值，判断当前点的值是不是正常数值
          !pcl_isfinite (cloud_in.points[i].y) || 
          !pcl_isfinite (cloud_in.points[i].z))
        continue;
      cloud_out.points[j] = cloud_in.points[i];
      index[j] = static_cast<int>(i);
      j++;
    }
    if (j != cloud_in.points.size ())
    {
      // Resize to the correct size
      cloud_out.points.resize (j);
      index.resize (j);
    }

    cloud_out.height = 1;
    cloud_out.width  = static_cast<uint32_t>(j);

    // Removing bad points => dense (note: 'dense' doesn't mean 'organized')
    cloud_out.is_dense = true;										// 将去除NaN后的点云设置为dense点云
  }
}

由以下语句：

 // If the data is dense, we don't need to check for NaN
  if (cloud_in.is_dense) 											// 判断点云中是否是 dense 点云
  {
    // Simply copy the data											// 如果是 dense 点云，则输出点云 = 输入点云
    cloud_out = cloud_in;

可以看出，如果点云是密集的（dense），那么使用 removeNaNFromPointCloud() 函数是不会生效的，因此需要在使用该函数前加上 cloud_in->is_dense = false 语句。

std::cout<<"cloud points = " << std::endl << cloud_in->size() << std:: endl;

此时，与使用removeNaNFromPointCloud() 函数之前相比，输出点云的数目减少，说明可以成功去除无效点。

参考：
[1] https://blog.csdn.net/AileenNut/article/details/80170146
[2] https://blog.csdn.net/qq_34792438/article/details/106648279
[3] https://github.com/PointCloudLibrary/pcl/issues/2870