PCL特征提取与匹配（3）：特征匹配问题解决

本文原创：转载请注明出处，谢谢！

使用2中的demo进行特征匹配，

1. keypoint-methods（关键点提取方法）使用Harris3D角点检测，
2. descriptor-types (特征点描述)使用FPFH
   运行到

   descriptor_kdtree.nearestKSearch(*source, i, k, k_indices, k_squared_distances);
   correspondences[i] = k_indices[0];
   

   遇到Assertion failed: point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!", file C:\pcl-1.9.1\kdtree\include\pcl/kdtree/impl/kdtree_flann.hpp, line 136的错误

解决方法

1. 输出计算得到的特征描述子
   发现存在nan点

   printf("inf1");
   typename pcl::PointCloud<FeatureType>::iterator itr;
   for (itr = source->begin(); itr != source->end(); itr++)
   {
       std::cout << *itr << endl;
   }
   printf("inf2");
   
   (nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan)
   (1.43461, 0.68249, 7.00692, 5.67639, 0.586942, 14.6281, 1.97115, 50.7711, 13.3196, 2.07839, 1.84432, 0.0302716, 6.23244,5.27624, 2.1539, 10.8, 21.7394, 9.44449, 9.60597, 18.2812, 6.65871, 9.77746, 0.8217, 54.9463, 10.4616, 1.65747, 5.87402, 7.36899, 1.68395, 3.46515, 4.46561, 8.38836, 0.866902)
   ...    
   

2. 剔除nan点
   本例中使用FPFH特征描述子，特征描述类型为pcl::FPFHSignature33，pcl::FPFHSignature33的结构中使用float histogram [33] = {0.f}来描述特征,详见PCL文档。并且demo中使用了模板编程，而对于不同的检测特征需要不同的nan点检测与剔除方法。因此需要对demo中findCorrespondences方法进行特化处理，针对pcl::FPFHSignature33类型的编写特定的nan点检测与剔除方法。

   //模板全特化pcl::FPFHSignature33
   template<>
   void ICCVTutorial<pcl::FPFHSignature33>::findCorrespondences(typename pcl::PointCloud<pcl::FPFHSignature33>::Ptr source, typename pcl::PointCloud<pcl::FPFHSignature33>::Ptr target, std::vector<int>& correspondences) const
   {
   std::cout << "correspondence assignment..." << std::flush;
   
   
   printf("inf1//////////////////////////////");
   std::cout << "source cloud size:" << static_cast<int> (source->size()) << endl;
   typename pcl::PointCloud<pcl::FPFHSignature33>::Ptr tempSource(new pcl::PointCloud<pcl::FPFHSignature33>);
   typename pcl::PointCloud<pcl::FPFHSignature33>::iterator itr;
   for (itr = source->begin(); itr != source->end(); itr++)
   {
   	std::cout << *itr << endl;
   	auto hadNan = false; //是否有nan点
   	auto allzero = true; //是否全零
   	for (auto i = 0; i < 33; i++)
   	{
   		//std::cout << (itr->histogram)[i] << endl;
   		if (pcl_isnan((itr->histogram)[i]))
   		{
   			std::cout << "exisist nan" << endl;
   			std::cout << (itr->histogram)[i] << endl;
   			hadNan = true;
   			break;
   		}
   		if (0 != (itr->histogram)[i]) {
   			allzero = false;
   			break;
   		}
   
   	}
   	if (!hadNan && !allzero)
   	{
   		std::cout << "good cloud" << endl;
   		tempSource->push_back(*itr);
   	}
   
   }
   std::cout << "tempSource cloud size:" << static_cast<int> (tempSource->size()) << endl;
   
   source->clear();
   std::cout << "source cloud size:" << static_cast<int> (source->size()) << endl;
   //(源，目标)
   pcl::copyPointCloud(*tempSource, *source);
   std::cout << "source cloud size:" << static_cast<int> (source->size()) << endl;
   printf("inf2//////////////////////////////");
   
   //printf("inf3//////////////////////////////");
   std::cout << "target cloud size:" << static_cast<int> (target->size()) << endl;
   typename pcl::PointCloud<pcl::FPFHSignature33>::Ptr tempSource2(new pcl::PointCloud<pcl::FPFHSignature33>);
   for (itr = target->begin(); itr != target->end(); itr++)
   {
   	std::cout << *itr << endl;
   	auto hadNan = false; //是否有nan点
   	auto allzero = true; //是否全零
   	for (auto i = 0; i < 33; i++)
   	{
   		//std::cout << (itr->histogram)[i] << endl;
   		if (pcl_isnan((itr->histogram)[i]))
   		{
   			std::cout << "exisist nan" << endl;
   			std::cout << (itr->histogram)[i] << endl;
   			hadNan = true;
   			break;
   		}
   		if (0 != (itr->histogram)[i]) {
   			allzero = false;
   			break;
   		}
   
   	}
   	if (!hadNan && !allzero)
   	{
   		std::cout << "good cloud" << endl;
   		tempSource2->push_back(*itr);
   	}
   
   }
   std::cout << "tempSource2 cloud size:" << static_cast<int> (tempSource2->size()) << endl;
   
   target->clear();
   std::cout << "target cloud size:" << static_cast<int> (target->size()) << endl;
   //(源，目标)
   pcl::copyPointCloud(*tempSource2, *target);
   std::cout << "target cloud size:" << static_cast<int> (target->size()) << endl;
   //printf("inf4//////////////////////////////");
   
   
   correspondences.resize(source->size());
   
   
   // Use a KdTree to search for the nearest matches in feature space
   pcl::KdTreeFLANN<pcl::FPFHSignature33> descriptor_kdtree;
   descriptor_kdtree.setInputCloud(target);
   
   // Find the index of the best match for each keypoint, and store it in "correspondences_out"
   const int k = 1;
   std::vector<int> k_indices(k);
   std::vector<float> k_squared_distances(k);
   
   
   for (int i = 0; i < static_cast<int> (source->size()); ++i)
   {
   	descriptor_kdtree.nearestKSearch(*source, i, k, k_indices, k_squared_distances);
   	correspondences[i] = k_indices[0];
   }
   std::cout << "OK" << std::endl;
   }
   

   上述方法剔除了点云中的nan特征和描述子为全0的特征。