PCL中粗配准上出现的问题

Question

你好，我最近打算用SAC-IA+ICP一起配合对点云数据进行高效的配准,利用的是fpfh特征估计。
但是现在我在进行SAC-IA粗配准的时候遇到了一个问题：
问题报错为：
Assertion failed: point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!", file C:\BuildAgent\work\1cb946cef51fc766\tags\pcl-1.6.0\kdtree\include\pcl/kdtree/impl/kdtree_flann.hpp, line 119
这个问题让我很头疼，因为根本不知道错在哪里了。
下面是我的代码，请教大神我的问题出现在哪里了？

 #include <pcl/registration/ia_ransac.h>
#include <boost/make_shared.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/search/kdtree.h>
#include <pcl/point_representation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/features/normal_3d.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
using pcl::NormalEstimation;
using pcl::search::KdTree;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;

void visualize_pcd(PointCloud::Ptr pcd_src1, 
    PointCloud::Ptr pcd_src2,
    PointCloud::Ptr pcd_final)
{
    int vp_1, vp_2;
    // Create a PCLVisualizer object
    pcl::visualization::PCLVisualizer viewer("PCL Viewer");
    viewer.createViewPort (0.0, 0, 0.5, 1.0, vp_1);
    viewer.createViewPort (0.5, 0, 1.0, 1.0, vp_2);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src1_h (pcd_src1, 0, 255, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src2_h (pcd_src2, 255, 0, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_h (pcd_final, 0, 255, 0);
    viewer.addPointCloud (pcd_src1, src1_h, "vp1_src1", vp_1);
    viewer.addPointCloud (pcd_src2, src2_h, "vp1_src2", vp_1);
    viewer.addPointCloud (pcd_final, final_h, "vp1_final", vp_2);

    viewer.spin();
}

int 
    main (int argc, char** argv)
{

    PointCloud::Ptr cloud_final (new PointCloud);


    PointCloud::Ptr cloud_src (new PointCloud);//将长的转成短的//读取点云数据
    pcl::io::loadPCDFile ("1.pcd",*cloud_src);
    pcl::NormalEstimation<pcl::PointXYZ,pcl::Normal> ne_src;
    ne_src.setInputCloud(cloud_src);
    pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_src(new pcl::search::KdTree< pcl::PointXYZ>());
    ne_src.setSearchMethod(tree_src);
    pcl::PointCloud<pcl::Normal>::Ptr cloud_src_normals(new pcl::PointCloud< pcl::Normal>);
    ne_src.setRadiusSearch(0.03);
    //ne_src.setKSearch(20);二选一
    ne_src.compute(*cloud_src_normals);

    PointCloud::Ptr cloud_tgt (new PointCloud);
    pcl::io::loadPCDFile ("2.pcd",*cloud_tgt);
    pcl::NormalEstimation<pcl::PointXYZ,pcl::Normal> ne_tgt;
    ne_tgt.setInputCloud(cloud_tgt);
    pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_tgt(new pcl::search::KdTree< pcl::PointXYZ>());
    ne_tgt.setSearchMethod(tree_tgt);
    pcl::PointCloud<pcl::Normal>::Ptr cloud_tgt_normals(new pcl::PointCloud< pcl::Normal>);
    //ne_tgt.setKSearch(20);
    ne_tgt.setRadiusSearch(0.03);
    //compute normal to cloud_normals
    ne_tgt.compute(*cloud_tgt_normals);


    //fpfh estimation
    pcl::FPFHEstimation<pcl::PointXYZ,pcl::Normal,pcl::FPFHSignature33> fpfh_src;
    fpfh_src.setInputCloud(cloud_src);
    fpfh_src.setInputNormals(cloud_src_normals);
    pcl::search::KdTree<PointT>::Ptr tree_src_fpfh (new pcl::search::KdTree<PointT>);
    fpfh_src.setSearchMethod(tree_src_fpfh);
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_src(new pcl::PointCloud<pcl::FPFHSignature33>());
    fpfh_src.setRadiusSearch(0.05);
    //fpfh_src.setKSearch(20);
    fpfh_src.compute(*fpfhs_src);

    pcl::FPFHEstimation<pcl::PointXYZ,pcl::Normal,pcl::FPFHSignature33> fpfh_tgt;
    fpfh_tgt.setInputCloud(cloud_tgt);
    fpfh_tgt.setInputNormals(cloud_tgt_normals);
    pcl::search::KdTree<PointT>::Ptr tree_tgt_fpfh (new pcl::search::KdTree<PointT>);
    fpfh_tgt.setSearchMethod(tree_tgt_fpfh);
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_tgt(new pcl::PointCloud<pcl::FPFHSignature33>());
    fpfh_tgt.setRadiusSearch(0.05);
    //fpfh_tgt.setKSearch(20);
    fpfh_tgt.compute(*fpfhs_tgt);

    //sample consensus initial alignment
    pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> scia;
    scia.setInputCloud(cloud_src);
    scia.setInputTarget(cloud_tgt);
    scia.setSourceFeatures(fpfhs_src);
    scia.setTargetFeatures(fpfhs_tgt);
    //scia.setMinSampleDistance(1);
    //scia.setNumberOfSamples(2);
    //scia.setCorrespondenceRandomness(20);
    PointCloud::Ptr cloud_result_initial (new PointCloud);
    scia.align(*cloud_result_initial);
    visualize_pcd(cloud_src,cloud_tgt,cloud_result_initial);
    std::cout  <<"totals time is:"<<clock()<<std::endl;
return (0);
}

Answer 1

没有去除点云中的NaN点，需要在加载点云文件后再加两句去除点云中的NaN点的代码，即：
pcl::io::loadPCDFile ("1.pcd",*cloud_src); /*之后加*/
std::vector indices; //保存去除的点的索引
pcl::removeNaNFromPointCloud(*cloud_src,*cloud_src, indices); //去除点云中的NaN点