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

PCL 点云配准 GICP算法（精配准）

news 2024/10/22 11:39:17

一、概述

1.1原理

1.2实现步骤

1.3应用场景

二、代码实现

2.1关键函数

2.1.1 执行GICP配准

2.1.2 可视化

2.2完整代码

三、实现效果

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

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

一、概述

广义迭代最近点（Generalized Iterative Closest Point，GICP）算法是一种点云精配准算法，基于ICP（迭代最近点）算法的扩展。GICP不同于传统的ICP，它在进行点云匹配时，不仅考虑了每个点的位置，还将其周围局部结构的协方差矩阵引入到匹配过程中，使得配准过程更具鲁棒性。

1.1原理

GICP的目标是通过迭代的方式，最小化源点云和目标点云之间的误差。该误差以广义点到点（Point-to-Point）和点到平面（Point-to-Plane）距离的组合形式存在，并通过使用局部结构的协方差矩阵进行加权，以实现更精确的配准。

1.2实现步骤

加载目标点云和源点云。
初始化GICP算法，并设置其参数，包括KD树加速方法。
执行GICP配准，计算刚体变换矩阵。
使用变换矩阵对源点云进行刚性变换。
可视化配准前后点云结果。

1.3应用场景

精细化的3D点云配准，适用于复杂几何结构的场景。
机器人和自动驾驶中的环境感知和物体检测。
在噪声较大或局部区域扭曲较明显时，相较于ICP更具鲁棒性。

二、代码实现

2.1关键函数

2.1.1 执行GICP配准

// 执行GICP配准，并返回最终的配准结果和变换矩阵
void perform_gicp(pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& icp_cloud, Eigen::Matrix4f& transformation_matrix, int iterations)
{// 创建GICP对象pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> gicp;// 设置KD树加速源点云与目标点云的匹配搜索pcl::search::KdTree<pcl::PointXYZ>::Ptr tree1(new pcl::search::KdTree<pcl::PointXYZ>);tree1->setInputCloud(source_cloud);pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2(new pcl::search::KdTree<pcl::PointXYZ>);tree2->setInputCloud(target_cloud);gicp.setSearchMethodSource(tree1);gicp.setSearchMethodTarget(tree2);// 设置GICP的输入源和目标点云gicp.setInputSource(source_cloud);gicp.setInputTarget(target_cloud);// 设置GICP的配准参数gicp.setMaxCorrespondenceDistance(100);      // 设置最大匹配距离gicp.setTransformationEpsilon(1e-10);        // 设置最小变换差异作为终止条件gicp.setEuclideanFitnessEpsilon(0.001);      // 设置收敛条件：均方误差和gicp.setMaximumIterations(iterations);       // 设置最大迭代次数// 执行GICP配准gicp.align(*icp_cloud);// 判断配准是否收敛并输出相关信息if (gicp.hasConverged()) {std::cout << "\nGICP has converged, score is " << gicp.getFitnessScore() << std::endl;transformation_matrix = gicp.getFinalTransformation();std::cout << "变换矩阵：\n" << transformation_matrix << std::endl;} else {PCL_ERROR("GICP配准未能收敛。\n");exit(-1); // 如果未收敛，退出程序}
}

2.1.2 可视化

// 可视化配准前后的点云结果，使用不同颜色显示源点云、目标点云和配准后的点云
void visualize_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr& source, pcl::PointCloud<pcl::PointXYZ>::Ptr& target, pcl::PointCloud<pcl::PointXYZ>::Ptr& result)
{// 创建可视化窗口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> result_color(result, 0, 0, 255);viewer->addPointCloud(result, result_color, "result cloud");// 启动可视化viewer->spin();
}

2.2完整代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/gicp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/console/time.h>using namespace std;// 执行GICP配准，并返回最终的配准结果和变换矩阵
void perform_gicp(pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud,pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud,pcl::PointCloud<pcl::PointXYZ>::Ptr& icp_cloud,Eigen::Matrix4f& transformation_matrix, int iterations)
{// 创建GICP对象pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> gicp;// 设置KD树加速源点云与目标点云的匹配搜索pcl::search::KdTree<pcl::PointXYZ>::Ptr tree1(new pcl::search::KdTree<pcl::PointXYZ>);tree1->setInputCloud(source_cloud);pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2(new pcl::search::KdTree<pcl::PointXYZ>);tree2->setInputCloud(target_cloud);gicp.setSearchMethodSource(tree1);gicp.setSearchMethodTarget(tree2);// 设置GICP的输入源和目标点云gicp.setInputSource(source_cloud);gicp.setInputTarget(target_cloud);// 设置GICP的配准参数gicp.setMaxCorrespondenceDistance(100);      // 设置最大匹配距离gicp.setTransformationEpsilon(1e-10);        // 设置最小变换差异作为终止条件gicp.setEuclideanFitnessEpsilon(0.001);      // 设置收敛条件：均方误差和gicp.setMaximumIterations(iterations);       // 设置最大迭代次数// 执行GICP配准gicp.align(*icp_cloud);// 判断配准是否收敛并输出相关信息if (gicp.hasConverged()) {std::cout << "\nGICP has converged, score is " << gicp.getFitnessScore() << std::endl;transformation_matrix = gicp.getFinalTransformation();std::cout << "变换矩阵：\n" << transformation_matrix << std::endl;}else {PCL_ERROR("GICP配准未能收敛。\n");exit(-1); // 如果未收敛，退出程序}
}// 可视化配准前后的点云结果
void visualize_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr& source,pcl::PointCloud<pcl::PointXYZ>::Ptr& target,pcl::PointCloud<pcl::PointXYZ>::Ptr& result)
{// 创建可视化窗口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> result_color(result, 0, 0, 255);viewer->addPointCloud(result, result_color, "result cloud");// 启动可视化viewer->spin();
}int main()
{pcl::console::TicToc time;// 加载源点云和目标点云pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);pcl::io::loadPCDFile<pcl::PointXYZ>("1.pcd", *target_cloud);pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>);pcl::io::loadPCDFile<pcl::PointXYZ>("2.pcd", *source_cloud);// 打印读取的点云个数std::cout << "读取源点云个数：" << source_cloud->points.size() << std::endl;std::cout << "读取目标点云个数：" << target_cloud->points.size() << std::endl;// 初始化GICP参数和结果变量pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud(new pcl::PointCloud<pcl::PointXYZ>);Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Identity();// 执行GICP配准perform_gicp(source_cloud, target_cloud, icp_cloud, transformation_matrix, 35);// 可视化配准结果visualize_registration(source_cloud, target_cloud, icp_cloud);return 0;
}