算法简介
K-4PCS方法主要分为两个步骤:
(1)利用VoxelGrid滤波器对点云进行下采样,然后使用标准方法(3D harris或者3D DoG.)进行3D关键点检测。
( 2)通过4PCS算法使用关键点集合而非原始点云进行数据的匹配,降低了搜索点集的规模,提高了运算效率。
代码实现
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/console/time.h> // pcl中计算程序运行时间有很多函数,其中利用控制台的时间计算
#include <pcl/features/normal_3d.h>//法线
#include <pcl/surface/gp3.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/math/special_functions/round.hpp>
#include <pcl/registration/ia_kfpcs.h>
using namespace std;
int
main(int argc, char** argv)
{
pcl::console::TicToc time;
// 加载第一次扫描点云数据作为目标云
pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ>("R1.pcd", *target_cloud) == -1)
{
PCL_ERROR("读取目标点云失败 \n");
return (-1);
}
cout << "从目标点云中读取 " << target_cloud->size() << " 个点" << endl;
// 加载从新视角得到的第二次扫描点云数据作为源点云
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ>("R11.pcd", *input_cloud) == -1)
{
PCL_ERROR("读取源标点云失败 \n");
return (-1);
}
cout << "从源点云中读取 " << input_cloud->size() << " 个点" << endl;
time.tic();
//初始化对象
pcl::registration::KFPCSInitialAlignment<pcl::PointXYZ, pcl::PointXYZ> kfpcs;
kfpcs.setInputCloud(input_cloud); //源点云
kfpcs.setInputTarget(target_cloud); //目标点云
kfpcs.setApproxOverlap(0.5);//设置源和目标之间的近似重叠。
kfpcs.setLambda(0.5);
kfpcs.setDelta(0.2);//对内部计算参数进行加权的常数因子增量。
//kfpcs.setMaxComputationTime(1000);//设置最大计算时间(以秒为单位)。
kfpcs.setNumberOfSamples(1000); //设置配准期间要使用的源点云采样的数量
pcl::PointCloud<pcl::PointXYZ>::Ptr kpcs(new pcl::PointCloud<pcl::PointXYZ>);
kfpcs.align(*kpcs);
cout << "KFPCS配准用时: " << time.toc() << " ms" << endl;
cout << "变换矩阵:\n" << kfpcs.getFinalTransformation() << endl;
// 使用创建的变换对为输入点云进行变换
pcl::transformPointCloud(*input_cloud, *kpcs, kfpcs.getFinalTransformation());
// 保存转换后的源点云作为最终的变换输出
// pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);
// 初始化点云可视化对象
boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer(new pcl::visualization::PCLVisualizer("显示点云"));
viewer->setBackgroundColor(0, 0, 0); //设置背景颜色为黑色
// 对目标点云着色可视化 (red).
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>target_color(target_cloud, 255, 0, 0);
viewer->addPointCloud<pcl::PointXYZ>(target_cloud, target_color, "target cloud");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud");
// 对源点云着色可视化 (blue).
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>input_color(input_cloud, 0, 0, 255);
viewer->addPointCloud<pcl::PointXYZ>(input_cloud, input_color, "input cloud");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "input cloud");
// 对转换后的源点云着色 (green)可视化.
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
output_color(kpcs, 0, 255, 0);
viewer->addPointCloud<pcl::PointXYZ>(kpcs, output_color, "output cloud");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "output cloud");
// 启动可视化
//viewer->addCoordinateSystem(0.1); //显示XYZ指示轴
//viewer->initCameraParameters(); //初始化摄像头参数
// 等待直到可视化窗口关闭
while (!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(1000));
}
return (0);
}
实验结果
实验数据:K4PCS实验数据
左边为点云初始位置,右边为配准后点云的位置