目录
PCL点云算法汇总及实战案例汇总的目录地址链接:
一、概述
在3D点云处理中,粗配准算法通常用于初步对齐源点云和目标点云。K-4PCS(4点一致性采样)算法是一种常见的粗配准方法,其主要通过匹配源点云和目标点云中的四个点来求解最佳刚体变换。本次的PCL实现中,通过K4PCS算法实现两个点云的粗配准,并在最终通过可视化进行结果展示。
1.1原理
K-4PCS算法通过以下步骤进行点云配准:
- 随机采样源点云中的四个点。
- 在目标点云中找到匹配的四个点。
- 计算刚体变换(旋转和平移矩阵),使得源点云与目标点云的这四个点对齐。
- 重复以上步骤,并选择使匹配点最多的变换作为最终结果。
1.2实现步骤
- 加载点云数据:读取源点云和目标点云数据。
- K4PCS配准:使用PCL中的 KFPCSInitialAlignment 类,设置相关参数后进行配准。
- 点云变换:应用计算得到的刚体变换矩阵,将源点云进行配准。
- 结果可视化:通过两个视口展示配准前后的点云结果。
1.3应用场景
- 3D扫描数据的初步对齐:在3D扫描数据处理中,K-4PCS算法用于对多个视角的点云数据进行初步对齐,为后续的精确配准(如ICP)提供初始位姿。
- 机器人定位与导航:K-4PCS算法可以用于多视角3D点云的匹配,帮助机器人在复杂环境中进行粗配准与定位。
二、代码实现
2.1关键函数
2.1.1 加载点云数据
// 加载点云数据,传入文件路径,返回加载后的点云
pcl::PointCloud<pcl::PointXYZ>::Ptr loadPointCloud(const std::string& file_path) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ>(file_path, *cloud) == -1) {
PCL_ERROR("Could not read file\n");
return nullptr;
}
return cloud;
}
2.1.2 执行K4PCS配准
// 通过K-4PCS算法进行点云的粗配准,返回配准后的点云
pcl::PointCloud<pcl::PointXYZ>::Ptr runK4PCSAlignment(pcl::PointCloud<pcl::PointXYZ>::Ptr source,
pcl::PointCloud<pcl::PointXYZ>::Ptr target) {
pcl::registration::KFPCSInitialAlignment<pcl::PointXYZ, pcl::PointXYZ> kfpcs;
kfpcs.setInputSource(source); // 设置源点云
kfpcs.setInputTarget(target); // 设置目标点云
kfpcs.setApproxOverlap(0.7); // 近似重叠率
kfpcs.setLambda(0.5); // 平移矩阵的加权系数
kfpcs.setDelta(0.002, false); // 源和目标之间的距离
kfpcs.setNumberOfThreads(6); // 多线程加速
kfpcs.setNumberOfSamples(200); // 采样点数量
pcl::PointCloud<pcl::PointXYZ>::Ptr kpcs(new pcl::PointCloud<pcl::PointXYZ>);
kfpcs.align(*kpcs); // 执行配准,返回配准后的点云
return kpcs;
}
2.1.3 可视化结果
// 可视化点云,展示配准前后的结果
void visualizePointClouds(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("Point Cloud Viewer"));
int v1(0), v2(1);
// 第一个视口,显示配准前的点云
viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
viewer->setBackgroundColor(0, 0, 0, v1);
viewer->addText("Before Registration", 10, 10, "v1_text", v1);
// 将目标点云设为蓝色
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target, 0, 0, 255);
viewer->addPointCloud(target, target_color, "target cloud", v1);
// 将源点云设为绿色
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(source, 0, 255, 0);
viewer->addPointCloud(source, source_color, "source cloud", v1);
// 第二个视口,显示配准后的点云
viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
viewer->setBackgroundColor(0.1, 0.1, 0.1, v2);
viewer->addText("After Registration", 10, 10, "v2_text", v2);
// 配准后的点云设为红色
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> aligned_color(aligned, 255, 0, 0);
viewer->addPointCloud(aligned, aligned_color, "aligned cloud", v2);
// 开始渲染
viewer->spin();
}
2.2完整代码
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/console/time.h>
#include <pcl/registration/ia_kfpcs.h> // K4PCS算法头文件
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
// 加载点云数据,传入文件路径,返回加载后的点云
pcl::PointCloud<pcl::PointXYZ>::Ptr loadPointCloud(const std::string& file_path) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ>(file_path, *cloud) == -1) {
PCL_ERROR("Could not read file\n");
return nullptr;
}
return cloud;
}
// 通过K-4PCS算法进行点云的粗配准,返回配准后的点云
pcl::PointCloud<pcl::PointXYZ>::Ptr runK4PCSAlignment(pcl::PointCloud<pcl::PointXYZ>::Ptr source,
pcl::PointCloud<pcl::PointXYZ>::Ptr target) {
pcl::registration::KFPCSInitialAlignment<pcl::PointXYZ, pcl::PointXYZ> kfpcs;
kfpcs.setInputSource(source); // 设置源点云
kfpcs.setInputTarget(target); // 设置目标点云
kfpcs.setApproxOverlap(0.7); // 近似重叠率
kfpcs.setLambda(0.5); // 平移矩阵的加权系数
kfpcs.setDelta(0.002, false); // 源和目标之间的距离
kfpcs.setNumberOfThreads(6); // 多线程加速
kfpcs.setNumberOfSamples(200); // 采样点数量
pcl::PointCloud<pcl::PointXYZ>::Ptr kpcs(new pcl::PointCloud<pcl::PointXYZ>);
kfpcs.align(*kpcs); // 执行配准,返回配准后的点云
return kpcs;
}
// 可视化点云,展示配准前后的结果
//void visualizePointClouds(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("Point Cloud Viewer"));
//
// int v1(0), v2(1);
// // 第一个视口,显示配准前的点云
// viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
// viewer->setBackgroundColor(0, 0, 0, v1);
// viewer->addText("Before Registration", 10, 10, "v1_text", v1);
//
// // 将目标点云设为蓝色
// pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target, 0, 0, 255);
// viewer->addPointCloud(target, target_color, "target cloud", v1);
//
// // 将源点云设为绿色
// pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(source, 0, 255, 0);
// viewer->addPointCloud(source, source_color, "source cloud", v1);
//
// // 第二个视口,显示配准后的点云
// viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
// viewer->setBackgroundColor(0.1, 0.1, 0.1, v2);
// viewer->addText("After Registration", 10, 10, "v2_text", v2);
//
// // 配准后的点云设为红色
// pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> aligned_color(aligned, 255, 0, 0);
// viewer->addPointCloud(aligned, aligned_color, "aligned cloud", v2);
// //viewer->addPointCloud(target, target_color, "target cloud2", v2);
// viewer->addPointCloud(source, source_color, "source cloud2", v2);
//
// // 开始渲染
// viewer->spin();
//}
void visualizePointClouds(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("Point Cloud Viewer"));
int v1(0), v2(1);
// 第一个视口,显示配准前的点云
viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
viewer->setBackgroundColor(0, 0, 0, v1);
viewer->addText("Before Registration", 10, 10, "v1_text", v1);
// 将目标点云设为蓝色
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(target, 0, 0, 255);
viewer->addPointCloud(target, target_color, "target cloud", v1);
// 将源点云设为绿色
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> source_color(source, 0, 255, 0);
viewer->addPointCloud(source, source_color, "source cloud", v1);
// 第二个视口,显示配准后的点云和目标点云
viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
viewer->setBackgroundColor(0.1, 0.1, 0.1, v2);
viewer->addText("After Registration", 10, 10, "v2_text", v2);
// 配准后的点云设为红色
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> aligned_color(aligned, 255, 0, 0);
viewer->addPointCloud(aligned, aligned_color, "aligned cloud", v2);
// 将目标点云也显示在配准后的视口中,设为蓝色
viewer->addPointCloud(target, target_color, "target cloud 2", v2);
// 开始渲染
viewer->spin();
}
int main(int argc, char** argv) {
// 加载源点云和目标点云
pcl::PointCloud<pcl::PointXYZ>::Ptr source = loadPointCloud("1.pcd");
pcl::PointCloud<pcl::PointXYZ>::Ptr target = loadPointCloud("2.pcd");
// 执行K4PCS配准
pcl::PointCloud<pcl::PointXYZ>::Ptr aligned = runK4PCSAlignment(source, target);
// 可视化点云结果
visualizePointClouds(source, target, aligned);
return 0;
}
三、实现效果
本站资源均来自互联网,仅供研究学习,禁止违法使用和商用,产生法律纠纷本站概不负责!如果侵犯了您的权益请与我们联系!
转载请注明出处: 免费源码网-免费的源码资源网站 » PCL 点云配准-K4PCS算法(粗配准)
发表评论 取消回复