PCL 点云匹配6 之4PCS super-4pcs 粗匹配

一、概述

4-Points Congruent Sets for Robust Pairwise Surface Registration——4PCS阅读笔记-CSDN博客

点云配准算法之super-4pcs_super4pcs-CSDN博客

SAC-IA粗配准+ICP精配准_sca-ia-CSDN博客3D点云配准算法-4PCS（4点全等集配准算法）_4pcs算法-CSDN博客

05-点云配准数学原理 - 黑马机器人 | PCL-3D点云

三、测试 

 // 4 点匹配
#if 1
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>  
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/console/time.h>   // pcl中计算程序运行时间有很多函数，其中利用控制台的时间计算
#include <pcl/features/normal_3d.h>//法线
#include <pcl/surface/gp3.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/math/special_functions/round.hpp>
#include <pcl/registration/ia_fpcs.h>

using namespace std;
int    main(int argc, char** argv)
{

	string target_path = "C:\\Users\\Albert\\Desktop\\halcon_to_pcl\\1.pcd";
	string source_path = "C:\\Users\\Albert\\Desktop\\halcon_to_pcl\\2.pcd";
	pcl::console::TicToc time;
	// 加载第一次扫描点云数据作为目标云
	pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr match_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ>(target_path, *target_cloud) == -1)
	{
		PCL_ERROR("读取目标点云失败 \n");
		return (-1);
	}
	cout << "从目标点云中读取 " << target_cloud->size() << " 个点" << endl;

	// 加载从新视角得到的第二次扫描点云数据作为源点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ>(source_path, *input_cloud) == -1)
	{
		PCL_ERROR("读取源标点云失败 \n");
		return (-1);
	}
	cout << "从源点云中读取 " << input_cloud->size() << " 个点" << endl;
	time.tic();


	//初始化对象
	pcl::registration::FPCSInitialAlignment<pcl::PointXYZ, pcl::PointXYZ> fpcs;
	fpcs.setInputCloud(input_cloud);  //源点云
	fpcs.setInputTarget(target_cloud);  //目标点云
	fpcs.setApproxOverlap(0.93);//设置源和目标之间的近似重叠。
	fpcs.setDelta(0.021);//对内部计算参数进行加权的常数因子增量。
	fpcs.setMaxComputationTime(100);//设置最大计算时间(以秒为单位)。
	fpcs.setNumberOfSamples(200); //设置配准期间要使用的源点云采样的数量

	pcl::PointCloud<pcl::PointXYZ>::Ptr pcs(new pcl::PointCloud<pcl::PointXYZ>);
	fpcs.align(*pcs);
	cout << "FPCS配准用时： " << time.toc() << " ms" << endl;
	cout << "变换矩阵：" << fpcs.getFinalTransformation() << endl;
	// 使用创建的变换对为输入点云进行变换
	pcl::transformPointCloud(*input_cloud, *pcs, fpcs.getFinalTransformation());
	// 保存转换后的源点云作为最终的变换输出
	//  pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);
	*match_cloud = *target_cloud;
	// 初始化点云可视化对象

	boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer(new pcl::visualization::PCLVisualizer("显示点云"));
	int v1(0);
	viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
	viewer->setBackgroundColor(0, 0, 0, v1);
	viewer->addText("viewer_01", 10, 10, "匹配前  ", v1);
	
	// 对目标点云着色可视化 (red).
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>target_color(target_cloud, 255, 0, 0);
	viewer->addPointCloud<pcl::PointXYZ>(target_cloud, target_color, "target cloud",v1);
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud");
	// 对源点云着色可视化 (blue).
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>input_color(input_cloud, 0, 0, 255);
	viewer->addPointCloud<pcl::PointXYZ>(input_cloud, input_color, "input cloud",v1);
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "input cloud");


	//第二个窗口显示内容进行设定
	int v2(0);
	viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);// 功能尚不明确，仅仅知道：四个数值代表X最小值，Y最小值，X最大值，Y最大值，显示窗口的ID号
	viewer->setBackgroundColor(0.3, 0.3, 0.3, v2);//设置v2的背景颜色  背景颜色的设置范围被归化到0-1之间
	viewer->addText("viewer_02", 10, 10, "匹配后", v2);//功能尚不明确？
	// 对转换后的源点云着色 (green)可视化.
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>	old_color(pcs, 0, 255, 0);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>output_color(input_cloud, 0, 0, 255);
	viewer->addPointCloud<pcl::PointXYZ>(match_cloud, old_color, "match cloud", v2);
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "match cloud");
	viewer->addPointCloud<pcl::PointXYZ>(pcs, output_color, "output cloud", v2);
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "output cloud");


	// 等待直到可视化窗口关闭
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(1000));
	}

	return (0);
}
#endif