当前位置：首页 > news >正文

PCL 点云配准非线性加权最小二乘优化的点到面ICP算法（精配准）

news 2025/5/5 5:21:31

一、概述

1.1原理

1.2实现步骤

1.3应用场景

二、代码实现

2.1关键函数

2.1.1 法线计算函数

2.1.2 执行非线性加权点到面ICP

2.1.3可视化

2.2完整代码

三、实现效果

PCL点云算法汇总及实战案例汇总的目录地址链接：

PCL点云算法与项目实战案例汇总（长期更新）

一、概述

非线性加权最小二乘法优化的点到面ICP算法是一种精确的点云配准方法。通过引入加权的点到面误差计算，能够更加精准地进行点云之间的刚体配准。这种方法特别适用于存在局部误差较大或噪声较多的场景，通过加权处理能够有效减少噪声对配准结果的影响。

在这个算法中，TransformationEstimationPointToPlaneWeighted 被用于计算点到面的加权误差，这种非线性最小二乘优化能更好地处理不规则形状和噪声干扰。

1.1原理

非线性加权最小二乘法优化的ICP算法使用每对对应点的权重来减少误差较大的点对的影响。该算法通过求解目标点和源点之间的点到面距离的加权误差最小化公式，实现对配准变换矩阵的优化。其核心步骤包括以下几点：

计算源点云与目标点云之间的对应关系。
计算每对对应点之间的加权点到面距离误差。
使用非线性最小二乘法，最小化加权误差，迭代更新变换矩阵，直至收敛。

1.2实现步骤

初始化ICP对象：使用 IterativeClosestPointNonLinear 类和 TransformationEstimationPointToPlaneWeighted 类进行ICP配准。
配准迭代：设置配准参数，进行35次ICP迭代。
变换矩阵输出：输出最终的变换矩阵，并将变换应用到源点云。
可视化：通过 PCLVisualizer 可视化源点云、目标点云和变换后的点云。

1.3应用场景

动驾驶场景的点云配准：在自动驾驶中，通常需要将来自多个激光雷达的点云进行配准。
工业检测：通过将不同时间的点云进行精确配准，能够检测物体的形变或位移。
三维重建：多视角的点云需要精确对齐，以实现更好的三维模型重建。

二、代码实现

2.1关键函数

2.1.1 法线计算函数

pcl::PointCloud<pcl::PointNormal>::Ptr compute_normals(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud)
{pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);n.setNumberOfThreads(8);           // 使用8线程加速法线计算n.setInputCloud(input_cloud);      // 输入点云n.setSearchMethod(tree);           // 设置KD树搜索方法n.setKSearch(10);                  // 设置K近邻搜索n.compute(*normals);               // 计算法线// 将点云和法线信息拼接pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);pcl::concatenateFields(*input_cloud, *normals, *cloud_with_normals);return cloud_with_normals;
}

2.1.2 执行非线性加权点到面ICP

void run_weighted_icp(pcl::PointCloud<pcl::PointNormal>::Ptr& source_normal,pcl::PointCloud<pcl::PointNormal>::Ptr& target_normal,Eigen::Matrix4f& final_transform, pcl::PointCloud<pcl::PointNormal>::Ptr& icp_cloud)
{// 初始化ICP对象pcl::IterativeClosestPointNonLinear<pcl::PointNormal, pcl::PointNormal> icp;// 设置加权的点到面ICP变换估计typedef pcl::registration::TransformationEstimationPointToPlaneWeighted <pcl::PointNormal, pcl::PointNormal> PointToPlane;std::shared_ptr<PointToPlane> point_to_plane(new PointToPlane);icp.setTransformationEstimation(point_to_plane);// 设置ICP参数icp.setInputSource(source_normal);icp.setInputTarget(target_normal);icp.setTransformationEpsilon(1e-10);    // 为终止条件设置最小转换差异icp.setMaxCorrespondenceDistance(10);   // 设置最大对应点距离icp.setEuclideanFitnessEpsilon(0.0001); // 设置收敛条件：误差阈值icp.setMaximumIterations(35);           // 最大迭代次数// 执行ICP配准icp.align(*icp_cloud);final_transform = icp.getFinalTransformation();std::cout << "ICP 配准完成，最终分数: " << icp.getFitnessScore() << std::endl;
}

2.1.3可视化

// 可视化
void visualize_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr& source,pcl::PointCloud<pcl::PointXYZ>::Ptr& target,pcl::PointCloud<pcl::PointXYZ>::Ptr& aligned)
{boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("配准结果"));viewer->setBackgroundColor(0, 0, 0);pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target, 255, 0, 0);viewer->addPointCloud(target, target_color, "target cloud");//pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(source, 0, 255, 0);//viewer->addPointCloud(source, source_color, "source cloud");pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> aligned_color(aligned, 0, 0, 255);viewer->addPointCloud(aligned, aligned_color, "aligned cloud");viewer->spin();
}

2.2完整代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d_omp.h> // 使用OMP加速法向量计算
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/transformation_estimation_point_to_plane_weighted.h> // 加权头文件
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/console/time.h>  using namespace std;// 计算法线
pcl::PointCloud<pcl::PointNormal>::Ptr compute_normals(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud)
{pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);n.setNumberOfThreads(8);           // 使用8线程加速法线计算n.setInputCloud(input_cloud);      // 输入点云n.setSearchMethod(tree);           // 设置KD树搜索方法n.setKSearch(10);                  // 设置K近邻搜索n.compute(*normals);               // 计算法线// 将点云和法线信息拼接pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);pcl::concatenateFields(*input_cloud, *normals, *cloud_with_normals);return cloud_with_normals;
}// 执行非线性加权点到面ICP
void run_weighted_icp(pcl::PointCloud<pcl::PointNormal>::Ptr& source_normal,pcl::PointCloud<pcl::PointNormal>::Ptr& target_normal,Eigen::Matrix4f& final_transform, pcl::PointCloud<pcl::PointNormal>::Ptr& icp_cloud)
{// 初始化ICP对象pcl::IterativeClosestPointNonLinear<pcl::PointNormal, pcl::PointNormal> icp;// 设置加权的点到面ICP变换估计typedef pcl::registration::TransformationEstimationPointToPlaneWeighted <pcl::PointNormal, pcl::PointNormal> PointToPlane;std::shared_ptr<PointToPlane> point_to_plane(new PointToPlane);icp.setTransformationEstimation(point_to_plane);// 设置ICP参数icp.setInputSource(source_normal);icp.setInputTarget(target_normal);icp.setTransformationEpsilon(1e-10);    // 为终止条件设置最小转换差异icp.setMaxCorrespondenceDistance(0.1);   // 设置最大对应点距离icp.setEuclideanFitnessEpsilon(0.0001); // 设置收敛条件：误差阈值icp.setMaximumIterations(50);           // 最大迭代次数// 执行ICP配准icp.align(*icp_cloud);final_transform = icp.getFinalTransformation();std::cout << "ICP 配准完成，最终分数: " << icp.getFitnessScore() << std::endl;
}// 可视化
void visualize_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr& source,pcl::PointCloud<pcl::PointXYZ>::Ptr& target,pcl::PointCloud<pcl::PointXYZ>::Ptr& aligned)
{boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("配准结果"));viewer->setBackgroundColor(0, 0, 0);pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target, 255, 0, 0);viewer->addPointCloud(target, target_color, "target cloud");/*pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(source, 0, 255, 0);viewer->addPointCloud(source, source_color, "source cloud");*/pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> aligned_color(aligned, 0, 0, 255);viewer->addPointCloud(aligned, aligned_color, "aligned cloud");viewer->spin();
}int main()
{pcl::console::TicToc time;// 读取点云数据pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);pcl::io::loadPCDFile<pcl::PointXYZ>("1.pcd", *target);pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);pcl::io::loadPCDFile<pcl::PointXYZ>("2.pcd", *source);// 计算源点云和目标点云的法线pcl::PointCloud<pcl::PointNormal>::Ptr targetNormal = compute_normals(target);pcl::PointCloud<pcl::PointNormal>::Ptr sourceNormal = compute_normals(source);// 执行非线性加权ICPpcl::PointCloud<pcl::PointNormal>::Ptr icp_cloud(new pcl::PointCloud<pcl::PointNormal>);Eigen::Matrix4f final_transform;run_weighted_icp(sourceNormal, targetNormal, final_transform, icp_cloud);// 输出变换矩阵std::cout << "变换矩阵：\n" << final_transform << std::endl;// 变换点云并进行可视化pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>);pcl::transformPointCloud(*source, *aligned, final_transform);visualize_registration(source, target, aligned);return 0;
}