ICP算法详述 + 代码注释

2代码注释

#include <pcl/registration/ia_ransac.h>//点云的ransac算法头文件
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>//pcd输入输出头文件
#include <pcl/registration/icp.h>//点云icp算法头文件
#include <pcl/visualization/pcl_visualizer.h>//点云可视化头文件
#include <time.h>

typedef pcl::PointXYZ PointT; //重定义pcl::PointXYZ为PointT
typedef pcl::PointCloud<PointT> PointCloud; //重定义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.addPointCloud (pcd_src, src_h, "source cloud");    //source绿色
   viewer.addPointCloud (pcd_tgt, tgt_h, "tgt cloud");     //target红色
   //viewer.addPointCloud (pcd_final, final_h, "final cloud");  //final蓝色

   while (!viewer.wasStopped())
   {
       viewer.spinOnce(100);
       boost::this_thread::sleep(boost::posix_time::microseconds(100000));
   }
}

int main (int argc, char** argv)
{
   //加载点云文件
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src_o (new pcl::PointCloud<pcl::PointXYZ>);//原点云，待配准
   pcl::io::loadPCDFile ("./1556262676934636.pcd",*cloud_src_o);  
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tgt_o (new pcl::PointCloud<pcl::PointXYZ>);//目标点云
   pcl::io::loadPCDFile ("./1556262677035502.pcd",*cloud_tgt_o);
   clock_t start=clock();

   //icp配准
   pcl::PointCloud<pcl::PointXYZ>::Ptr icp_result (new pcl::PointCloud<pcl::PointXYZ>);
   pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;

   // 最大迭代次数
   icp.setMaximumIterations (50);
   // 两次变化矩阵之间的差值
   icp.setTransformationEpsilon (1e-10);
   // 均方误差
   icp.setEuclideanFitnessEpsilon (0.01);

   icp.setInputSource(cloud_src_o);//录入source点云
   icp.setInputTarget(cloud_tgt_o);//录入target点云
   pcl::PointCloud<pcl::PointXYZ> final_cloud;
   icp.align(final_cloud);//最终配准结果
   clock_t end = clock();
   cout << "total time: " << (double)(end-start) / (double)CLOCKS_PER_SEC << " s"<< endl;//输出配准所用时间
   std::cout << "ICP has converged:" << icp.hasConverged() << " score: " <<    icp.getFitnessScore() << std::endl;
   Eigen::Matrix4f icp_trans;
   icp_trans = icp.getFinalTransformation();
   std::cout <<endl<< "R,T="<<icp_trans << endl;//输出R、T
   //使用创建的变换对未过滤的输入点云进行变换
   pcl::transformPointCloud(*cloud_src_o, *icp_result, icp_trans);

   visualize_pcd(cloud_src_o,cloud_tgt_o,icp_result);
   return (0);
}

配准前点云


配准结果

刚性变换矩阵