点云配准——（2）四点法

点云配准的方法

基于局部特征配准：PFH、FPFH、3Dsc

基于全局特征配准：4pcs、super4pcs、kfpcs

基于概率配准：NDT

今天实现一下四点法配准

四点法配准原理：根据原始点云中不共面四点的仿射不变性，从目标点云中寻找，从而获得变换矩阵。

代码实现如下：

 

 

#include 
#include 
#include 
#include //法线
#include 
#include 
#include 
#include 
#include 
#include 
#include 


using namespace std;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud PointCloud;

void visualize_pcd(PointCloud::Ptr pcd_src, PointCloud::Ptr pcd_tgt, PointCloud::Ptr pcd_final)
{
	pcl::visualization::PCLVisualizer viewer("registration Viewer");
	//原始点云绿色
	pcl::visualization::PointCloudColorHandlerCustom src_h(pcd_src, 0, 255, 0);
	//目标点云红色
	pcl::visualization::PointCloudColorHandlerCustom tgt_h(pcd_tgt, 255, 0, 0);
	//匹配好的点云蓝色
	pcl::visualization::PointCloudColorHandlerCustom final_h(pcd_final, 0, 0, 255);

	viewer.setBackgroundColor(255, 255, 255);
	viewer.addPointCloud(pcd_src, src_h, "source cloud");
	viewer.addPointCloud(pcd_tgt, tgt_h, "target cloud");
	viewer.addPointCloud(pcd_final, final_h, "result cloud");
	while (!viewer.wasStopped())
	{
		viewer.spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}

}

int main(int argc, char** argv)
{
	//加载点云文件
	PointCloud::Ptr cloud_source(new PointCloud);
	PointCloud::Ptr cloud_target(new PointCloud);

	pcl::io::loadOBJFile("E:/vc14/data/hippo1.obj", *cloud_source);
	pcl::io::loadOBJFile("E:/vc14/data/hippo2.obj", *cloud_target);
	
	clock_t start = clock();


	//四点法配准
	PointCloud::Ptr pcs(new PointCloud);
	pcl::registration::FPCSInitialAlignment fpcs;
	
	fpcs.setInputSource(cloud_source);
	fpcs.setInputTarget(cloud_target);

	//参数设置
	fpcs.setApproxOverlap(0.7);
	fpcs.setDelta(0.01);
	fpcs.setMaxComputationTime(1000);
	fpcs.setNumberOfSamples(200);

	fpcs.align(*pcs);


	clock_t end = clock();

	cout << "时间为： " << (double)(end - start) / (double)CLOCKS_PER_SEC<

结果如下：

 

 

 

 

在对上述结果进行icp精配准

结果如下:

 

可以看出，变换后的点云与目标点云更切合，变换矩阵也更精确。

其他全局配准方法也同理，比如kfpcs配准。

完整的配准代码如下会稍后上传。