点云配准方法之四点法

点云配准的方法

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

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

基于概率配准：NDT

今天实现一下四点法配准

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

代码实现如下：

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/obj_io.h>
#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 <iostream>
#include <pcl/registration/ia_fpcs.h>
#include <pcl/registration/ia_kfpcs.h>
#include <time.h>


using namespace std;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> 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<pcl::PointXYZ> src_h(pcd_src, 0, 255, 0);
	//目标点云红色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(pcd_tgt, 255, 0, 0);
	//匹配好的点云蓝色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> 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<pcl::PointXYZ,pcl::PointXYZ> 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<<endl;
	Eigen::Matrix4f tras = fpcs.getFinalTransformation();
	cout << "变换矩阵" << tras << endl;

	PointCloud::Ptr cloud_end(new PointCloud);
	pcl::transformPointCloud(*cloud_source,*cloud_end,tras);
	//pcl::io::loadOBJFile("E:/vc14/choose_bat/supre4pcs/super4pcs_fast.obj", *cloud_end);

	visualize_pcd(cloud_source, cloud_target, cloud_end);
	return (0);


}

结果如下：

点云配准方法之四点法

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

结果如下:

点云配准方法之四点法

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

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

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

点云配准方法之四点法

相关推荐