点云配准是将两个或多个点云数据集融合到一个统一的坐标系统中的过程。这通常是为了创建一个完整的模型或融合从不同视角采集的数据。
点云配准一般分为粗配准和精配准，粗配准指的是在两幅点云之间的变换完全未知的情况下进行较为粗糙的配准，目的主要是为精配准提供较好的变换初值；精配准则是给定一个初始变换，进一步优化得到更精确的变换。这里我们主要介绍精配准。
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/io/ply_io.h>//pcd输入输出头文件
#include <pcl/registration/icp.h>//点云icp算法头文件
#include <pcl/registration/ndt.h>//点云NDT算法头文件
#include <pcl/registration/ia_ransac.h>//点云ransac迭代对齐算法(SAC-IA)算法头文件
#include <pcl/registration/gicp.h>//点云GICP算法的头文件
#include <pcl/visualization/pcl_visualizer.h>//点云可视化头文件
#include <time.h>
#include <boost/thread.hpp>
#include <pcl/features/normal_3d_omp.h>
typedef pcl::PointXYZ PointT; //重定义pcl::PointXYZ为PointT
typedef pcl::PointCloud<PointT> PointCloud; //重定义pcl::PointCloud<PointT>为PointCloud
//点云配准类型
typedef pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> ICP;
typedef pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> NDT;
typedef pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> SAC_IA;
typedef pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> GICP;
//FPFH特征
typedef pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> FPFHEstimation;
typedef pcl::PointCloud<pcl::FPFHSignature33> FPFH;
//法线估计
typedef pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> NormalEstimation;
typedef pcl::PointCloud<pcl::Normal> Normal;
typedef pcl::search::KdTree<pcl::PointXYZ> KdTreeT;//点云可视化
void visualize_pcd(PointCloud::Ptr pcd_src,PointCloud::Ptr pcd_tgt,PointCloud::Ptr pcd_final)
{pcl::visualization::PCLVisualizer viewer("registration Viewer");// 设置背景viewer.setBackgroundColor(1, 1, 1);// 添加坐标轴到可视化对象，参数指定轴的大小（长度），这里设置为1.0单位长度viewer.addCoordinateSystem(0.5);// 设置相机位置和方向viewer.initCameraParameters();pcl::visualization::PointCloudColorHandlerCustom<PointT> src_h(pcd_src, 0, 255, 0);pcl::visualization::PointCloudColorHandlerCustom<PointT> tgt_h(pcd_tgt, 255, 0, 0);pcl::visualization::PointCloudColorHandlerCustom<PointT> 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(1000));}
}void printinfo(Eigen::Matrix4f icp_trans, Eigen::Matrix3f rotation,Eigen::Vector3f translation, Eigen::Vector3f euler_angles_deg,Eigen::Quaternionf quaternion)
{cout.setf(ios::fixed);cout.precision(5);cout << endl << "Transformation matrix:" << endl << icp_trans << endl;//输出变换矩阵cout << endl << "Rotation matrix:" << endl << rotation << endl;cout << endl << "Translation vector:" << endl << translation << endl;cout << endl << "Euler Angle:" << endl;cout << endl << "roll(x):" << euler_angles_deg[2] << endl;cout << endl << "pitch(y):" << euler_angles_deg[1] << endl;cout << endl << "yaw(z):" << euler_angles_deg[0] << endl;// 输出四元数,由于欧拉角时常出现万向节，所以可以使用四元数来表示旋转，这样更精确cout << endl << "Rotation quaternion: " << endl<< "w = " << quaternion.w() << endl<< ", x = " << quaternion.x() << endl<< ", y = " << quaternion.y() << endl<< ", z = " << quaternion.z() << endl;
}void downsampling(PointCloud::Ptr cloud_src_o, PointCloud::Ptr cloud_tgt_o,pcl::VoxelGrid<pcl::PointXYZ>::Ptr voxel_grid)
{float leaf = 0.005f;voxel_grid->setLeafSize(leaf, leaf, leaf); // 示例叶子大小voxel_grid->setInputCloud(cloud_src_o);voxel_grid->filter(*cloud_src_o);voxel_grid->setInputCloud(cloud_tgt_o);voxel_grid->filter(*cloud_tgt_o);
}
void normalestimation(NormalEstimation ne, Normal::Ptr src_normals,Normal::Ptr tgt_normals, KdTreeT::Ptr tree,PointCloud::Ptr cloud_src_o, PointCloud::Ptr cloud_tgt_o)
{//创建一个NormalEstimationOMP对象, 进行法线计算;ne.setSearchMethod(tree);ne.setKSearch(20);ne.setInputCloud(cloud_src_o);ne.compute(*src_normals);ne.setInputCloud(cloud_tgt_o);ne.compute(*tgt_normals);cout << "原点云法线计算完成" << endl << *src_normals << endl;cout << "目标点云法线计算完成" << endl << *tgt_normals << endl;
}void fpfh_features(FPFHEstimation fpfh, FPFH::Ptr src_fpfhs,FPFH::Ptr tgt_fpfhs, PointCloud::Ptr cloud_src_o,PointCloud::Ptr cloud_tgt_o, KdTreeT::Ptr tree,Normal::Ptr src_normals, Normal::Ptr tgt_normals)
{fpfh.setSearchMethod(tree);fpfh.setRadiusSearch(0.05); // Use a radius search instead of a KdTree searchfpfh.setInputCloud(cloud_src_o);fpfh.setInputNormals(src_normals);fpfh.compute(*src_fpfhs);fpfh.setInputCloud(cloud_tgt_o);fpfh.setInputNormals(tgt_normals);fpfh.compute(*tgt_fpfhs);cout << "原点云特征计算完成" << *src_fpfhs << endl;cout << "目标点云特征计算完成" << *tgt_fpfhs << endl;
}GICP& gicp_registration(GICP& gicp, PointCloud& final_cloud,PointCloud::Ptr cloud_src_o, PointCloud::Ptr cloud_tgt_o)
{gicp.setInputSource(cloud_src_o);gicp.setInputTarget(cloud_tgt_o);// 配准时最大迭代次数gicp.setMaximumIterations(50);// 两次变化矩阵之间的差异小于这个阈值时，就认为已经收敛，停止迭代gicp.setTransformationEpsilon(1e-6);// 对应点之间的最大距离gicp.setMaxCorrespondenceDistance(0.05);// 采用随机采样一致性方法进行配准gicp.setRANSACOutlierRejectionThreshold(1.5);// 最小内点比例。在RANSAC配准方法中，当内点的比例小于此值时，认为配准失败。gicp.setRANSACIterations(20);// 执行配准，并将结果存储在Final中gicp.align(final_cloud);return gicp;}
SAC_IA& sacia_registration(SAC_IA& sac_ia, PointCloud& final_cloud,PointCloud::Ptr cloud_src_o, PointCloud::Ptr cloud_tgt_o,FPFH::Ptr& src_fpfhs, FPFH::Ptr& tgt_fpfhs)
{sac_ia.setInputSource(cloud_src_o);sac_ia.setSourceFeatures(src_fpfhs);sac_ia.setInputTarget(cloud_tgt_o);sac_ia.setTargetFeatures(tgt_fpfhs);// 设置SAC-IA配准的参数sac_ia.setMinSampleDistance(0.05f);sac_ia.setMaxCorrespondenceDistance(0.01f);sac_ia.setMaximumIterations(500);// 创建一个空的PointCloud对象来接收结果pcl::PointCloud<pcl::PointXYZ> final_registration;// 执行配准，并将结果存储在final_registration中sac_ia.align(final_registration);return sac_ia;
}
NDT& ndt_registration(NDT& ndt, PointCloud& final_cloud,PointCloud::Ptr cloud_src_o, PointCloud::Ptr cloud_tgt_o)
{// 配置NDTndt.setTransformationEpsilon(0.1);ndt.setStepSize(0.5);ndt.setResolution(2.0);ndt.setMaximumIterations(20);ndt.setInputSource(cloud_src_o);ndt.setInputTarget(cloud_tgt_o);ndt.align(final_cloud);return ndt;
}ICP& icp_registration(ICP& icp, PointCloud& final_cloud,PointCloud::Ptr cloud_src_o, PointCloud::Ptr cloud_tgt_o)
{icp.setMaximumIterations(50);// 两次变化矩阵之间的差值icp.setTransformationEpsilon(1e-10);// 均方误差icp.setEuclideanFitnessEpsilon(0.01);icp.setInputSource(cloud_src_o);//录入source点云icp.setInputTarget(cloud_tgt_o);//录入target点云icp.align(final_cloud);//最终配准结果return icp;
}
int main(int argc, char** argv)
{//加载点云文件PointCloud::Ptr cloud_src_o(new pcl::PointCloud<pcl::PointXYZ>);//原点云，待配准pcl::io::loadPLYFile("0.ply", *cloud_src_o);PointCloud::Ptr cloud_tgt_o(new pcl::PointCloud<pcl::PointXYZ>);//目标点云pcl::io::loadPLYFile("1.ply", *cloud_tgt_o);clock_t start = clock();//下采样pcl::VoxelGrid<pcl::PointXYZ>::Ptr voxel_grid(new pcl::VoxelGrid<pcl::PointXYZ>);downsampling(cloud_src_o, cloud_tgt_o, voxel_grid);//创建一个NormalEstimationOMP对象, 进行法线计算NormalEstimation ne;Normal::Ptr src_normals(new pcl::PointCloud<pcl::Normal>);Normal::Ptr tgt_normals(new pcl::PointCloud<pcl::Normal>);KdTreeT::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());normalestimation(ne, src_normals, tgt_normals, tree, cloud_src_o, cloud_tgt_o);//计算FPFH特征/*FPFHEstimation fpfh;FPFH::Ptr src_fpfhs(new pcl::PointCloud<pcl::FPFHSignature33>());FPFH::Ptr tgt_fpfhs(new pcl::PointCloud<pcl::FPFHSignature33>());fpfh_features(fpfh, src_fpfhs, tgt_fpfhs, cloud_src_o, cloud_tgt_o, tree, src_normals, tgt_normals);*/// 点云配准PointCloud::Ptr icp_result(new pcl::PointCloud<pcl::PointXYZ>);PointCloud final_cloud;ICP icp;ICP& reg_p = icp_registration(icp, final_cloud, cloud_src_o, cloud_tgt_o);/*NDT ndt;NDT & reg_p = ndt_registration(ndt, final_cloud, cloud_src_o, cloud_tgt_o);*//*GICP gicp;GICP& reg_p = gicp_registration(gicp, final_cloud, cloud_src_o, cloud_tgt_o);*///SAC_IA sac_ia;//SAC_IA& reg_p = sacia_registration(sac_ia, final_cloud, cloud_src_o, cloud_tgt_o, src_fpfhs, tgt_fpfhs);clock_t end = clock();cout << "total time: " << (double)(end - start) / (double)CLOCKS_PER_SEC << " s" << endl;//输出配准所用时间cout << "ICP has converged:" << reg_p.hasConverged() << " score: " << reg_p.getFitnessScore() << std::endl;Eigen::Matrix4f reg_p_trans;// 变换矩阵reg_p_trans = reg_p.getFinalTransformation();// 平移向量Eigen::Vector3f translation = reg_p_trans.block<3, 1>(0, 3);// 旋转矩阵Eigen::Matrix3f rotation = reg_p_trans.block<3, 3>(0, 0);// 转换为四元数Eigen::Quaternionf quaternion(rotation);// 使用四元数重新计算欧拉角Eigen::Vector3f euler_angles_rad = quaternion.toRotationMatrix().eulerAngles(2, 1, 0);//获取欧拉角（弧度）//Eigen::Vector3f euler_angles_rad = rotation.eulerAngles(0, 1, 2);// 将弧度转换为角度Eigen::Vector3f euler_angles_deg = euler_angles_rad * 180.0 / M_PI;printinfo(reg_p_trans, rotation, translation, euler_angles_deg, quaternion);//使用创建的变换对未过滤的输入点云进行变换pcl::transformPointCloud(*cloud_src_o, *icp_result, reg_p_trans);visualize_pcd(cloud_src_o, cloud_tgt_o, icp_result);return (0);
}
