近期课题中需要使用PCL进行点云处理,故而学习了一下PCL点云算法库。使用了PCL中的FPFH特征检测,之后利用SAC-IA算法对两个点云进行初始配准。原理可以官方讲解或者大佬的博客。
Function.cpp
#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>
#include<iostream>
typedef pcl::PointXYZ PointT; //定义单个点的类型【输入点类型】
typedef pcl::PointCloud<PointT> PointCloudT; //定义点云的类型
typedef pcl::Normal NormalT; //法向量类型
typedef pcl::PointCloud<pcl::Normal>NormalCloudT; //法向量点云
typedef Eigen::Matrix4f Mat4f;
typedef pcl::search::KdTree<pcl::PointXYZ> SearchT; //搜索方法
typedef pcl::FPFHSignature33 FeatureT; //定义FPFH特征类型,33维向量
typedef pcl::FPFHEstimation<PointT,NormalT,FeatureT> FeatureEstimationT; //点云特征匹配对
typedef pcl::PointCloud<FeatureT> FeatureCloudT; //FPFH特征点云
typedef pcl::SampleConsensusPrerejective<PointT,PointT,FeatureT>AlignmentT;
typedef pcl::visualization::PointCloudColorHandlerCustom<PointT> ColorHandlerT; //可视化对象类型
//【1】定义计算FPFH特征的函数
/*
输入:原始点云数据(pcl::PointXYZ)和KdTree结构(pcl::search::KdTree<pcl::PointXYZ>::Ptr tree)
输出:FPFH特征点云(pcl::PointCloud<FeatureT>)
*/
FeatureCloudT::Ptr computeFeature(PointCloudT::Ptr input_cloud,SearchT::Ptr tree);
//【2】点云配准,并计算变换矩阵
/*
输入:原始对象点云、原始场景点云、对象特征点云、场景特征点云
输出:变换矩阵Eigen::Matrix4f
*/
AlignmentT registration(PointCloudT::Ptr object,PointCloudT::Ptr scene,FeatureCloudT::Ptr object_f,FeatureCloudT::Ptr scene_f);
int main(int argc,char **argv)
{
//定义对象和场景点云句柄
PointCloudT::Ptr object(new PointCloudT);
PointCloudT::Ptr scene(new PointCloudT);
//【1】判断输入语法
if(argc != 3)
{
pcl::console::print_error ("Syntax is: %s object.pcd scene.pcd\n", argv[0]);
return (-1);
}
//【2】加载点云数据
pcl::console::print_highlight ("Loading point clouds...\n");
if(pcl::io::loadPCDFile<PointT> (argv[1], *object) < 0 || pcl::io::loadPCDFile<PointT> (argv[2], *scene) < 0)
{
pcl::console::print_error ("Error loading object/scene file!\n");
return (-1);
}
//【3】下采样操作,减少点云数量
pcl::console::print_highlight ("Downsampling...\n");
pcl::VoxelGrid<PointT> grid; //定义网格点
const float leaf = 0.005f; //设置下采样的程度
grid.setLeafSize (leaf, leaf, leaf);
grid.setInputCloud (object);//对原始点云进行下采样
grid.filter (*object);
grid.setInputCloud (scene);//对场景点云进行下采样
grid.filter (*scene);
//【4】设置搜索方法
SearchT::Ptr tree(new SearchT);
//【5】计算对象和场景点云的FPFH特征
pcl::console::print_highlight ("Estimating features...\n");
FeatureCloudT::Ptr object_f=computeFeature(object,tree);
FeatureCloudT::Ptr scene_f=computeFeature(scene,tree);
//【6】实时配准
pcl::console::print_highlight ("Starting alignment...\n");
AlignmentT align=registration(object,scene,object_f,scene_f);
PointCloudT::Ptr object_aligned(new PointCloudT);
align.align(*object_aligned); //计算得到对象配准后的点云
//【7】计算变换矩阵
if(!align.hasConverged())
{
pcl::console::print_error ("Alignment failed!\n");
return (-1);
}
else
{
Eigen::Matrix4f transformation = align.getFinalTransformation();
//输出变换矩阵
pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2));
pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2));
pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2));
pcl::console::print_info ("\n");
pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3));
pcl::console::print_info ("\n");
pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), object->size ());
}
//【8】可视化
pcl::visualization::PCLVisualizer Viwer("3D Viwer");
int v1(0),v2(0);
Viwer.createViewPort(0,0,0.5,1,v1);
Viwer.createViewPort(0.5,0,1,1,v2);
Viwer.setBackgroundColor(255,255,255,v1);
Viwer.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene",v1);
Viwer.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned",v1);
Viwer.addPointCloud(object,ColorHandlerT (object, 0.0, 255.0, 0.0), "object_before_aligned",v2);
Viwer.addPointCloud(scene,ColorHandlerT (scene, 0.0, 0.0, 255.0), "scene_v2",v2);
Viwer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"scene");
Viwer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"object_aligned");
Viwer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"object_before_aligned");
Viwer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"scene_v2");
Viwer.spin ();
return 0;
}
FeatureCloudT::Ptr computeFeature(PointCloudT::Ptr input_cloud,SearchT::Ptr tree)
{
//【1】在计算FPFH特征点云之前需要首先计算法向量点云
//定义原始点云对应的法向量点云
NormalCloudT::Ptr point_normal(new NormalCloudT);
//定义法向量估计对象
pcl::NormalEstimation<PointT,NormalT>en;
//设置en估计参数
en.setInputCloud(input_cloud); //设置带估计的点云
en.setSearchMethod(tree); //设置搜索的方法
en.setKSearch(10); //设置领域的大小K为10;
en.compute(*point_normal);//设置计算结果的保存对象
//【2】在得到原始点云对应的法向量点云后,计算FPFH特征点云
FeatureCloudT::Ptr featureCloud(new FeatureCloudT);
//pcl::FPFHEstimationOMP<pcl::PointXYZ,pcl::Normal,pcl::FPFHSignature33> est_fpfh;
FeatureEstimationT fe;
fe.setInputCloud(input_cloud); //设置原始点云
fe.setInputNormals(point_normal);//设置计算得到的法向量点云
fe.setSearchMethod(tree);//设置搜索方法
fe.setKSearch(10); //设置搜索邻域大小为10
fe.compute(*featureCloud);
return featureCloud;
}
AlignmentT registration(PointCloudT::Ptr object,PointCloudT::Ptr scene,FeatureCloudT::Ptr object_f,FeatureCloudT::Ptr scene_f)
{
pcl::SampleConsensusPrerejective<PointT,PointT,FeatureT>align;//设置配准对象
align.setInputSource(object);//添加原始对象点云
align.setSourceFeatures(object_f); //添加对象特征点云
align.setInputTarget(scene);//添加原始场景点云
align.setTargetFeatures(scene_f);//添加场景特征点云
//设置配准参数
align.setMaximumIterations (50000); // 采样一致性迭代次数
align.setNumberOfSamples (3); // 创建假设所需的样本数
align.setCorrespondenceRandomness (5); // 使用的临近特征点的数目
align.setSimilarityThreshold (0.9f); // 多边形边长度相似度阈值
align.setMaxCorrespondenceDistance (2.5f * 0.005); // 判断是否为内点的距离阈值
align.setInlierFraction (0.25f); //接受位姿假设所需的内点比例
return align;
}
CMakeLists.txt
cmake_minimum_required(VERSION 2.6)
project(pcl_test)
find_package(PCL 1.8 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable(pcl_test Function.cpp)
target_link_libraries (pcl_test ${PCL_LIBRARIES})
install(TARGETS pcl_test RUNTIME DESTINATION bin)