SAC-IA（Sample Consensus Initial Alignment）和ICP（Iterative Closest Point）是点云配准中常用的两个算法，可以使用 SAC-IA进行粗配准结合ICP进行精配准。
1.核心思想
-
SAC-IA：
- 作用：SAC-IA用于粗配准，估计两个点云之间的初始变换矩阵。
- 流程：
- 随机选择源点云中的一组点作为初始匹配点对。
- 在目标点云中查找与这组点最接近的点。
- 使用基于快速点特征直方图（FPFH）的SAC-IA算法，实现粗配准。
- 返回初始变换矩阵。
- 特点：SAC-IA对于匹配的阈值和点对的质量有一定要求。
-
ICP：
- 作用：ICP用于精确配准，进一步优化位姿。
- 流程：
- 使用SAC-IA得到的初始变换矩阵。
- 迭代优化变换矩阵，使点云对齐。
- ICP通过最小化点云之间的距离来优化变换矩阵。
- 特点：ICP适用于点云的局部配准，但容易陷入局部最优解。
2.整体流程
- 首先，使用SAC-IA估计初始的点云位姿。
- 然后，使用ICP迭代优化变换矩阵，使点云更准确地对齐。
3.相关程序
#include <pcl/registration/ia_ransac.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/search/kdtree.h>
#include <pcl/io/ply_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <time.h>
#include <pcl/registration/registration.h>
#include <vector>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/registration/registration.h>
using namespace std;
using pcl::NormalEstimation;
using pcl::search::KdTree;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
int
main(int argc, char** argv)
{
PointCloud::Ptr cloud_src_o(new PointCloud);
pcl::io::loadPLYFile("C:/Users/17927/Desktop/QYQ/D1-2.ply", *source);
PointCloud::Ptr cloud_tgt_o(new PointCloud);
pcl::io::loadPLYFile("C:/Users/17927/Desktop/QYQ/D1-1.ply", *target);
clock_t start = clock();
pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne_src;
ne_src.setInputCloud(source);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_src(new pcl::search::KdTree<pcl::PointXYZ>());
ne_src.setSearchMethod(tree_src);
pcl::PointCloud<pcl::Normal>::Ptr cloud_src_normals(new pcl::PointCloud<pcl::Normal>);
ne_src.setRadiusSearch(0.02);
ne_src.setNumberOfThreads(6);
ne_src.compute(*source_normals);
pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne_tgt;
ne_tgt.setInputCloud(target);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_tgt(new pcl::search::KdTree<pcl::PointXYZ>());
ne_tgt.setSearchMethod(tree_tgt);
pcl::PointCloud<pcl::Normal>::Ptr cloud_tgt_normals(new pcl::PointCloud<pcl::Normal>);
ne_tgt.setRadiusSearch(0.02);
ne_tgt.setNumberOfThreads(6);
ne_tgt.compute(*target_normals);
//----------------------计算FPFH------------------------------
pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_src;
fpfh_src.setInputCloud(source);
fpfh_src.setInputNormals(source_normals);
pcl::search::KdTree<PointT>::Ptr tree_src_fpfh(new pcl::search::KdTree<PointT>);
fpfh_src.setSearchMethod(tree_src_fpfh);
pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_src(new pcl::PointCloud<pcl::FPFHSignature33>());
fpfh_src.setRadiusSearch(0.05);
fpfh_src.compute(*fpfhs_src);
std::cout << "compute *source fpfh" << endl;
pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_tgt;
fpfh_tgt.setInputCloud(target);
fpfh_tgt.setInputNormals(target_normals);
pcl::search::KdTree<PointT>::Ptr tree_tgt_fpfh(new pcl::search::KdTree<PointT>);
fpfh_tgt.setSearchMethod(tree_tgt_fpfh);
pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_tgt(new pcl::PointCloud<pcl::FPFHSignature33>());
fpfh_tgt.setRadiusSearch(0.05);
fpfh_tgt.compute(*fpfhs_tgt);
std::cout << "compute *target fpfh" << endl;
//---------------------------SAC配准-----------------------------
pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> scia;
scia.setInputSource(source);
scia.setInputTarget(target);
scia.setSourceFeatures(fpfhs_src);
scia.setTargetFeatures(fpfhs_tgt);
PointCloud::Ptr sac_result(new PointCloud);
scia.align(*sac_result);
std::cout << "sac has converged:" << scia.hasConverged() << " score: " << scia.getFitnessScore() << endl;
Eigen::Matrix4f sac_trans;
sac_trans = scia.getFinalTransformation();
std::cout << sac_trans << endl;
clock_t sac_time = clock();
//--------------------------ICP配准----------------------------
PointCloud::Ptr icp_result(new PointCloud);
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(source);
icp.setInputTarget(target);
icp.setMaxCorrespondenceDistance(100);
icp.setMaximumIterations(100);
icp.setTransformationEpsilon(1e-10);
icp.setEuclideanFitnessEpsilon(0.001);
icp.align(*icp_result, sac_trans);
clock_t end = clock();
cout << "total time: " << (double)(end - start) / (double)CLOCKS_PER_SEC << " s" << endl;
cout << "sac time: " << (double)(sac_time - start) / (double)CLOCKS_PER_SEC << " s" << endl;
cout << "icp time: " << (double)(end - sac_time) / (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 << icp_trans << endl;
pcl::transformPointCloud(*cloud_src_o, *icp_result, icp_trans);
pcl::io::savePLYFileASCII("sac-ia_icp.ply", *icp_result);
//--------------------可视化------------------------
boost::shared_ptr<pcl::visualization::PCLVisualizer>
viewer_final(new pcl::visualization::PCLVisualizer("配准结果"));
viewer_final->setBackgroundColor(0, 0, 0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
target_color(target, 255, 0, 0);
viewer_final->addPointCloud<pcl::PointXYZ>(target, target_color, "target cloud");
viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1, "target cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
output_color(icp_result, 0, 0, 255);
viewer_final->addPointCloud<pcl::PointXYZ>(icp_result, output_color, "output cloud");
viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1, "output cloud");
while (!viewer_final->wasStopped())
{
viewer_final->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
return (0);
}
4.实验结果
