问题描述

PCL中粗配准上出现的问题

你好，我最近打算用SAC-IA+ICP一起配合对点云数据进行高效的配准利用的是fpfh特征估计。
但是现在我在进行SAC-IA粗配准的时候遇到了一个问题：
问题报错为：
Assertion failed: point_representation_->isValid (point) && ""Invalid (NaN Inf) point coordinates given to radiusSearch!"" file C:BuildAgentwork1cb946cef51fc766tagspcl-1.6.0kdtreeincludepcl/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_hvp1_src1"" vp_1);    viewer.addPointCloud (pcd_src2 src2_hvp1_src2"" vp_1);    viewer.addPointCloud (pcd_final final_hvp1_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::PointXYZpcl::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::PointXYZpcl::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::PointXYZpcl::Normalpcl::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::PointXYZpcl::Normalpcl::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_srccloud_tgtcloud_result_initial);    std::cout  <<""totals time is:""<<clock()<<std::endl;return (0);}