ICP配准同名点对输出

使用PCL库的icp算法对两组点云数据进行配准，并输出两组点云的同名点对信息，由于icp算法流程里，source点云向target点云匹配的过程中，每一次迭代都会为source点云内的每个点在target点云内寻找最近点，因此每次迭代过程都会生成一组同名点对。输出每一次迭代过程中的点对信息，就可以找到原始两组点云内的同名点对。
具体操作：修改icp.hpp文件，文件路径：%PCL安装路径%\include\pcl-1.9\pcl\registration\impl\icp.hpp。在函数pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation ( PointCloudSource &output, const Matrix4 &guess)中做如下修改：
在

if (use_reciprocal_correspondence_)
      correspondence_estimation_->determineReciprocalCorrespondences (*correspondences_, corr_dist_threshold_);
    else
      correspondence_estimation_->determineCorrespondences (*correspondences_, corr_dist_threshold_);

后面添加

	//输出对应点对
	//for(int i=0;i<input_->size();i++)
		//cout << "第"<<i<<"组点对:in source " << correspondences_->at(i).index_query <<" ("<< input_transformed->at(i).x << "," << input_transformed->at(i).y << "," << input_transformed->at(i).z
		//<< "), in target "<<correspondences_->at(i).index_match<<" ("<<target_->at(i).x<<","<< target_->at(i).y<<","<< target_->at(i).z<<")." << endl;
	FILE* pOutFile;
	char* strCorFileName = new char[80];
	sprintf(strCorFileName, "e:\\Livox\\%d.txt", nr_iterations_);
	fopen_s(&pOutFile, strCorFileName, "w");
	for (int i = 0; i<input_->size(); i++)
	{
		fprintf(pOutFile, "%f,",input_transformed->at(correspondences_->at(i).index_query).x);
		fprintf(pOutFile, "%f,",input_transformed->at(correspondences_->at(i).index_query).y);
		fprintf(pOutFile, "%f,",input_transformed->at(correspondences_->at(i).index_query).z);
		fprintf(pOutFile, "%d,",correspondences_->at(i).index_query);
		fprintf(pOutFile, "%f,",target_->at(correspondences_->at(i).index_match).x);
		fprintf(pOutFile, "%f,",target_->at(correspondences_->at(i).index_match).y);
		fprintf(pOutFile, "%f,",target_->at(correspondences_->at(i).index_match).z);
		fprintf(pOutFile, "%d,",correspondences_->at(i).index_match);
		fprintf(pOutFile, "%f,",correspondences_->at(i).distance);
		fprintf(pOutFile, "\n");
	}
	fclose(pOutFile);
	delete[] strCorFileName;
	strCorFileName = NULL;

说明：点对的索引值保存在结构体数组Correspondence_里，两个成员index_query和index_match分别表示source点云中点的索引和target点云中寻找到的与source中对应点欧氏距离最近的点的索引。用这两个索引可以访问点云中的点的具体坐标值。source点云每次迭代后的新坐标值保存在input_transformed里，target点云的坐标保存在Registration类的target_成员里。
可以根据具体需求选择要输出的信息类别和顺序，这里分别输出source和target中对应点的坐标和索引，以及两点间的欧氏距离。
输出原始两组点云之间的同名点对：

//输出最终对应点对
  float fDistance = 0.0;
  FILE* pCorFile;
  char* strCorFileName = new char[80];
  sprintf(strCorFileName, "e:\\Livox\\correspondence.txt");
  fopen_s(&pCorFile, strCorFileName, "w");
  float fDistance = 0.0;
  for (int i = 0; i<input_->size(); i++)
  {
	  fprintf(pCorFile, "%f,", input_->at(correspondences_->at(i).index_query).x);
	  fprintf(pCorFile, "%f,", input_->at(correspondences_->at(i).index_query).y);
	  fprintf(pCorFile, "%f,", input_->at(correspondences_->at(i).index_query).z);
	  fprintf(pCorFile, "%d,", correspondences_->at(i).index_query);
	  fprintf(pCorFile, "%f,", target_->at(correspondences_->at(i).index_match).x);
	  fprintf(pCorFile, "%f,", target_->at(correspondences_->at(i).index_match).y);
	  fprintf(pCorFile, "%f,", target_->at(correspondences_->at(i).index_match).z);
	  fprintf(pCorFile, "%d,", correspondences_->at(i).index_match);
	  fDistance = (input_->at(correspondences_->at(i).index_query).x - target_->at(correspondences_->at(i).index_match).x)*
		  (input_->at(correspondences_->at(i).index_query).x - target_->at(correspondences_->at(i).index_match).x) +
		  (input_->at(correspondences_->at(i).index_query).y - target_->at(correspondences_->at(i).index_match).y)*
		  (input_->at(correspondences_->at(i).index_query).y - target_->at(correspondences_->at(i).index_match).y) +
		  (input_->at(correspondences_->at(i).index_query).z - target_->at(correspondences_->at(i).index_match).z)*
		  (input_->at(correspondences_->at(i).index_query).z - target_->at(correspondences_->at(i).index_match).z);
	  fprintf(pCorFile, "%f", fDistance);
	  fprintf(pCorFile, "\n");
  }
  fclose(pCorFile);
  delete[] strCorFileName;
  strCorFileName = NULL;

这样输出的是把source点云中的每个点都在target点云中寻找对应点作为点对，如果两幅点云并不是绝对重合，这种方法就会输出一些非同名点对的对应点，即在现实世界中，该点出现在在source点云中，却并不出现在target点云中。可以在for循环里添加一个if判断：最后一次迭代得到的对应点对里，点对的欧式距离小于一个阈值时，才输出对应点的信息。
参考：这里