[icp] pcl::IterativeClosestPoint crash when encountering an invalid point in debug mode

[QiuYilin]

pcl::IterativeClosestPoint will crash when encountering an invalid point in debug mode, but not in release mode.

Assertion failed: point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!", file C:\src\vcpkg\buildtrees\pcl\src\head\f84d767a5c-61de3f9c1f.clean\kdtree\include\pcl/kdtree/impl/kdtree_flann.hpp, line 239

code:

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>

#include <iostream>

int main() {
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(
      new pcl::PointCloud<pcl::PointXYZ>(5, 1));
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(
      new pcl::PointCloud<pcl::PointXYZ>);

  pcl::io::loadPCDFile<pcl::PointXYZ>("../../source/croped.pcd", *cloud_in);

  pcl::io::loadPCDFile<pcl::PointXYZ>("../../source/croped.pcd", *cloud_out);

  std::cout << "Transformed " << cloud_in->size()
            << " data points:" << std::endl;

  pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
  icp.setInputSource(cloud_in);
  icp.setInputTarget(cloud_out);

  pcl::PointCloud<pcl::PointXYZ> Final;
  icp.align(Final);

  std::cout << "has converged:" << icp.hasConverged()
            << " score: " << icp.getFitnessScore() << std::endl;
  std::cout << icp.getFinalTransformation() << std::endl;

  return (0);
}

pcd file : https://github.com/QiuYilin/test/blob/main/pcl_test/source/croped.pcd

Your Environment (please complete the following information):

• OS: [Windows 10]
• Compiler: [MSVC 2022]
• PCL Version: 8e844ea

[mvieth]

So in release mode, how are the results (the aligned cloud and the final transformation)? Do they make sense?

Most users probably use some kind of filter (e.g. PassThrough or VoxelGrid) before ICP, so ICP currently does not check for invalid points.

[QiuYilin]

The result in Release mode are meaningless. The pcd file is result of filter cropbox.

[mvieth]

The result in Release mode are meaningless. The pcd file is result of filter cropbox.

Could you describe the crop-box filtering a bit more, perhaps post some code? Did you use a pcl::CropBox<pcl::PointXYZI> or a pcl::CropBox<pcl::PCLPointCloud2> or something else?

[QiuYilin]

Yes，I used pcl::CropBoxpcl::PointXYZI with "keep organized" on.

Part of the point cloud in ascii format：

# .PCD v0.7 - Point Cloud Data file format
VERSION 0.7
FIELDS x y z intensity
SIZE 4 4 4 4
TYPE F F F F
COUNT 1 1 1 1
WIDTH 3200
HEIGHT 400
VIEWPOINT 0 0 0 1 0 0 0
POINTS 1280000
DATA ascii
nan nan nan 0
nan nan nan 0
nan nan nan 0
nan nan nan 0
nan nan nan 0
nan nan nan 0
nan nan nan 0
nan nan nan 0
nan nan nan 0
nan nan nan 0

[QiuYilin]

There is still something wrong in icp.getFitnessScore(). Fix in #6056