PCL NARF关键点提取、SIFT关键点提取、Harris关键点提取

一、NARF关键点提取 

1、背景

关键点也称为兴趣点，是通过定义检测标准来获取的具有稳定性、区别性的点集。从技术上来说，关键点的数量要比原始点云的数目少很多，与局部特征描述子结合在一起，组成关键点描述子常用来形成原始数据的紧凑表示，而且不失代表性与描述性，从而加快识别、追踪有等对数据的处理速度。故而，关键点提取就成了2D和3D信息处理中不可或缺的关键技术。

本例程将点云数据生成深度图数据，然后对深度图数据进行关键点提取，最后将关键点在点云视图中进行可视化

2、关键点提取代码

//提取NARF关键点
pcl::RangeImageBorderExtractor range_image_border_extractor;//创建深度图像边界提取对象
pcl::NarfKeypoint narf_keypoint_detector(&range_image_border_extractor);//创建Narf关键点提取器，输入为深度图像边缘提取器
narf_keypoint_detector.setRangeImage(&range_image);//关键点提取设置输入深度图像
narf_keypoint_detector.getParameters().support_size=support_size;//关键点提取的参数：搜索空间球体的半径
narf_keypoint_detector.getParameters ().add_points_on_straight_edges = true;//对竖直边是否感兴趣
//narf_keypoint_detector.getParameters ().distance_for_additional_points = 10;//与support_size一起作用

pcl::PointCloud<int> keypoint_indices;//关键点索引
narf_keypoint_detector.compute(keypoint_indices);//关键点计算,结果放置到keypoint_indices
std::cerr<<"Found "<<keypoint_indices.points.size()<<" key points.\n";//输出得到的NARF关键点数目

3、全部代码

/* \author Bastian Steder */

#include <iostream>
#include <boost/thread/thread.hpp>
#include <pcl/range_image/range_image.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/features/range_image_border_extractor.h>
#include <pcl/keypoints/narf_keypoint.h>//特征点提取头文件
#include <pcl/console/parse.h>//命令行解析

#include <pcl/visualization/common/float_image_utils.h>//保存深度图像相关头文件
#include <pcl/io/png_io.h>//保存深度图像相关头文件

typedef pcl::PointXYZ PointType;
//参数
float angular_resolution=0.5f;
float support_size=0.2f;//默认关键点提取的参数（关键点提取器所支持的范围：搜索空间球体的半径，指定了计算感兴趣值的测度时所使用的邻域范围）
pcl::RangeImage::CoordinateFrame coordinate_frame=pcl::RangeImage::CAMERA_FRAME;
bool setUnseenToMaxRange=false;
//打印帮助
void
printUsage(const char* progName)
{
std::cout<<"\n\nUsage: "<<progName<<" [options] <scene.pcd>\n\n"
<<"Options:\n"
<<"-------------------------------------------\n"
<<"-r <float>   angular resolution in degrees (default "<<angular_resolution<<")\n"
<<"-c <int>     coordinate frame (default "<<(int)coordinate_frame<<")\n"
<<"-m           Treat all unseen points as maximum range readings\n"
<<"-s <float>   support size for the interest points (diameter of the used sphere - "
<<"default "<<support_size<<")\n"
<<"-h           this help\n"
<<"\n\n";
}

void
setViewerPose(pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose)
{
Eigen::Vector3f pos_vector = viewer_pose*Eigen::Vector3f(0,0,0);
Eigen::Vector3f look_at_vector=viewer_pose.rotation()*Eigen::Vector3f(0,0,1)+pos_vector;
Eigen::Vector3f up_vector=viewer_pose.rotation()*Eigen::Vector3f(0,-1,0);
viewer.setCameraPosition(pos_vector[0],pos_vector[1],pos_vector[2],
                         look_at_vector[0],look_at_vector[1],look_at_vector[2],
                         up_vector[0],up_vector[1],up_vector[2]);
/*
viewer.camera_.pos[0]=pos_vector[0];
viewer.camera_.pos[1]=pos_vector[1];
viewer.camera_.pos[2]=pos_vector[2];
viewer.camera_.focal[0]=look_at_vector[0];
viewer.camera_.focal[1]=look_at_vector[1];
viewer.camera_.focal[2]=look_at_vector[2];
viewer.camera_.view[0]=up_vector[0];
viewer.camera_.view[1]=up_vector[1];
viewer.camera_.view[2]=up_vector[2];
viewer.updateCamera();
 */
}

// -----Main-----
int
main(int argc,char** argv)
{
//解析命令行参数
if(pcl::console::find_argument(argc,argv,"-h")>=0)
{
printUsage(argv[0]);
return 0;
}
if(pcl::console::find_argument(argc,argv,"-m")>=0)
{
setUnseenToMaxRange=true;
cout<<"Setting unseen values in range image to maximum range readings.\n";
}
int tmp_coordinate_frame;
if(pcl::console::parse(argc,argv,"-c",tmp_coordinate_frame)>=0)
{
coordinate_frame=pcl::RangeImage::CoordinateFrame(tmp_coordinate_frame);
cout<<"Using coordinate frame "<<(int)coordinate_frame<<".\n";
}
if(pcl::console::parse(argc,argv,"-s",support_size)>=0)
cout<<"Setting support size to "<<support_size<<".\n";
if(pcl::console::parse(argc,argv,"-r",angular_resolution)>=0)
cout<<"Setting angular resolution to "<<angular_resolution<<"deg.\n";
angular_resolution=pcl::deg2rad(angular_resolution);
//读取给定的pcd文件或者自行创建随机点云
pcl::PointCloud<PointType>::Ptr point_cloud_ptr(new pcl::PointCloud<PointType>);
pcl::PointCloud<PointType>&point_cloud=*point_cloud_ptr;
pcl::PointCloud<pcl::PointWithViewpoint> far_ranges;
Eigen::Affine3f scene_sensor_pose(Eigen::Affine3f::Identity());
std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument(argc,argv,"pcd");
if(!pcd_filename_indices.empty())
{
std::string filename=argv[pcd_filename_indices[0]];
if(pcl::io::loadPCDFile(filename,point_cloud)==-1)//打开失败
{
cerr<<"Was not able to open file \""<<filename<<"\".\n";
printUsage(argv[0]);
return 0;
}
//打开pcd文件成功
scene_sensor_pose=Eigen::Affine3f(Eigen::Translation3f(point_cloud.sensor_origin_[0],
point_cloud.sensor_origin_[1],
point_cloud.sensor_origin_[2]))*
Eigen::Affine3f(point_cloud.sensor_orientation_);
std::string far_ranges_filename = pcl::getFilenameWithoutExtension(filename)+"_far_ranges.pcd";
if(pcl::io::loadPCDFile(far_ranges_filename.c_str(),far_ranges)==-1)//如果打开far_ranges文件失败
std::cout<<"Far ranges file \""<<far_ranges_filename<<"\" does not exists.\n";
}

//没有给的输入点云，则生成一个点云
else
{
setUnseenToMaxRange=true;
cout<<"\nNo *.pcd file given =>Genarating example point cloud.\n\n";
for(float x = -0.5f;x<=0.5f;x+=0.01f)
{
for(float y = -0.5f;y<=0.5f;y+=0.01f)
{
PointType point;point.x=x;point.y=y;point.z=2.0f-y;
point_cloud.points.push_back(point);
}
}
point_cloud.width=(int)point_cloud.points.size();point_cloud.height=1;//height = 1为无序点云
}
//从点云创建距离图像
float noise_level=0.0;
float min_range=0.0f;
int border_size=1;//深度图边界大小
boost::shared_ptr<pcl::RangeImage> range_image_ptr(new pcl::RangeImage);
pcl::RangeImage&range_image=*range_image_ptr;
//生成深度图像range_image
range_image.createFromPointCloud(point_cloud,angular_resolution,pcl::deg2rad(360.0f),pcl::deg2rad(180.0f),scene_sensor_pose,coordinate_frame,noise_level,min_range,border_size);
range_image.integrateFarRanges(far_ranges);//不是太懂
if(setUnseenToMaxRange)
range_image.setUnseenToMaxRange();
// 创建3D点云可视化窗口，并显示点云
pcl::visualization::PCLVisualizer viewer("3D Viewer");//3D窗口
//第一个窗口，点云和关键点都显示
int v1(0);
viewer.createViewPort(0,0,0.5,1,v1);
viewer.setBackgroundColor(255,1,1,v1);//背景颜色
pcl::visualization::PointCloudColorHandlerCustom<PointType> color1(point_cloud_ptr,200,10,10);//输入点云的颜色
//pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange>range_image_color_handler(range_image_ptr,0,0,0);//输入点云的颜色,赋值兼容规则
viewer.addPointCloud(point_cloud_ptr,color1,"point_cloud",v1);//添加点云
//viewer.addPointCloud(range_image_ptr,range_image_color_handler,"range image",v1);//添加点云
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1,"range image",v1);//设置点的大小

//第二个窗口，只显示关键点
int v2(0);
viewer.createViewPort(0.5,0,1,1,v2);
viewer.setBackgroundColor(128,0,190,v2);

//viewer.addCoordinateSystem (1.0f);
//PointCloudColorHandlerCustom<PointType>point_cloud_color_handler (point_cloud_ptr, 150, 150, 150);
//viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud");
viewer.initCameraParameters();
setViewerPose(viewer,range_image.getTransformationToWorldSystem());
// 显示距离图像
pcl::visualization::RangeImageVisualizer range_image_widget("Range image");
range_image_widget.showRangeImage(range_image);
//保存深度图像
float* ranges = range_image.getRangesArray();
unsigned char* rgb_image = pcl::visualization::FloatImageUtils::getVisualImage(ranges,range_image.width,range_image.height);
pcl::io::saveRgbPNGFile("range_image.png",rgb_image,range_image.width,range_image.height);//保存深度图像至range_image.png
std::cerr << "range_image has been saved!" << std::endl;


//提取NARF关键点
pcl::RangeImageBorderExtractor range_image_border_extractor;//创建深度图像边界提取对象
pcl::NarfKeypoint narf_keypoint_detector(&range_image_border_extractor);//创建Narf关键点提取器，输入为深度图像边缘提取器
narf_keypoint_detector.setRangeImage(&range_image);//关键点提取设置输入深度图像
narf_keypoint_detector.getParameters().support_size=support_size;//关键点提取的参数：搜索空间球体的半径
narf_keypoint_detector.getParameters ().add_points_on_straight_edges = true;//对竖直边是否感兴趣
//narf_keypoint_detector.getParameters ().distance_for_additional_points = 10;//与support_size一起作用

pcl::PointCloud<int> keypoint_indices;//关键点索引
narf_keypoint_detector.compute(keypoint_indices);//关键点计算,结果放置到keypoint_indices
std::cerr<<"Found "<<keypoint_indices.points.size()<<" key points.\n";//输出得到的NARF关键点数目
//保存关键点
//转换关键点类型
pcl::PointCloud<PointType>::Ptr narf_points_ptr(new pcl::PointCloud<PointType>);
narf_points_ptr->width = keypoint_indices.points.size();
narf_points_ptr->height = 1;
narf_points_ptr->resize(keypoint_indices.points.size());
narf_points_ptr->is_dense = false;
for (size_t i = 0;i<keypoint_indices.points.size();i++)
{
    narf_points_ptr->points[i].getVector3fMap() = range_image.points[keypoint_indices.points[i]].getVector3fMap();
}
//输出narf转换后的关键点
std::cerr<<"narf_points: "<<narf_points_ptr->points.size()<<std::endl;
//for (size_t i = 0;i<narf_points_ptr->points.size();i++)
//   std::cerr<<"   ["<<i<<"]   "<<narf_points_ptr->points[i].x<<", "<<narf_points_ptr->points[i].y<<", "<<narf_points_ptr->points[i].z<<std::endl;
//保存narf关键点
pcl::io::savePCDFileASCII("narf_points.pcd",*narf_points_ptr);
std::cerr<<"narf_points save succeed!"<<std::endl;
//在距离图像显示组件内显示关键点
//for (size_ti=0; i<keypoint_indices.points.size (); ++i)
//range_image_widget.markPoint (keypoint_indices.points[i]%range_image.width,
//keypoint_indices.points[i]/range_image.width);
//在3D窗口中显示关键点
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_ptr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>&keypoints=*keypoints_ptr;
keypoints.points.resize(keypoint_indices.points.size());
for(size_t i=0;i<keypoint_indices.points.size();++i)
keypoints.points[i].getVector3fMap()=range_image.points[keypoint_indices.points[i]].getVector3fMap();//将得到的关键点转换给keypoints(pcl::PointCloud<pcl::PointXYZ>类型)
//设置关键点在3D Viewer中的颜色大小属性
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>keypoints_color_handler(keypoints_ptr,0,255,0);//颜色
viewer.addPointCloud<pcl::PointXYZ>(keypoints_ptr,keypoints_color_handler,"keypoints");//显示
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,7,"keypoints");//关键点的大小
// 主循环
while(!viewer.wasStopped())
{
range_image_widget.spinOnce();// process GUI events
viewer.spinOnce();
pcl_sleep(0.01);
}
}

4、可视化 

1）、

将” narf_keypoint_detector.getParameters ().add_points_on_straight_edges = true;//对竖直边是否感兴趣”设置为不感兴趣(false<默认>)，此时关键点少

2）、

将” narf_keypoint_detector.getParameters ().add_points_on_straight_edges = true;//对竖直边是否感兴趣”设置为感兴趣(true)，此时关键点多 

二、SIFT关键点提取

１、背景

本例程将演示如何检测点云的SIFT关键点。SIFT，即尺度不变特征变换(Scale-invariant feature transform, SIFT)，最初是图像处理领域的一种描述。这种描述具有尺度不变性，可在图像中检测出关键点，是一种局部特征描述子，后来被引入3D点云领域用于关键点检测。

2、SIFT检测代码

  const float min_scale = atof(argv[2]);//尺度空间中最小尺度的标准偏差
  const int n_octaves = atoi(argv[3]);//高斯金字塔中组的数目
  const int n_scales_per_octave = atoi(argv[4]);//每组计算的尺度数目
  const float min_contrast = atof(argv[5]);//设置关键点检测的阈值

  //SIFT关键点检测
  pcl::SIFTKeypoint<pcl::PointXYZ, pcl::PointWithScale> sift;//创建sift关键点检测对象
  pcl::PointCloud<pcl::PointWithScale> result;//SIFT关键点提取结果
  sift.setInputCloud(cloud_xyz);//设置输入点云
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ> ());
  sift.setSearchMethod(tree);//创建一个空的kd树对象tree，并把它传递给sift检测对象
  sift.setScales(min_scale, n_octaves, n_scales_per_octave);//指定搜索关键点的尺度范围
  sift.setMinimumContrast(min_contrast);//设置限制关键点检测的阈值
  sift.compute(result);//执行sift关键点检测，保存结果在result

3、全部代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/io.h>
#include <pcl/keypoints/sift_keypoint.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/time.h>
using namespace std;

namespace pcl
{
  template<>
    struct SIFTKeypointFieldSelector<PointXYZ>
    {
      inline float
      operator () (const PointXYZ &p) const
      {
    return p.z;
      }
    };
}

int
main(int argc, char *argv[])
{

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::io::loadPCDFile (argv[1], *cloud_xyz);//将第二个命令行参数所代表的文件作为输入文件读入

  const float min_scale = atof(argv[2]);//尺度空间中最小尺度的标准偏差
  const int n_octaves = atoi(argv[3]);//高斯金字塔中组的数目
  const int n_scales_per_octave = atoi(argv[4]);//每组计算的尺度数目
  const float min_contrast = atof(argv[5]);//设置关键点检测的阈值

  //SIFT关键点检测
  pcl::SIFTKeypoint<pcl::PointXYZ, pcl::PointWithScale> sift;//创建sift关键点检测对象
  pcl::PointCloud<pcl::PointWithScale> result;//SIFT关键点提取结果
  sift.setInputCloud(cloud_xyz);//设置输入点云
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ> ());
  sift.setSearchMethod(tree);//创建一个空的kd树对象tree，并把它传递给sift检测对象
  sift.setScales(min_scale, n_octaves, n_scales_per_octave);//指定搜索关键点的尺度范围
  sift.setMinimumContrast(min_contrast);//设置限制关键点检测的阈值
  sift.compute(result);//执行sift关键点检测，保存结果在result

  //类型转换
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp (new pcl::PointCloud<pcl::PointXYZ>);
  copyPointCloud(result, *cloud_temp);//将点类型pcl::PointWithScale的数据转换为点类型pcl::PointXYZ的数据
 
  //可视化输入点云和关键点
  pcl::visualization::PCLVisualizer viewer("Sift keypoint");
  viewer.setBackgroundColor( 255, 255, 255 );
  viewer.addPointCloud(cloud_xyz, "cloud");//在视窗中添加原始点云数据
  viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,0,0,0,"cloud");
  viewer.addPointCloud(cloud_temp, "keypoints");//将SIFT关键点添加至视窗
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 9, "keypoints");
  viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,0,0,255,"keypoints");

  while(!viewer.wasStopped ())
  {
    viewer.spinOnce ();
  }
  return 0;
  
}

4、可视化

三、Harris关键点提取

1、背景

 本小结将演示如何检测点云的3D Harris角点。Harris算子是常见的特征检测算子，既可以提取角点又可以提取边缘点。与2D Harris角点检测原理不同，3D Harris角点检测利用的就是点云的法向量信息。

2、Harris关键点提取代码

	pcl::PointCloud<pcl::PointXYZI>::Ptr Harris_keypoints (new pcl::PointCloud<pcl::PointXYZI> ());//存放最后的特征点提取结果
	//实例化一个Harris特征检测对象harris_detector
	pcl::HarrisKeypoint3D<pcl::PointXYZ,pcl::PointXYZI,pcl::Normal>* harris_detector = new pcl::HarrisKeypoint3D<pcl::PointXYZ,pcl::PointXYZI,pcl::Normal> ;

	//harris_detector->setNonMaxSupression(true);
	harris_detector->setRadius(r_normal);//设置法向量估计的半径
	harris_detector->setRadiusSearch(r_keypoint);//设置关键点估计的近邻搜索半径
	harris_detector->setInputCloud (input_cloud);//设置输入点云
	//harris_detector->setNormals(normal_source);
	//harris_detector->setMethod(pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::LOWE);
	harris_detector->compute (*Harris_keypoints);//结果存放在Harris_keypoints
	cout<<"Harris_keypoints number: "<<Harris_keypoints->size()<<endl;
	//writer.write<pcl::PointXYZI> ("Harris_keypoints.pcd",*Harris_keypoints,false);//结果保存

3、全部代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/io.h>
#include <pcl/keypoints/harris_3d.h>//harris特征点估计类头文件声明
#include <cstdlib>
#include <vector>
#include <pcl/console/parse.h>
using namespace std;



int main(int argc,char *argv[]) 
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);//输入点云的指针
	pcl::io::loadPCDFile (argv[1], *input_cloud);//将命令行参数文件读至input_cloud指针
	pcl::PCDWriter writer;
	float r_normal;//法向量估计的半径
	float r_keypoint;//关键点估计的近邻搜索半径

	r_normal=atof(argv[2]);//将命令行的第三个参数解析为法向量估计的半径
	r_keypoint=atof(argv[3]);//将命令行的第四个参数解析为关键点估计的近邻搜索半径

	typedef pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZI> ColorHandlerT3;

	pcl::PointCloud<pcl::PointXYZI>::Ptr Harris_keypoints (new pcl::PointCloud<pcl::PointXYZI> ());//存放最后的特征点提取结果
	//实例化一个Harris特征检测对象harris_detector
	pcl::HarrisKeypoint3D<pcl::PointXYZ,pcl::PointXYZI,pcl::Normal>* harris_detector = new pcl::HarrisKeypoint3D<pcl::PointXYZ,pcl::PointXYZI,pcl::Normal> ;

	//harris_detector->setNonMaxSupression(true);
	harris_detector->setRadius(r_normal);//设置法向量估计的半径
	harris_detector->setRadiusSearch(r_keypoint);//设置关键点估计的近邻搜索半径
	harris_detector->setInputCloud (input_cloud);//设置输入点云
	//harris_detector->setNormals(normal_source);
	//harris_detector->setMethod(pcl::HarrisKeypoint3D<pcl::PointXYZRGB,pcl::PointXYZI>::LOWE);
	harris_detector->compute (*Harris_keypoints);//结果存放在Harris_keypoints
	cout<<"Harris_keypoints number: "<<Harris_keypoints->size()<<endl;
	//writer.write<pcl::PointXYZI> ("Harris_keypoints.pcd",*Harris_keypoints,false);//结果保存
	pcl::io::savePCDFileASCII("Harris keypoints.pcd" , *Harris_keypoints);
	cout<<"Points: "<<Harris_keypoints->points.size()<<endl;

	//可视化点云
	pcl::visualization::PCLVisualizer visu3("clouds");
	visu3.setBackgroundColor(255,255,255);
	//Harris_keypoints关键点可视化
	visu3.addPointCloud (Harris_keypoints, ColorHandlerT3 (Harris_keypoints, 0.0, 0.0, 255.0), "Harris_keypoints");
	visu3.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,8,"Harris_keypoints");
	//原始点云可视化
	visu3.addPointCloud(input_cloud,"input_cloud");
	visu3.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,0,0,0,"input_cloud");
	visu3.spin ();
}

4、可视化

深度图：