PCL点云库学习笔记(6)——关键点

1.关键点相关概念及算法

关键点也称为兴趣点,它是 2D 图像、3D 点云或曲面模型上,可以通过定义检测标准来获取的具有稳定性、区别性的点集。从技术上来说,关键点的数量相比于原始点云或图像的 数据量小很多,它与局部特征描述子结合在一起,组成关键点描述子,常用来形成原始数据 的紧凑表示,而且不失代表性与描述性,从而可以加快后续识别、追踪等对数据的处理速 度。关键点提取是 2D 与 3D 信息处理中不可或缺的关键技术。
1)NARF关键点
NARF(Normal Aligned Radial Feature)关键点是为了从深度图中提取物体提出的。NARF关键点提取要求:提取过程必须将边缘以及物体表面变化信息考虑在内;关键点位置必须稳定,可以在不同视角时被重复探测;关键点所在位置必须有稳定的支持区域,可以计算描述子并进行唯一的法向量估计。
提取步骤:
(1) 遍历每个深度图像点,通过寻找在近邻区域有深度突变的位置进行边缘检测。
(2) 遍历每个深度图像点,根据近邻区域的表面变化决定一种测度表面变化的系数,以及变化的主 方向。
(3) 根据第二步找到的主方向计算兴趣值,表征该方向与其他方向的不同,以及该处表面的变化情 况,即该点有多稳定。
(4) 对兴趣值进行平滑过滤。
(5) 进行无最大值压缩找到最终的关键点,即为 NARF 关键点。
2)Harris关键点
Harris 关键点检测算法于 1988 年由 Chris Harris 和 Mike Stephens 提出,也称为 Plessey 关键点检测算法,是早期经典的一种关键点检测算法。Harris关键点检测通过计算图像点的Harris矩阵和矩阵对应的特征值来判断是否为关键点,若矩阵的两个特征值都很大,则该点为关键点。
3)SIFT关键点提取
SIFT即尺度不变特征变换( Scale invariant featuretransform, SIFT), 最初用于图像处理领域的一种描述。这种描述具有尺度不变性,可在图像中检测出关键点,是一种局部特征描述子,后来被引入 3D 点云领域用于关键点的检测。

2.提取NARF特征点


#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include //特征点提取头文件
#include //命令行解析
 
#include //保存深度图像相关头文件
#include //保存深度图像相关头文件
 
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] \n\n"
<<"Options:\n"
<<"-------------------------------------------\n"
<<"-r    angular resolution in degrees (default "<<angular_resolution<<")\n"
<<"-c      coordinate frame (default "<<(int)coordinate_frame<<")\n"
<<"-m           Treat all unseen points as maximum range readings\n"
<<"-s    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]);

}
 
// -----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);//输入点云的颜色
viewer.addPointCloud(point_cloud_ptr,color1,"point_cloud",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.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;//对竖直边是否感兴趣

 
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;

//保存narf关键点
pcl::io::savePCDFileASCII("narf_points.pcd",*narf_points_ptr);
std::cerr<<"narf_points save succeed!"<<std::endl;
//在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类型)
//设置关键点在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();
viewer.spinOnce();
pcl_sleep(0.01);
}
}

PCL点云库学习笔记(6)——关键点_第1张图片

3.SIFT关键点提取

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
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);
  
  //一直提示error: ‘stof’ was not declared in this scope,是因为编译方式不支持c++11,但没有尝试成功
  // const float min_scale = stof(argv[2]);          
  // const int n_octaves = stof(argv[3]);            
  // const int n_scales_per_octave = stof(argv[4]);  
  // const float min_contrast = stof(argv[5]);
  const float min_scale = 0.01;          
  const int n_octaves = 6;            
  const int n_scales_per_octave = 4;  
  const float min_contrast = 0.01;
 
  pcl::SIFTKeypoint<pcl::PointXYZ, pcl::PointWithScale> sift;//创建sift关键点检测对象
  pcl::PointCloud<pcl::PointWithScale> result;
  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);//制定搜索关键点的尺度范围
                        //min_scale:设置尺度空间中最小尺度的标准偏差
                        //n_octaves:高斯金字塔中组(octave)的数目
                        //n_scales_per_octave:每组octave的计算尺度(scale)数目
  sift.setMinimumContrast(min_contrast);//设置限制关键点检测的阈值
  sift.compute(result);//计算得出关键点检测结果

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp (new pcl::PointCloud<pcl::PointXYZ>);
  copyPointCloud(result, *cloud_temp);//便于显示
 
  //可视化
  pcl::visualization::PCLVisualizer viewer("Sift keypoint");
  int v1(0);
  viewer.createViewPort(0,0,1,0.5,v1);
  viewer.setBackgroundColor( 255, 255, 255, v1);
  viewer.addPointCloud(cloud_xyz, "cloud", v1);
  viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,0,0,0,"cloud");
  int v2(0);
  viewer.createViewPort(0,0.5,1,1,v2);
  viewer.setBackgroundColor(255,233,122, v2);
  viewer.addPointCloud(cloud_temp, "keypoints");
  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;
  
}

PCL点云库学习笔记(6)——关键点_第2张图片

4.Harris特征点提取

#include 
#include 
#include 
#include 
#include 
//#include //harris
#include
#include 
#include 
#include 
#include 
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);
	pcl::PCDWriter writer;
	float r_normal;
	float r_keypoint;

	r_normal=0.1;
	r_keypoint=0.1;

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

	pcl::PointCloud<pcl::PointXYZI>::Ptr Harris_keypoints (new pcl::PointCloud<pcl::PointXYZI> ());
	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::LOWE);
	harris_detector->compute (*Harris_keypoints);
	cout<<"Harris_keypoints�Ĵ�С��"<<Harris_keypoints->size()<<endl;
	writer.write<pcl::PointXYZI> ("Harris_keypoints.pcd",*Harris_keypoints,false);

	pcl::visualization::PCLVisualizer visu3("clouds");
	visu3.setBackgroundColor(255,255,255);
	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 ();
}

PCL点云库学习笔记(6)——关键点_第3张图片
5.基于对应点分类的对象识别
使用对应点聚类算法(Correspondence Grouping Algorithms),对经过特征匹配得到的场景中的对应点对进行聚类,使其进一步成为待识别模型实例。该算法输出的每个聚类,代表场景中的一个模型实例假设,同时,对应一个变换矩阵,是该模型实例假设对应的位姿估计。

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

typedef pcl::PointXYZRGBA PointType;
typedef pcl::Normal NormalType;
typedef pcl::ReferenceFrame RFType;
typedef pcl::SHOT352 DescriptorType;

std::string model_filename_;
std::string scene_filename_;

//Algorithm params
bool show_keypoints_ (false);
bool show_correspondences_ (false);
bool use_cloud_resolution_ (false);
bool use_hough_ (true);
float model_ss_ (0.01f);
float scene_ss_ (0.03f);
float rf_rad_ (0.015f);
float descr_rad_ (0.02f);
float cg_size_ (0.01f);
float cg_thresh_ (5.0f);

void
showHelp (char *filename)
{
  std::cout << std::endl;
  std::cout << "***************************************************************************" << std::endl;
  std::cout << "*                                                                         *" << std::endl;
  std::cout << "*             Correspondence Grouping Tutorial - Usage Guide              *" << std::endl;
  std::cout << "*                                                                         *" << std::endl;
  std::cout << "***************************************************************************" << std::endl << std::endl;
  std::cout << "Usage: " << filename << " model_filename.pcd scene_filename.pcd [Options]" << std::endl << std::endl;
  std::cout << "Options:" << std::endl;
  std::cout << "     -h:                     Show this help." << std::endl;
  std::cout << "     -k:                     Show used keypoints." << std::endl;
  std::cout << "     -c:                     Show used correspondences." << std::endl;
  std::cout << "     -r:                     Compute the model cloud resolution and multiply" << std::endl;
  std::cout << "                             each radius given by that value." << std::endl;
  std::cout << "     --algorithm (Hough|GC): Clustering algorithm used (default Hough)." << std::endl;
  std::cout << "     --model_ss val:         Model uniform sampling radius (default 0.01)" << std::endl;
  std::cout << "     --scene_ss val:         Scene uniform sampling radius (default 0.03)" << std::endl;
  std::cout << "     --rf_rad val:           Reference frame radius (default 0.015)" << std::endl;
  std::cout << "     --descr_rad val:        Descriptor radius (default 0.02)" << std::endl;
  std::cout << "     --cg_size val:          Cluster size (default 0.01)" << std::endl;
  std::cout << "     --cg_thresh val:        Clustering threshold (default 5)" << std::endl << std::endl;
}

void
parseCommandLine (int argc, char *argv[])//解析命令行参数
{
  //Show help
  if (pcl::console::find_switch (argc, argv, "-h"))
  {
    showHelp (argv[0]);
    exit (0);
  }

  //Model & scene filenames
  std::vector<int> filenames;
  filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
  if (filenames.size () != 2)
  {
    std::cout << "Filenames missing.\n";
    showHelp (argv[0]);
    exit (-1);
  }

  model_filename_ = argv[filenames[0]];
  scene_filename_ = argv[filenames[1]];

  //Program behavior
  if (pcl::console::find_switch (argc, argv, "-k"))
  {
    show_keypoints_ = true;
  }
  if (pcl::console::find_switch (argc, argv, "-c"))
  {
    show_correspondences_ = true;
  }
  if (pcl::console::find_switch (argc, argv, "-r"))
  {
    use_cloud_resolution_ = true;//所有参数将与点云分辨率相乘得到最终使用参数
  }

  std::string used_algorithm;
  if (pcl::console::parse_argument (argc, argv, "--algorithm", used_algorithm) != -1)
  {
    if (used_algorithm.compare ("Hough") == 0)
    {
      use_hough_ = true;
    }else if (used_algorithm.compare ("GC") == 0)
    {
      use_hough_ = false;
    }
    else
    {
      std::cout << "Wrong algorithm name.\n";
      showHelp (argv[0]);
      exit (-1);
    }
  }

  //General parameters
  pcl::console::parse_argument (argc, argv, "--model_ss", model_ss_);
  pcl::console::parse_argument (argc, argv, "--scene_ss", scene_ss_);
  pcl::console::parse_argument (argc, argv, "--rf_rad", rf_rad_);
  pcl::console::parse_argument (argc, argv, "--descr_rad", descr_rad_);
  pcl::console::parse_argument (argc, argv, "--cg_size", cg_size_);
  pcl::console::parse_argument (argc, argv, "--cg_thresh", cg_thresh_);
}

double
computeCloudResolution (const pcl::PointCloud<PointType>::ConstPtr &cloud)//计算点云的空间分辨率,算出点云中每个点与其邻近点距离的平均值
{
  double res = 0.0;
  int n_points = 0;
  int nres;
  std::vector<int> indices (2);
  std::vector<float> sqr_distances (2);
  pcl::search::KdTree<PointType> tree;
  tree.setInputCloud (cloud);

  for (size_t i = 0; i < cloud->size (); ++i)
  {
    if (! pcl_isfinite ((*cloud)[i].x))
    {
      continue;
    }
    //Considering the second neighbor since the first is the point itself.
    nres = tree.nearestKSearch (i, 2, indices, sqr_distances);
    if (nres == 2)
    {
      res += sqrt (sqr_distances[1]);
      ++n_points;
    }
  }
  if (n_points != 0)
  {
    res /= n_points;
  }
  return res;
}

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

  pcl::PointCloud<PointType>::Ptr model (new pcl::PointCloud<PointType> ());
  pcl::PointCloud<PointType>::Ptr model_keypoints (new pcl::PointCloud<PointType> ());
  pcl::PointCloud<PointType>::Ptr scene (new pcl::PointCloud<PointType> ());
  pcl::PointCloud<PointType>::Ptr scene_keypoints (new pcl::PointCloud<PointType> ());
  pcl::PointCloud<NormalType>::Ptr model_normals (new pcl::PointCloud<NormalType> ());
  pcl::PointCloud<NormalType>::Ptr scene_normals (new pcl::PointCloud<NormalType> ());
  pcl::PointCloud<DescriptorType>::Ptr model_descriptors (new pcl::PointCloud<DescriptorType> ());
  pcl::PointCloud<DescriptorType>::Ptr scene_descriptors (new pcl::PointCloud<DescriptorType> ());

  //
  //  Load clouds
  //
  if (pcl::io::loadPCDFile (model_filename_, *model) < 0)
  {
    std::cout << "Error loading model cloud." << std::endl;
    showHelp (argv[0]);
    return (-1);
  }
  if (pcl::io::loadPCDFile (scene_filename_, *scene) < 0)
  {
    std::cout << "Error loading scene cloud." << std::endl;
    showHelp (argv[0]);
    return (-1);
  }

  //
  //  Set up resolution invariance
  //
  if (use_cloud_resolution_)
  {
    float resolution = static_cast<float> (computeCloudResolution (model));
    if (resolution != 0.0f)
    {
      model_ss_   *= resolution;
      scene_ss_   *= resolution;
      rf_rad_     *= resolution;
      descr_rad_  *= resolution;
      cg_size_    *= resolution;
    }

    std::cout << "Model resolution:       " << resolution << std::endl;
    std::cout << "Model sampling size:    " << model_ss_ << std::endl;
    std::cout << "Scene sampling size:    " << scene_ss_ << std::endl;
    std::cout << "LRF support radius:     " << rf_rad_ << std::endl;
    std::cout << "SHOT descriptor radius: " << descr_rad_ << std::endl;
    std::cout << "Clustering bin size:    " << cg_size_ << std::endl << std::endl;
  }

  //
  //  Compute Normals
  //
  pcl::NormalEstimationOMP<PointType, NormalType> norm_est;//计算场景和模型中的每个点的法向量
  norm_est.setKSearch (10);//计算法向量时利用10个临近点,根据经验值设定
  norm_est.setInputCloud (model);
  norm_est.compute (*model_normals);

  norm_est.setInputCloud (scene);
  norm_est.compute (*scene_normals);

  //
  //  Downsample Clouds to Extract keypoints
  //直接使用下采样达到的关键点
  pcl::PointCloud<int> sampled_indices;
  pcl::UniformSampling<PointType> uniform_sampling;
  uniform_sampling.setInputCloud (model);
  uniform_sampling.setRadiusSearch (model_ss_);
  uniform_sampling.compute (sampled_indices);
  pcl::copyPointCloud (*model, sampled_indices.points, *model_keypoints);
  std::cout << "Model total points: " << model->size () << "; Selected Keypoints: " << model_keypoints->size () << std::endl;

  uniform_sampling.setInputCloud (scene);
  uniform_sampling.setRadiusSearch (scene_ss_);
  uniform_sampling.compute (sampled_indices);
  pcl::copyPointCloud (*scene, sampled_indices.points, *scene_keypoints);
  std::cout << "Scene total points: " << scene->size () << "; Selected Keypoints: " << scene_keypoints->size () << std::endl;


  //
  //  Compute Descriptor for keypoints
  //
  pcl::SHOTEstimationOMP<PointType, NormalType, DescriptorType> descr_est;
  descr_est.setRadiusSearch (descr_rad_);

  descr_est.setInputCloud (model_keypoints);
  descr_est.setInputNormals (model_normals);
  descr_est.setSearchSurface (model);
  descr_est.compute (*model_descriptors);

  descr_est.setInputCloud (scene_keypoints);
  descr_est.setInputNormals (scene_normals);
  descr_est.setSearchSurface (scene);
  descr_est.compute (*scene_descriptors);

  //
  //  Find Model-Scene Correspondences with KdTree
  //
  pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ());

  pcl::KdTreeFLANN<DescriptorType> match_search;//基于kdtreeFLANN在欧式空间中对
  match_search.setInputCloud (model_descriptors);

  //  For each scene keypoint descriptor, find nearest neighbor into the model keypoints descriptor cloud and add it to the correspondences vector.
  for (size_t i = 0; i < scene_descriptors->size (); ++i)
  {
    std::vector<int> neigh_indices (1);
    std::vector<float> neigh_sqr_dists (1);
    if (!pcl_isfinite (scene_descriptors->at (i).descriptor[0])) //skipping NaNs
    {
      continue;
    }
    int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), 1, neigh_indices, neigh_sqr_dists);//搜索场景描述子与模型描述子最近邻点对,找到最近邻点返回值为1
    if(found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) //  add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design)
    {
      pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
      model_scene_corrs->push_back (corr);
    }
  }
  std::cout << "Correspondences found: " << model_scene_corrs->size () << std::endl;

  //
  //  Actual Clustering
  //
  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
  std::vector<pcl::Correspondences> clustered_corrs;

  //  Using Hough3D
  if (use_hough_)
  {
    //
    //  Compute (Keypoints) Reference Frames only for Hough
    //
    pcl::PointCloud<RFType>::Ptr model_rf (new pcl::PointCloud<RFType> ());
    pcl::PointCloud<RFType>::Ptr scene_rf (new pcl::PointCloud<RFType> ());

    pcl::BOARDLocalReferenceFrameEstimation<PointType, NormalType, RFType> rf_est;
    rf_est.setFindHoles (true);
    rf_est.setRadiusSearch (rf_rad_);

    rf_est.setInputCloud (model_keypoints);
    rf_est.setInputNormals (model_normals);
    rf_est.setSearchSurface (model);
    rf_est.compute (*model_rf);

    rf_est.setInputCloud (scene_keypoints);
    rf_est.setInputNormals (scene_normals);
    rf_est.setSearchSurface (scene);
    rf_est.compute (*scene_rf);

    //  Clustering
    pcl::Hough3DGrouping<PointType, PointType, RFType, RFType> clusterer;
    clusterer.setHoughBinSize (cg_size_);
    clusterer.setHoughThreshold (cg_thresh_);
    clusterer.setUseInterpolation (true);
    clusterer.setUseDistanceWeight (false);

    clusterer.setInputCloud (model_keypoints);
    clusterer.setInputRf (model_rf);
    clusterer.setSceneCloud (scene_keypoints);
    clusterer.setSceneRf (scene_rf);
    clusterer.setModelSceneCorrespondences (model_scene_corrs);

    //clusterer.cluster (clustered_corrs);
    clusterer.recognize (rototranslations, clustered_corrs);
  }
  else // Using GeometricConsistency
  {
    pcl::GeometricConsistencyGrouping<PointType, PointType> gc_clusterer;
    gc_clusterer.setGCSize (cg_size_);
    gc_clusterer.setGCThreshold (cg_thresh_);

    gc_clusterer.setInputCloud (model_keypoints);
    gc_clusterer.setSceneCloud (scene_keypoints);
    gc_clusterer.setModelSceneCorrespondences (model_scene_corrs);

    //gc_clusterer.cluster (clustered_corrs);
    gc_clusterer.recognize (rototranslations, clustered_corrs);
  }

  //
  //  Output results
  //
  std::cout << "Model instances found: " << rototranslations.size () << std::endl;
  for (size_t i = 0; i < rototranslations.size (); ++i)
  {
    std::cout << "\n    Instance " << i + 1 << ":" << std::endl;
    std::cout << "        Correspondences belonging to this instance: " << clustered_corrs[i].size () << std::endl;

    // Print the rotation matrix and translation vector
    Eigen::Matrix3f rotation = rototranslations[i].block<3,3>(0, 0);
    Eigen::Vector3f translation = rototranslations[i].block<3,1>(0, 3);

    printf ("\n");
    printf ("            | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2));
    printf ("        R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2));
    printf ("            | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2));
    printf ("\n");
    printf ("        t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2));
  }

  //
  //  Visualization
  //
  pcl::visualization::PCLVisualizer viewer ("���ƿ�PCLѧϰ�̵̳ڶ���-���ڶ�Ӧ������3Dģ��ʶ��");
  viewer.addPointCloud (scene, "scene_cloud");
  viewer.setBackgroundColor(255,255,255);
  pcl::PointCloud<PointType>::Ptr off_scene_model (new pcl::PointCloud<PointType> ());
  pcl::PointCloud<PointType>::Ptr off_scene_model_keypoints (new pcl::PointCloud<PointType> ());

  if (show_correspondences_ || show_keypoints_)
  {
    //  We are translating the model so that it doesn't end in the middle of the scene representation
    pcl::transformPointCloud (*model, *off_scene_model, Eigen::Vector3f (0,0,0), Eigen::Quaternionf (1, 0, 0, 0));
    pcl::transformPointCloud (*model_keypoints, *off_scene_model_keypoints, Eigen::Vector3f (0,0,0), Eigen::Quaternionf (1, 0, 0, 0));

    pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_color_handler (off_scene_model, 0, 255, 0);
    viewer.addPointCloud (off_scene_model, off_scene_model_color_handler, "off_scene_model");
  }

  if (show_keypoints_)
  {
    pcl::visualization::PointCloudColorHandlerCustom<PointType> scene_keypoints_color_handler (scene_keypoints, 0, 0, 255);
    viewer.addPointCloud (scene_keypoints, scene_keypoints_color_handler, "scene_keypoints");
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "scene_keypoints");

    pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_keypoints_color_handler (off_scene_model_keypoints, 0, 0, 255);
    viewer.addPointCloud (off_scene_model_keypoints, off_scene_model_keypoints_color_handler, "off_scene_model_keypoints");
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "off_scene_model_keypoints");
  }

  for (size_t i = 0; i < rototranslations.size (); ++i)
  {
    pcl::PointCloud<PointType>::Ptr rotated_model (new pcl::PointCloud<PointType> ());
    pcl::transformPointCloud (*model, *rotated_model, rototranslations[i]);

    std::stringstream ss_cloud;
    ss_cloud << "instance" << i;

    pcl::visualization::PointCloudColorHandlerCustom<PointType> rotated_model_color_handler (rotated_model, 255, 0, 0);
    viewer.addPointCloud (rotated_model, rotated_model_color_handler, ss_cloud.str ());

    if (show_correspondences_)
    {
      for (size_t j = 0; j < clustered_corrs[i].size (); ++j)
      {
        std::stringstream ss_line;
        ss_line << "correspondence_line" << i << "_" << j;
        PointType& model_point = off_scene_model_keypoints->at (clustered_corrs[i][j].index_query);
        PointType& scene_point = scene_keypoints->at (clustered_corrs[i][j].index_match);

        //  We are drawing a line for each pair of clustered correspondences found between the model and the scene
        viewer.addLine<PointType, PointType> (model_point, scene_point, 0, 255, 0, ss_line.str ());
      }
    }
  }

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

  return (0);
}

PCL点云库学习笔记(6)——关键点_第4张图片

你可能感兴趣的:(点云处理,PCL,点云处理)