PCL-FPFH特征检测_匹配_位姿估计

近期课题中需要使用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)