PCL配准——粗配准+ICP

由于ICP本身属于精配准方法，在PCL文档中也只给出了简单的使用方式。在此，我结合自己的了解稍微拓展。对于一个点云数据我们先执行粗配准，然后再通过ICP进行精配准。

算法流程

粗配准（SAC-IA）

通过特征点先计算一个大概的初值，帮助ICP收敛到一个更准确的位置。总体流程是 空间均匀采样 → 特征匹配 → RANSAC 评估刚性变换。，PCL中可以使用SAC-IA 算法来完成上面的粗配准流程。该算法流程如下：

采样与特征计算
从待配准点云中按最小距离约束选取空间上分散的采样点，并为这些采样点计算 FPFH 等局部几何特征，以保证特征具有区分性。
特征匹配建立候选对应
根据采样点的 FPFH 特征，在目标点云中查找特征相似的点，形成源点云与目标点云之间的候选对应点对集合，其中包含大量错误匹配。
RANSAC 估计初始刚性变换
通过 RANSAC 从候选对应点对中随机采样最小点集，估计刚性变换，并以变换后点对的距离误差作为一致性评价标准，选择支持度最高的变换作为粗配准结果。

精配准

精配准算法在PCL的教程文档里介绍了两个，一个是ICP还有一个是NDT。

ICP

ICP算法假定我们当前已经有一个较为合理的初始位姿。算法在此基础上通过最小二乘法进一步实现更精确的配准。

建立对应关系
对源点云中每个点，在目标点云中通过最近邻搜索（KD-Tree）找到对应点，并剔除距离过大的对应。
估计刚体变换
基于当前对应点对，采用最小二乘法（SVD / 点到平面）计算最优的旋转矩阵 𝑅和平移向量𝑡。
更新并迭代
将变换应用到源点云，重复步骤 1–2，直到变换收敛或达到最大迭代次数。

PCL中的ICP算法如果不指定初始变换矩阵，默认为单位矩阵。

实验

这里我们使用兔子点云，按照上面的流程完整的走一遍粗配准+精配准的流程。

首先对点云进行采样。
然后计算特征并进行粗配准
根据粗配准结果，使用ICP再次配准。此外，可以考虑使用带法线版本的ICP，因为这里的变换还涉及到了旋转。匹配效果非常好，由于点高度重合导致显示的颜色也已经发生改变了。我们没有设置其他的参数，直接使用默认参数。

完整代码

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <corecrt_io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/surface/on_nurbs/fitting_surface_tdm.h>
#include <pcl/surface/on_nurbs/fitting_curve_2d_asdm.h>
#include <pcl/surface/on_nurbs/triangulation.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/ia_ransac.h>
#include <boost/thread/thread.hpp>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/filters/uniform_sampling.h>


void filterCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr filteredCloud)
{
	pcl::UniformSampling<pcl::PointXYZ> us;

	us.setInputCloud(cloud);
	us.setNegative(false);
	us.setRadiusSearch(0.5f);
	us.filter(*filteredCloud);
}

void calCulateDescor(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh)
{
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
	pcl::PointCloud<pcl::Normal>::Ptr cloudWithNormal(new pcl::PointCloud<pcl::Normal>());
	tree->setInputCloud(cloud);
	pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;
	ne.setNumberOfThreads(4);
	ne.setInputCloud(cloud);
	ne.setSearchMethod(tree);
	ne.setKSearch(10);
	ne.compute(*cloudWithNormal);
	//ne.setViewPoint
	//计算特征
	pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fe;
	fe.setInputCloud(cloud);   // 设置法向量
	fe.setSearchMethod(tree);       // 使用KD树加速搜索
	fe.setKSearch(10);              // 设置K近邻点数量为10
	fe.setInputNormals(cloudWithNormal);
	fe.setNumberOfThreads(4);
	fe.compute(*fpfh);
}


// 点云可视化函数
void visualizePointCloudsRegistration(pcl::PointCloud<pcl::PointXYZ>::Ptr source, pcl::PointCloud<pcl::PointXYZ>::Ptr target,
	pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud)
{
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Registration Result"));
	int v1 = 0, v2 = 1;
	viewer->createViewPort(0, 0, 0.5, 1, v1);  // 创建左视口
	viewer->createViewPort(0.5, 0, 1, 1, v2);  // 创建右视口
	viewer->setBackgroundColor(0, 0, 0, v1);   // 设置背景颜色
	viewer->setBackgroundColor(0.05, 0, 0, v2);

	// 设置点云的颜色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(source, 255, 0, 0);  // 源点云颜色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(target, 0, 0, 255);  // 目标点云颜色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transe(icp_cloud, 255, 0, 0); // 配准后的点云颜色

	viewer->addPointCloud(source, src_h, "source cloud", v1);   // 添加源点云
	viewer->addPointCloud(target, tgt_h, "target cloud", v1);   // 添加目标点云
	viewer->addPointCloud(target, tgt_h, "target cloud1", v2);  // 目标点云
	viewer->addPointCloud(icp_cloud, transe, "pcs cloud", v2);  // 添加配准后的点云

	// 启动可视化窗口
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(10000));
	}
}



int main()
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>()),
		source_filtered(new pcl::PointCloud<pcl::PointXYZ>()),
		target(new pcl::PointCloud<pcl::PointXYZ>()),
		target_filtered(new pcl::PointCloud<pcl::PointXYZ>()),
		aligned_cloud(new pcl::PointCloud<pcl::PointXYZ>()),
		final_cloud(new pcl::PointCloud<pcl::PointXYZ>());

	pcl::PointCloud<pcl::PointXYZ>::Ptr coraseCloud(new pcl::PointCloud<pcl::PointXYZ>());
	if (pcl::io::loadPCDFile("./rabbit.pcd", *source) != 0)
	{
		std::cout << "文件读取失败" << std::endl;
	}
	Eigen::Affine3f trans = Eigen::Affine3f::Identity();
	trans.translation() << 0.1, 2, 1;
	trans.rotate(Eigen::AngleAxisf(30.0f * M_PI / 180.0f, Eigen::Vector3f::UnitZ()));
	pcl::transformPointCloud(*source, *target, trans);
	//visualizePointCloudsRegistration(source, target, target);
	//滤波
	filterCloud(source, source_filtered);
	filterCloud(target, target_filtered);


	visualizePointCloudsRegistration(source_filtered, target_filtered, target_filtered);
	//
	////分别计算特征
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh_source(new pcl::PointCloud<pcl::FPFHSignature33>);
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh_target(new pcl::PointCloud<pcl::FPFHSignature33>);
	calCulateDescor(source_filtered, fpfh_source);
	calCulateDescor(target_filtered, fpfh_target);

	std::cout << "特征计算完成" << std::endl;
	//
	////进行粗匹配
	pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sa;

	sa.setInputSource(source_filtered);
	sa.setSourceFeatures(fpfh_source);
	sa.setInputTarget(target_filtered);
	sa.setTargetFeatures(fpfh_target);
	sa.setMinSampleDistance(0.1);                  // 设置样本点之间的最小距离
	sa.setCorrespondenceRandomness(6);
	sa.align(*aligned_cloud);
	//显示粗匹配结果
	visualizePointCloudsRegistration(source_filtered, target_filtered, aligned_cloud);
	//ICP
	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;

	icp.setInputSource(source_filtered);
	icp.setInputTarget(target_filtered);
	icp.align(*final_cloud, sa.getFinalTransformation());
	//最终结果

	//我们用的是采样的点  但实际上 变换的是最初的点云
	pcl::transformPointCloud(*source, *final_cloud, icp.getFinalTransformation());
	visualizePointCloudsRegistration(source, target, final_cloud);
}

posted @ 2025-12-20 22:31 Ytytyty 阅读(6) 评论(0) 收藏举报

刷新页面返回顶部

Ytytyty

PCL配准——粗配准+ICP

算法流程

粗配准（SAC-IA）

精配准

ICP

实验

公告