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

点云配准的方法

基于局部特征配准：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配准。

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