small_gicp::RegistrationPCL<pcl::PointXYZ, pcl::PointXYZ> reg.getFitnessScore(); error...

[InguChoi]

Describe the bug I used the function getFitnessScore(); in the small_gicp::RegistrationPCL<pcl::PointXYZ, pcl::PointXYZ> reg;. It can be builded successfully, but It can not be operated..

The error results is below in the terminal.

/usr/local/include/pcl-1.14/pcl/kdtree/impl/kdtree_flann.hpp:239: int pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch(const PointT&, unsigned int, pcl::Indices&, std::vector&) const [with PointT = pcl::PointXYZ; Dist = flann::L2_Simple; pcl::Indices = std::vector]: Assertion `pointrepresentation->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"' failed.

Environment (please complete the following information):

• OS: Ubuntu
• Version 22.04
• Language C++

[Xiangzhaohong]

removeNAN for the input clouds first?

[InguChoi]

removeNAN for the input clouds first?

Your comment is right.

The point cloud data from my bag file is needed to pre-processing such as NAN, INFINITE.

inline void removeNaNAndInfinitePoints(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>());
    int valid_points = 0;

    for (const auto& point : cloud->points) {
        if (std::isfinite(point.x) && std::isfinite(point.y) && std::isfinite(point.z)) {
            filtered_cloud->points.push_back(point); 
            valid_points++;
        }
    }
    cloud.swap(filtered_cloud);
}

[koide3]

FYI: pcl::removeNaNFromPointCloud would be useful for this purpose. https://pointclouds.org/documentation/group__filters.html#ga9de762e05951938f074479c3bf8ae084