如果要对一个三维点云进行描述，光有点云的位置是不够的，常常需要计算一些额外的参数，
比如法线方向、曲率、文理特征、颜色、领域中心距、协方差矩阵、熵等等。
如同图像的特征（sifi surf orb）一样，我们需要使用类似的方式来描述三维点云的特征。
常用的特征描述算法有：
法线和曲率计算 normal_3d_feature 、
特征值分析、
PFH 点特征直方图描述子 nk2、
FPFH 快速点特征直方图描述子 FPFH是PFH的简化形式 nk、
3D Shape Context、 文理特征
Spin Image
VFH视点特征直方图(Viewpoint Feature Histogram)
NARF关键点 pcl::NarfKeypoint narf特征 pcl::NarfDescriptor
RoPs特征(Rotational Projection Statistics)
然后计算narf特征 pcl::NarfDescriptor
边缘提取
直接把三维的点云投射成二维的图像不就好了。
这种投射方法叫做range_image.
#include <pcl/range_image/range_image.h>// RangeImage 深度图像
#include <pcl/keypoints/narf_keypoint.h>// narf关键点检测
这个类还允许提取云的轴对齐和定向包围盒。
但请记住，不可能的最小提取OBB包围盒。
首先计算点云的协方差矩阵，提取其特征值和向量。
你可以认为所得的特征向量是标准化的，
总是形成右手坐标系（主要特征向量表示x轴，而较小的向量表示z轴）。
在下一个步骤中，迭代过程发生。每次迭代时旋转主特征向量。
旋转顺序总是相同的，并且在其他特征向量周围执行，
这提供了点云旋转的不变性。
此后，我们将把这个旋转主向量作为当前轴。
然后在当前轴上计算转动惯量 和 将点云投影到以旋转向量为法线的平面上
比如法线方向、曲率、文理特征、颜色、领域中心距、协方差矩阵、熵等等。
如同图像的特征（sifi surf orb）一样，我们需要使用类似的方式来描述三维点云的特征。
常用的特征描述算法有：
法线和曲率计算 normal_3d_feature 、
特征值分析、
PFH 点特征直方图描述子 nk2、
FPFH 快速点特征直方图描述子 FPFH是PFH的简化形式 nk、
3D Shape Context、 文理特征
Spin Image
VFH视点特征直方图(Viewpoint Feature Histogram)
NARF关键点 pcl::NarfKeypoint narf特征 pcl::NarfDescriptor
RoPs特征(Rotational Projection Statistics)
(GASD）全局一致的空间分布描述子特征 Globally Aligned Spatial Distribution (GASD) descriptors
【1 】估计表面法线的解决方案就变成了分析一个协方差矩阵的特征矢量和特征值
（或者PCA—主成分分析），这个协方差矩阵从查询点的近邻元素中创建。
更具体地说，对于每一个点Pi,对应的协方差矩阵。
#include <pcl/features/normal_3d.h>//法线特征
/*
点云表面法线特征
直接从点云数据集中近似推断表面法线。
下载桌子点云数据
https://raw.github.com/PointCloudLibrary/data/master/tutorials/table_scene_lms400.pcd
表面法线是几何体表面的重要属性，在很多领域都有大量应用，
例如：在进行光照渲染时产生符合可视习惯的效果时需要表面法线信息才能正常进行，
对于一个已知的几何体表面，根据垂直于点表面的矢量，因此推断表面某一点的法线方向通常比较简单。
确定表面一点法线的问题近似于估计表面的一个相切面法线的问题，
因此转换过来以后就变成一个最小二乘法平面拟合估计问题。
估计表面法线的解决方案就变成了分析一个协方差矩阵的特征矢量和特征值
（或者PCA—主成分分析），这个协方差矩阵从查询点的近邻元素中创建。
更具体地说，对于每一个点Pi,对应的协方差矩阵。
*/
#include <iostream>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>//点云文件pcd 读写
#include <pcl/visualization/cloud_viewer.h>//点云可视化
#include <pcl/visualization/pcl_visualizer.h>// 高级可视化点云类
#include <pcl/features/normal_3d.h>//法线特征
#include <boost/thread/thread.hpp>
using namespace std;
// 别名
typedef pcl::PointCloud<pcl::PointXYZ> Cloud;
typedef pcl::PointXYZ PointType;
int
main (int argc, char** argv)
{
// 定义 点云对象 指针
Cloud::Ptr cloud_ptr (new Cloud);
// 读取点云文件 填充点云对象
pcl::PCDReader reader;
reader.read( "../../Filtering/table_scene_lms400.pcd", *cloud_ptr);
if(cloud_ptr==NULL) { cout << "pcd file read err" << endl; return -1;}
cout << "PointCLoud size() " << cloud_ptr->width * cloud_ptr->height
<< " data points ( " << pcl::getFieldsList (*cloud_ptr) << "." << endl;
// 创建法线估计类====================================
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud_ptr);
/*
法线估计类NormalEstimation的实际计算调用程序内部执行以下操作：
对点云P中的每个点p
1.得到p点的最近邻元素
2.计算p点的表面法线n
3.检查n的方向是否一致指向视点，如果不是则翻转
在PCL内估计一点集对应的协方差矩阵，可以使用以下函数调用实现：
//定义每个表面小块的3x3协方差矩阵的存储对象
Eigen::Matrix3fcovariance_matrix;
//定义一个表面小块的质心坐标16-字节对齐存储对象
Eigen::Vector4fxyz_centroid;
//估计质心坐标
compute3DCentroid(cloud,xyz_centroid);
//计算3x3协方差矩阵
computeCovarianceMatrix(cloud,xyz_centroid,covariance_matrix);
*/
// 添加搜索算法 kdtree search 最近的几个点 估计平面 协方差矩阵PCA分解 求解法线
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
ne.setSearchMethod (tree);
// 输出点云 带有法线描述
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ptr (new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<pcl::Normal>& cloud_normals = *cloud_normals_ptr;
// Use all neighbors in a sphere of radius 3cm
ne.setRadiusSearch (0.03);//半径内搜索临近点
//ne.setKSearch(8); //其二 指定临近点数量
// 计算表面法线特征
ne.compute (cloud_normals);
// 点云+法线 可视化
//pcl::visualization::PCLVisualizer viewer("pcd viewer");// 显示窗口的名字
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_ptr (new pcl::visualization::PCLVisualizer ("3D Viewer"));
//设置一个boost共享对象，并分配内存空间
viewer_ptr->setBackgroundColor(0.0, 0.0, 0.0);//背景黑色
//viewer.setBackgroundColor (1, 1, 1);//白色
viewer_ptr->addCoordinateSystem (1.0f, "global");//坐标系
// pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
//pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZ> rgb(cloud);
// pcl::visualization::PointCloudColorHandlerRandom<PointType> cloud_color_handler(cloud_ptr);
//该句的意思是：对输入的点云着色，Random表示的是随机上色，以上是其他两种渲染色彩的方式.
pcl::visualization::PointCloudColorHandlerCustom<PointType> cloud_color_handler (cloud_ptr, 255, 0, 0);//红色
//viewer->addPointCloud<pcl::PointXYZRGB>(cloud_ptr,cloud_color_handler,"sample cloud");//PointXYZRGB 类型点
viewer_ptr->addPointCloud<PointType>(cloud_ptr, cloud_color_handler, "original point cloud");//点云标签
viewer_ptr->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud_ptr, cloud_normals_ptr, 5,0.02,"normal");//法线标签
//其中，参数5表示整个点云中每5各点显示一个法向量（若全部显示，可设置为1， 0.02表示法向量的长度，最后一个参数暂时还不知道 如何影响的）);
viewer_ptr->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "original point cloud");
//渲染属性，可视化工具，3维数据， 其中PCL_VISUALIZER_POINT_SIZE表示设置点的大小为3
//viewer_ptr->addCoordinateSystem(1.0);//建立空间直角坐标系
//viewer_ptr->setCameraPosition(0,0,200); //设置坐标原点
viewer_ptr->initCameraParameters();//初始化相机参数
while (!viewer_ptr->wasStopped())
{
viewer_ptr->spinOnce ();
pcl_sleep(0.01);
}
return (0);
}
【2】使用积分图计算一个有序点云的法线，注意该方法只适用于有序点云
表面法线是几何体表面的重要属性，在很多领域都有大量应用，
例如：在进行光照渲染时产生符合可视习惯的效果时需要表面法线信息才能正常进行，
对于一个已知的几何体表面，根据垂直于点表面的矢量，因此推断表面某一点的法线方向通常比较简单。
头文件
#include <pcl/features/integral_image_normal.h>
/*
使用积分图计算一个有序点云的法线，注意该方法只适用于有序点云!!
点云表面法线特征
直接从点云数据集中近似推断表面法线。
下载桌子点云数据
https://raw.github.com/PointCloudLibrary/data/master/tutorials/table_scene_lms400.pcd
表面法线是几何体表面的重要属性，在很多领域都有大量应用，
例如：在进行光照渲染时产生符合可视习惯的效果时需要表面法线信息才能正常进行，
对于一个已知的几何体表面，根据垂直于点表面的矢量，因此推断表面某一点的法线方向通常比较简单。
积分图像估计 点云表面法线
*/
#include <iostream>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>//点云文件pcd 读写
#include <pcl/visualization/cloud_viewer.h>//点云可视化
//#include <pcl/features/normal_3d.h>//法线特征
#include <pcl/features/integral_image_normal.h>
using namespace std;
// 别名
typedef pcl::PointCloud<pcl::PointXYZ> Cloud;
typedef pcl::PointXYZ PointType;
int
main (int argc, char** argv)
{
// 定义 点云对象 指针
Cloud::Ptr cloud_ptr (new Cloud);
// 读取点云文件 填充点云对象
pcl::PCDReader reader;
reader.read( "../../Filtering/table_scene_lms400.pcd", *cloud_ptr);//非有序点云不可用
if(cloud_ptr==NULL) { cout << "pcd file read err" << endl; return -1;}
cout << "PointCLoud size() " << cloud_ptr->width * cloud_ptr->height
<< " data points ( " << pcl::getFieldsList (*cloud_ptr) << "." << endl;
// 估计的法线 normals
pcl::PointCloud<pcl::Normal>::Ptr normals_ptr (new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<pcl::Normal>& normals = *normals_ptr;
// 积分图像方法 估计点云表面法线
pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
/*
预测方法
enum NormalEstimationMethod
{
COVARIANCE_MATRIX, 从最近邻的协方差矩阵创建了9个积分图去计算一个点的法线
AVERAGE_3D_GRADIENT, 创建了6个积分图去计算3D梯度里面竖直和水平方向的光滑部分，同时利用两个梯度的卷积来计算法线。
AVERAGE_DEPTH_CHANGE 造了一个单一的积分图，从平均深度的变化中来计算法线。
};
*/
ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
ne.setMaxDepthChangeFactor(0.02f);
ne.setNormalSmoothingSize(10.0f);
ne.setInputCloud(cloud_ptr);
ne.compute(normals);
// 点云+法线 可视化
pcl::visualization::PCLVisualizer viewer("pcd viewer");// 显示窗口的名字
viewer.setBackgroundColor(0.0, 0.0, 0.0);//背景黑色
//viewer.setBackgroundColor (1, 1, 1);//白色
viewer.addCoordinateSystem (1.0f, "global");//坐标系
viewer.addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud_ptr, normals_ptr);
//pcl::visualization::PointCloudColorHandlerCustom<PointType> cloud_color_handler (cloud_ptr, 1, 0, 0);//红色
//viewer.addPointCloud (cloud_ptr, cloud_color_handler, "original point cloud");
while (!viewer.wasStopped())
{
viewer.spinOnce ();
pcl_sleep(0.05);
}
return (0);
}
3】 点特征直方图(PFH)描述子
计算法线---计算临近点对角度差值-----直方图--
点特征直方图(PFH)描述子
点特征直方图(Point Feature Histograms)
正如点特征表示法所示，表面法线和曲率估计是某个点周围的几何特征基本表示法。
虽然计算非常快速容易，但是无法获得太多信息，因为它们只使用很少的
几个参数值来近似表示一个点的k邻域的几何特征。然而大部分场景中包含许多特征点，
这些特征点有相同的或者非常相近的特征值，因此采用点特征表示法，
其直接结果就减少了全局的特征信息。
http://www.pclcn.org/study/shownews.php?lang=cn&id=101
是基于点与其k邻域之间的关系以及它们的估计法线，
简言之，它考虑估计法线方向之间所有的相互作用，
试图捕获最好的样本表面变化情况，以描述样本的几何特征。
每一点对，原有12个参数，6个坐标值，6个坐标姿态（基于法线）
PHF计算没一点对的 相对坐标角度差值三个值和 坐标点之间的欧氏距离 d
从12个参数减少到4个参数
默认PFH的实现使用5个区间分类（例如：四个特征值中的每个都使用5个区间来统计），
其中不包括距离（在上文中已经解释过了——但是如果有需要的话，
也可以通过用户调用computePairFeatures方法来获得距离值），
这样就组成了一个125浮点数元素的特征向量（15），
其保存在一个pcl::PFHSignature125的点类型中。
头文件
#include <pcl/features/pfh.h>
#include <pcl/features/normal_3d.h>//法线特征
/*
计算法线---计算临近点对角度差值-----直方图--
点特征直方图(PFH)描述子
点特征直方图(Point Feature Histograms)
正如点特征表示法所示，表面法线和曲率估计是某个点周围的几何特征基本表示法。
虽然计算非常快速容易，但是无法获得太多信息，因为它们只使用很少的
几个参数值来近似表示一个点的k邻域的几何特征。然而大部分场景中包含许多特征点，
这些特征点有相同的或者非常相近的特征值，因此采用点特征表示法，
其直接结果就减少了全局的特征信息。
http://www.pclcn.org/study/shownews.php?lang=cn&id=101
通过参数化查询点与邻域点之间的空间差异，并形成一个多维直方图对点的k邻域几何属性进行描述。
直方图所在的高维超空间为特征表示提供了一个可度量的信息空间，
对点云对应曲面的6维姿态来说它具有不变性，
并且在不同的采样密度或邻域的噪音等级下具有鲁棒性。
是基于点与其k邻域之间的关系以及它们的估计法线，
简言之，它考虑估计法线方向之间所有的相互作用，
试图捕获最好的样本表面变化情况，以描述样本的几何特征。
Pq 用红色标注并放在圆球的中间位置，半径为r，
(Pq)的所有k邻元素（即与点Pq的距离小于半径r的所有点）
全部互相连接在一个网络中。最终的PFH描述子通过计
算邻域内所有两点之间关系而得到的直方图，
因此存在一个O(nk^2) 的计算复杂性。
每一点对，原有12个参数，6个坐标值，6个坐标姿态（基于法线）
PHF计算没一点对的 相对坐标角度差值三个值和 坐标点之间的欧氏距离 d
从12个参数减少到4个参数
computePairFeatures (const Eigen::Vector4f &p1, const Eigen::Vector4f &n1,
const Eigen::Vector4f &p2, const Eigen::Vector4f &n2,
float &f1, float &f2, float &f3, float &f4);
为查询点创建最终的PFH表示，所有的四元组将会以某种统计的方式放进直方图中，
这个过程首先把每个特征值范围划分为b个子区间，并统计落在每个子区间的点数目，
因为四分之三的特征在上述中为法线之间的角度计量，
在三角化圆上可以将它们的参数值非常容易地归一到相同的区间内。
默认PFH的实现使用5个区间分类（例如：四个特征值中的每个都使用5个区间来统计），
其中不包括距离（在上文中已经解释过了——但是如果有需要的话，
也可以通过用户调用computePairFeatures方法来获得距离值），
这样就组成了一个125浮点数元素的特征向量（35），
其保存在一个pcl::PFHSignature125的点类型中。
*/
#include <pcl/point_types.h>
#include <pcl/features/pfh.h>
#include <pcl/io/pcd_io.h>//点云文件pcd 读写
#include <pcl/features/normal_3d.h>//法线特征
#include <pcl/visualization/histogram_visualizer.h> //直方图的可视化
#include <pcl/visualization/pcl_plotter.h>// 直方图的可视化 方法2
//using namespace std;
using std::cout;
using std::endl;
int main(int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
//======【1】 读取点云文件 填充点云对象======
pcl::PCDReader reader;
reader.read( "../../Filtering/table_scene_lms400.pcd", *cloud_ptr);
if(cloud_ptr==NULL) { cout << "pcd file read err" << endl; return -1;}
cout << "PointCLoud size() " << cloud_ptr->width * cloud_ptr->height
<< " data points ( " << pcl::getFieldsList (*cloud_ptr) << "." << endl;
// =====【2】计算法线========创建法线估计类====================================
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud_ptr);
/*
法线估计类NormalEstimation的实际计算调用程序内部执行以下操作：
对点云P中的每个点p
1.得到p点的最近邻元素
2.计算p点的表面法线n
3.检查n的方向是否一致指向视点，如果不是则翻转
在PCL内估计一点集对应的协方差矩阵，可以使用以下函数调用实现：
//定义每个表面小块的3x3协方差矩阵的存储对象
Eigen::Matrix3fcovariance_matrix;
//定义一个表面小块的质心坐标16-字节对齐存储对象
Eigen::Vector4fxyz_centroid;
//估计质心坐标
compute3DCentroid(cloud,xyz_centroid);
//计算3x3协方差矩阵
computeCovarianceMatrix(cloud,xyz_centroid,covariance_matrix);
*/
// 添加搜索算法 kdtree search 最近的几个点 估计平面 协方差矩阵PCA分解 求解法线
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
ne.setSearchMethod (tree);//设置近邻搜索算法
// 输出点云 带有法线描述
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ptr (new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<pcl::Normal>& cloud_normals = *cloud_normals_ptr;
// Use all neighbors in a sphere of radius 3cm
ne.setRadiusSearch (0.03);//半价内搜索临近点 3cm
// 计算表面法线特征
ne.compute (cloud_normals);
//=======【3】创建PFH估计对象pfh，并将输入点云数据集cloud和法线normals传递给它=================
pcl::PFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::PFHSignature125> pfh;// phf特征估计其器
pfh.setInputCloud (cloud_ptr);
pfh.setInputNormals (cloud_normals_ptr);
//如果点云是类型为PointNormal,则执行pfh.setInputNormals (cloud);
//创建一个空的kd树表示法，并把它传递给PFH估计对象。
//基于已给的输入数据集，建立kdtree
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZ> ());
//pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree2 (new pcl::KdTreeFLANN<pcl::PointXYZ> ()); //-- older call for PCL 1.5-
pfh.setSearchMethod (tree2);//设置近邻搜索算法
//输出数据集
pcl::PointCloud<pcl::PFHSignature125>::Ptr pfh_fe_ptr (new pcl::PointCloud<pcl::PFHSignature125> ());//phf特征
//使用半径在5厘米范围内的所有邻元素。
//注意：此处使用的半径必须要大于估计表面法线时使用的半径!!!
pfh.setRadiusSearch (0.05);
//计算pfh特征值
pfh.compute (*pfh_fe_ptr);
cout << "phf feature size : " << pfh_fe_ptr->points.size() << endl;
// 应该与input cloud->points.size ()有相同的大小，即每个点都有一个pfh特征向量
// ========直方图可视化=============================
pcl::visualization::PCLHistogramVisualizer view;//直方图可视化
view.setBackgroundColor(255,0,0);//背景红色
view.addFeatureHistogram<pcl::PFHSignature125> (*pfh_fe_ptr,"pfh",100);
view.spinOnce(); //循环的次数
//view.spin(); //无限循环
// pcl::visualization::PCLPlotter plotter;
//plotter.addFeatureHistogram(*pfh_fe_ptr, 300); //设置的很坐标长度，该值越大，则显示的越细致
//plotter.plot();
return 0;
}
【4】phf点特征直方图 计算复杂度还是太高
计算法线---计算临近点对角度差值-----直方图--
因此存在一个O(nk^2) 的计算复杂性。
k个点之间相互的点对 k×k条连接线
每一点对，原有12个参数，6个坐标值，6个坐标姿态（基于法线）
PHF计算没一点对的 相对坐标角度差值三个值和 坐标点之间的欧氏距离 d
从12个参数减少到4个参数
快速点特征直方图FPFH（Fast Point Feature Histograms）把算法的计算复杂度降低到了O(nk) ，
但是任然保留了PFH大部分的识别特性。
查询点和周围k个点的连线 的4参数特征
也就是1×k=k个线
默认的FPFH实现使用11个统计子区间（例如：四个特征值中的每个都将它的参数区间分割为11个），
特征直方图被分别计算然后合并得出了浮点值的一个33元素的特征向量，
这些保存在一个pcl::FPFHSignature33点类型中。
/*
phf点特征直方图 计算复杂度还是太高
计算法线---计算临近点对角度差值-----直方图--
因此存在一个O(nk^2) 的计算复杂性。
k个点之间相互的点对 k×k条连接线
每一点对，原有12个参数，6个坐标值，6个坐标姿态（基于法线）
PHF计算没一点对的 相对坐标角度差值三个值和 坐标点之间的欧氏距离 d
从12个参数减少到4个参数
快速点特征直方图FPFH（Fast Point Feature Histograms）把算法的计算复杂度降低到了O(nk) ，
但是任然保留了PFH大部分的识别特性。
查询点和周围k个点的连线 的4参数特征
也就是1×k=k个线
默认的FPFH实现使用11个统计子区间（例如：四个特征值中的每个都将它的参数区间分割为11个），
特征直方图被分别计算然后合并得出了浮点值的一个33元素的特征向量，
这些保存在一个pcl::FPFHSignature33点类型中。
*/
#include <pcl/point_types.h>
//#include <pcl/features/pfh.h>
#include <pcl/features/fpfh.h>
#include <pcl/io/pcd_io.h>//点云文件pcd 读写
#include <pcl/features/normal_3d.h>//法线特征
#include <pcl/visualization/histogram_visualizer.h> //直方图的可视化
#include <pcl/visualization/pcl_plotter.h>// 直方图的可视化 方法2
//using namespace std;
using std::cout;
using std::endl;
int main(int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
//======【1】 读取点云文件 填充点云对象======
pcl::PCDReader reader;
reader.read( "../../Filtering/table_scene_lms400.pcd", *cloud_ptr);
if(cloud_ptr==NULL) { cout << "pcd file read err" << endl; return -1;}
cout << "PointCLoud size() " << cloud_ptr->width * cloud_ptr->height
<< " data points ( " << pcl::getFieldsList (*cloud_ptr) << "." << endl;
// =====【2】计算法线========创建法线估计类====================================
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud_ptr);
/*
法线估计类NormalEstimation的实际计算调用程序内部执行以下操作：
对点云P中的每个点p
1.得到p点的最近邻元素
2.计算p点的表面法线n
3.检查n的方向是否一致指向视点，如果不是则翻转
在PCL内估计一点集对应的协方差矩阵，可以使用以下函数调用实现：
//定义每个表面小块的3x3协方差矩阵的存储对象
Eigen::Matrix3fcovariance_matrix;
//定义一个表面小块的质心坐标16-字节对齐存储对象
Eigen::Vector4fxyz_centroid;
//估计质心坐标
compute3DCentroid(cloud,xyz_centroid);
//计算3x3协方差矩阵
computeCovarianceMatrix(cloud,xyz_centroid,covariance_matrix);
*/
// 添加搜索算法 kdtree search 最近的几个点 估计平面 协方差矩阵PCA分解 求解法线
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
ne.setSearchMethod (tree);//设置近邻搜索算法
// 输出点云 带有法线描述
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ptr (new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<pcl::Normal>& cloud_normals = *cloud_normals_ptr;
// Use all neighbors in a sphere of radius 3cm
ne.setRadiusSearch (0.03);//半价内搜索临近点 3cm
// 计算表面法线特征
ne.compute (cloud_normals);
//=======【3】创建FPFH估计对象fpfh, 并将输入点云数据集cloud和法线normals传递给它=================
//pcl::PFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::PFHSignature125> pfh;// phf特征估计其器
pcl::FPFHEstimation<pcl::PointXYZ,pcl::Normal,pcl::FPFHSignature33> fpfh;
// pcl::FPFHEstimationOMP<pcl::PointXYZ,pcl::Normal,pcl::FPFHSignature33> fpfh;//多核加速
fpfh.setInputCloud (cloud_ptr);
fpfh.setInputNormals (cloud_normals_ptr);
//如果点云是类型为PointNormal,则执行pfh.setInputNormals (cloud);
//创建一个空的kd树表示法，并把它传递给PFH估计对象。
//基于已给的输入数据集，建立kdtree
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZ> ());
//pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree2 (new pcl::KdTreeFLANN<pcl::PointXYZ> ()); //-- older call for PCL 1.5-
fpfh.setSearchMethod (tree2);//设置近邻搜索算法
//输出数据集
//pcl::PointCloud<pcl::PFHSignature125>::Ptr pfh_fe_ptr (new pcl::PointCloud<pcl::PFHSignature125> ());//phf特征
pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh_fe_ptr (new pcl::PointCloud<pcl::FPFHSignature33>());//fphf特征
//使用半径在5厘米范围内的所有邻元素。
//注意：此处使用的半径必须要大于估计表面法线时使用的半径!!!
fpfh.setRadiusSearch (0.05);
//计算pfh特征值
fpfh.compute (*fpfh_fe_ptr);
cout << "phf feature size : " << fpfh_fe_ptr->points.size() << endl;
// 应该与input cloud->points.size ()有相同的大小，即每个点都有一个pfh特征向量
// ========直方图可视化=============================
pcl::visualization::PCLHistogramVisualizer view;//直方图可视化
view.setBackgroundColor(255,0,0);//背景红色
view.addFeatureHistogram<pcl::FPFHSignature33> (*fpfh_fe_ptr,"fpfh",100); //对下标为100的点的直方图特征可视化
view.spinOnce(); //循环的次数
//view.spin(); //无限循环
// pcl::visualization::PCLPlotter plotter;
//plotter.addFeatureHistogram(*fpfh_fe_ptr, 300); //设置的很坐标长度，该值越大，则显示的越细致
//plotter.plot();
return 0;
}
【5】视点特征直方图VFH(Viewpoint Feature Histogram)描述子，
它是一种新的特征表示形式，应用在点云聚类识别和六自由度位姿估计问题。
视点特征直方图（或VFH）是源于FPFH描述子.
由于它的获取速度和识别力，我们决定利用FPFH强大的识别力，
但是为了使构造的特征保持缩放不变性的性质同时，
还要区分不同的位姿，计算时需要考虑加入视点变量。
我们做了以下两种计算来构造特征，以应用于目标识别问题和位姿估计：
1.扩展FPFH，使其利用整个点云对象来进行计算估计（如2图所示），
在计算FPFH时以物体中心点与物体表面其他所有点之间的点对作为计算单元。
2.添加视点方向与每个点估计法线之间额外的统计信息，为了达到这个目的，
我们的关键想法是在FPFH计算中将视点方向变量直接融入到相对法线角计算当中。
因此新组合的特征被称为视点特征直方图（VFH）。
下图表体现的就是新特征的想法，包含了以下两部分：
1.一个视点方向相关的分量
2.一个包含扩展FPFH的描述表面形状的分量
对扩展的FPFH分量来说，默认的VFH的实现使用45个子区间进行统计，
而对于视点分量要使用128个子区间进行统计，这样VFH就由一共308个浮点数组成阵列。
在PCL中利用pcl::VFHSignature308的点类型来存储表示。P
FH/FPFH描述子和VFH之间的主要区别是：
对于一个已知的点云数据集，只一个单一的VFH描述子，
而合成的PFH/FPFH特征的数目和点云中的点数目相同。
头文件
#include <pcl/features/vfh.h>
#include <pcl/features/normal_3d.h>//法线特征
/*
视点特征直方图VFH(Viewpoint Feature Histogram)描述子，
它是一种新的特征表示形式，应用在点云聚类识别和六自由度位姿估计问题。
视点特征直方图（或VFH）是源于FPFH描述子.
由于它的获取速度和识别力，我们决定利用FPFH强大的识别力，
但是为了使构造的特征保持缩放不变性的性质同时，
还要区分不同的位姿，计算时需要考虑加入视点变量。
我们做了以下两种计算来构造特征，以应用于目标识别问题和位姿估计：
1.扩展FPFH，使其利用整个点云对象来进行计算估计（如2图所示），
在计算FPFH时以物体中心点与物体表面其他所有点之间的点对作为计算单元。
2.添加视点方向与每个点估计法线之间额外的统计信息，为了达到这个目的，
我们的关键想法是在FPFH计算中将视点方向变量直接融入到相对法线角计算当中。
通过统计视点方向与每个法线之间角度的直方图来计算视点相关的特征分量。
注意：并不是每条法线的视角，因为法线的视角在尺度变换下具有可变性，
我们指的是平移视点到查询点后的视点方向和每条法线间的角度。
第二组特征分量就是前面PFH中讲述的三个角度，如PFH小节所述，
只是现在测量的是在中心点的视点方向和每条表面法线之间的角度。
因此新组合的特征被称为视点特征直方图（VFH）。
下图表体现的就是新特征的想法，包含了以下两部分：
1.一个视点方向相关的分量
2.一个包含扩展FPFH的描述表面形状的分量
对扩展的FPFH分量来说，默认的VFH的实现使用45个子区间进行统计，
而对于视点分量要使用128个子区间进行统计，这样VFH就由一共308个浮点数组成阵列。
在PCL中利用pcl::VFHSignature308的点类型来存储表示。P
FH/FPFH描述子和VFH之间的主要区别是：
对于一个已知的点云数据集，只一个单一的VFH描述子，
而合成的PFH/FPFH特征的数目和点云中的点数目相同。
*/
#include <pcl/point_types.h>
//#include <pcl/features/pfh.h>
//#include <pcl/features/fpfh.h>
#include <pcl/features/vfh.h>
#include <pcl/io/pcd_io.h>//点云文件pcd 读写
#include <pcl/features/normal_3d.h>//法线特征
#include <pcl/visualization/histogram_visualizer.h> //直方图的可视化
#include <pcl/visualization/pcl_plotter.h>// 直方图的可视化 方法2
// 可视化 https://segmentfault.com/a/1190000006685118
//using namespace std;
using std::cout;
using std::endl;
int main(int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ>);
//======【1】 读取点云文件 填充点云对象======
pcl::PCDReader reader;
reader.read( "../../Filtering/table_scene_lms400.pcd", *cloud_ptr);
if(cloud_ptr==NULL) { cout << "pcd file read err" << endl; return -1;}
cout << "PointCLoud size() " << cloud_ptr->width * cloud_ptr->height
<< " data points ( " << pcl::getFieldsList (*cloud_ptr) << "." << endl;
// =====【2】计算法线========创建法线估计类====================================
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud_ptr);
/*
法线估计类NormalEstimation的实际计算调用程序内部执行以下操作：
对点云P中的每个点p
1.得到p点的最近邻元素
2.计算p点的表面法线n
3.检查n的方向是否一致指向视点，如果不是则翻转
在PCL内估计一点集对应的协方差矩阵，可以使用以下函数调用实现：
//定义每个表面小块的3x3协方差矩阵的存储对象
Eigen::Matrix3fcovariance_matrix;
//定义一个表面小块的质心坐标16-字节对齐存储对象
Eigen::Vector4fxyz_centroid;
//估计质心坐标
compute3DCentroid(cloud,xyz_centroid);
//计算3x3协方差矩阵
computeCovarianceMatrix(cloud,xyz_centroid,covariance_matrix);
*/
// 添加搜索算法 kdtree search 最近的几个点 估计平面 协方差矩阵PCA分解 求解法线
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
ne.setSearchMethod (tree);//设置近邻搜索算法
// 输出点云 带有法线描述
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ptr (new pcl::PointCloud<pcl::Normal>);
pcl::PointCloud<pcl::Normal>& cloud_normals = *cloud_normals_ptr;
// Use all neighbors in a sphere of radius 3cm
ne.setRadiusSearch (0.03);//半价内搜索临近点 3cm
// 计算表面法线特征
ne.compute (cloud_normals);
//=======【3】创建VFH估计对象vfh，并把输入数据集cloud和法线normal传递给它================
pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> vfh;
//pcl::PFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::PFHSignature125> pfh;// phf特征估计其器
//pcl::FPFHEstimation<pcl::PointXYZ,pcl::Normal,pcl::FPFHSignature33> fpfh;// fphf特征估计其器
// pcl::FPFHEstimationOMP<pcl::PointXYZ,pcl::Normal,pcl::FPFHSignature33> fpfh;//多核加速
vfh.setInputCloud (cloud_ptr);
vfh.setInputNormals (cloud_normals_ptr);
//如果点云是PointNormal类型，则执行vfh.setInputNormals (cloud);
//创建一个空的kd树对象，并把它传递给FPFH估计对象。
//基于已知的输入数据集，建立kdtree
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZ> ());
//pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree2 (new pcl::KdTreeFLANN<pcl::PointXYZ> ()); //-- older call for PCL 1.5-
vfh.setSearchMethod (tree2);//设置近邻搜索算法
//输出数据集
//pcl::PointCloud<pcl::PFHSignature125>::Ptr pfh_fe_ptr (new pcl::PointCloud<pcl::PFHSignature125> ());//phf特征
//pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh_fe_ptr (new pcl::PointCloud<pcl::FPFHSignature33>());//fphf特征
pcl::PointCloud<pcl::VFHSignature308>::Ptr vfh_fe_ptr (new pcl::PointCloud<pcl::VFHSignature308> ());//vhf特征
//使用半径在5厘米范围内的所有邻元素。
//注意：此处使用的半径必须要大于估计表面法线时使用的半径!!!
//fpfh.setRadiusSearch (0.05);
//计算pfh特征值
vfh.compute (*vfh_fe_ptr);
cout << "phf feature size : " << vfh_fe_ptr->points.size() << endl;
// 应该 等于 1
// ========直方图可视化=============================
//pcl::visualization::PCLHistogramVisualizer view;//直方图可视化
//view.setBackgroundColor(255,0,0);//背景红色
//view.addFeatureHistogram<pcl::VFHSignature308> (*vfh_fe_ptr,"vfh"，0);
//view.spinOnce(); //循环的次数
//view.spin(); //无限循环
pcl::visualization::PCLPlotter plotter;
plotter.addFeatureHistogram(*vfh_fe_ptr, 300); //设置的很坐标长度，该值越大，则显示的越细致
plotter.plot();
return 0;
}
【6】NARF 从深度图像(RangeImage)中提取NARF关键点 pcl::NarfKeypoint
然后计算narf特征 pcl::NarfDescriptor
边缘提取
直接把三维的点云投射成二维的图像不就好了。
这种投射方法叫做range_image.
#include <pcl/range_image/range_image.h>// RangeImage 深度图像
#include <pcl/keypoints/narf_keypoint.h>// narf关键点检测
#include <pcl/features/narf_descriptor.h>// narf特征
/*
NARF
从深度图像(RangeImage)中提取NARF关键点 pcl::NarfKeypoint
然后计算narf特征 pcl::NarfDescriptor
1. 边缘提取
对点云而言，场景的边缘代表前景物体和背景物体的分界线。
所以，点云的边缘又分为三种：
前景边缘，背景边缘，阴影边缘。
就是点a 和点b 如果在 rangImage 上是相邻的，然而在三维距离上却很远，那么多半这里就有边缘。
在提取关键点时，
边缘应该作为一个重要的参考依据。
但一定不是唯一的依据。
对于某个物体来说关键点应该是表达了某些特征的点，而不仅仅是边缘点。
所以在设计关键点提取算法时，需要考虑到以下一些因素：
边缘和曲面结构都要考虑进去；
关键点要能重复；
关键点最好落在比较稳定的区域，方便提取法线。
图像的Harris角点算子将图像的关键点定义为角点。
角点也就是物体边缘的交点，
harris算子利用角点在两个方向的灰度协方差矩阵响应都很大，来定义角点。
既然关键点在二维图像中已经被成功定义且使用了，
看来在三维点云中可以沿用二维图像的定义
不过今天要讲的是另外一种思路，简单粗暴，
直接把三维的点云投射成二维的图像不就好了。
这种投射方法叫做range_image.
*/
#include <iostream>//标准输入输出流
#include <boost/thread/thread.hpp>
#include <pcl/range_image/range_image.h>// RangeImage 深度图像
#include <pcl/io/pcd_io.h>//PCL的PCD格式文件的输入输出头文件
#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>// narf关键点检测
#include <pcl/features/narf_descriptor.h>// narf特征
#include <pcl/console/parse.h>//解析 命令行 参数
//定义别名
typedef pcl::PointXYZ PointType;
// --------------------
// -----参数 Parameters-----
// --------------------
//参数 全局变量
float angular_resolution = 0.5f;//角坐标分辨率
float support_size = 0.2f;//感兴趣点的尺寸（球面的直径）
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;//坐标框架：相机框架（而不是激光框架）
bool setUnseenToMaxRange = false;//是否将所有不可见的点 看作 最大距离
bool rotation_invariant = true;//
// --------------
// -----打印帮助信息 Help-----
// --------------
//当用户输入命令行参数-h，打印帮助信息
void
printUsage (const char* progName)
{
std::cout << "\n\n用法 Usage: "<<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"
<< "-o <0/1> switch rotational invariant version of the feature on/off"
<< "-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)
{
// --------------------------------------
// ----- 解析 命令行 参数 Parse Command Line Arguments-----
// --------------------------------------
if (pcl::console::find_argument (argc, argv, "-h") >= 0)//help参数
{
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";
}
if (pcl::console::parse (argc, argv, "-o", rotation_invariant) >= 0)
cout << "Switching rotation invariant feature version "<< (rotation_invariant ? "on" : "off")<<".\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);
// ------------------------------------------------------------------
// -----Read pcd file or create example point cloud if not given-----
// ------------------------------------------------------------------
//读取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 ());//仿射变换
//检查参数中是否有pcd格式文件名，返回参数向量中的索引号
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)//如果指定了pcd文件，读取pcd文件
{
std::cerr << "Was not able to open file \""<<filename<<"\".\n";
printUsage (argv[0]);
return 0;
}
//设置传感器的姿势
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)
std::cout << "Far ranges file \""<<far_ranges_filename<<"\" does not exists.\n";
}
else//没有指定pcd文件，生成点云，并填充它
{
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;
}
// -----------------------------------------------
// -----Create RangeImage from the PointCloud-----
// -----------------------------------------------
// ======从点云数据，创建深度图像=====================
// 直接把三维的点云投射成二维的图像
float noise_level = 0.0;
//noise level表示的是容差率，因为1°X1°的空间内很可能不止一个点，
//noise level = 0则表示去最近点的距离作为像素值，如果=0.05则表示在最近点及其后5cm范围内求个平均距离
//minRange表示深度最小值，如果=0则表示取1°X1°的空间内最远点，近的都忽略
float min_range = 0.0f;
//bordersieze表示图像周边点
int border_size = 1;
boost::shared_ptr<pcl::RangeImage> range_image_ptr (new pcl::RangeImage);//创建RangeImage对象（智能指针）
pcl::RangeImage& range_image = *range_image_ptr; //RangeImage的引用
//从点云创建深度图像
//rangeImage也是PCL的基本数据结构
//pcl::RangeImage rangeImage;
// 球坐标系
//角分辨率
//float angularResolution = (float) ( 1.0f * (M_PI/180.0f)); // 1.0 degree in radians 弧度
//phi可以取360°
// float maxAngleWidth = (float) (360.0f * (M_PI/180.0f)); // 360.0 degree in radians
//a取180°
// float maxAngleHeight = (float) (180.0f * (M_PI/180.0f)); // 180.0 degree in radians
//半圆扫一圈就是整个图像了
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 ();
// --------------------------------------------
// -----Open 3D viewer and add point cloud-----
// --------------------------------------------
// 3D点云显示
pcl::visualization::PCLVisualizer viewer ("3D Viewer");
viewer.setBackgroundColor (1, 1, 1);//背景颜色 白色
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_ptr, 0, 0, 0);
viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image");//添加点云
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");
//viewer.addCoordinateSystem (1.0f, "global");
//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 ());
// --------------------------
// -----Show range image-----
// --------------------------
//显示深度图像（平面图）
pcl::visualization::RangeImageVisualizer range_image_widget ("Range image");
range_image_widget.showRangeImage (range_image);
// --------------------------------
// -----Extract NARF keypoints-----
// --------------------------------
// ==================提取NARF关键点=======================
pcl::RangeImageBorderExtractor range_image_border_extractor;//创建深度图像的边界提取器，用于提取NARF关键点
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 = 0.5;
pcl::PointCloud<int> keypoint_indices;//用于存储关键点的索引 PointCloud<int>
narf_keypoint_detector.compute (keypoint_indices);//计算NARF关键
std::cout << "Found找到关键点： "<<keypoint_indices.points.size ()<<" key points.\n";
// ----------------------------------------------
// -----Show keypoints in range image widget-----
// ----------------------------------------------
//在range_image_widget中显示关键点
//for (size_t i=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);
// -------------------------------------
// -----Show keypoints in 3D viewer-----
// -------------------------------------
//在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 ();
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> keypoints_color_handler (keypoints_ptr, 255, 0, 0);//红色
viewer.addPointCloud<pcl::PointXYZ> (keypoints_ptr, keypoints_color_handler, "keypoints");//添加显示关键点
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");
//渲染属性，可视化工具，3维数据， 其中PCL_VISUALIZER_POINT_SIZE表示设置点的大小为7
// ------------------------------------------------------
//========================提取 NARF 特征 ====================
// ------------------------------------------------------
std::vector<int> keypoint_indices2;//用于存储关键点的索引 vector<int>
keypoint_indices2.resize (keypoint_indices.points.size ());
for (unsigned int i=0; i<keypoint_indices.size (); ++i) // This step is necessary to get the right vector type
keypoint_indices2[i] = keypoint_indices.points[i];//narf关键点 索引
pcl::NarfDescriptor narf_descriptor (&range_image, &keypoint_indices2);//narf特征描述子
narf_descriptor.getParameters().support_size = support_size;
narf_descriptor.getParameters().rotation_invariant = rotation_invariant;
pcl::PointCloud<pcl::Narf36> narf_descriptors;
narf_descriptor.compute (narf_descriptors);
cout << "Extracted "<<narf_descriptors.size ()<<" descriptors for "
<<keypoint_indices.points.size ()<< " keypoints.\n";
//--------------------
// -----Main loop-----
//--------------------
while (!viewer.wasStopped ())
{
range_image_widget.spinOnce (); // process GUI events 处理 GUI事件
viewer.spinOnce ();
pcl_sleep(0.01);
}
}
【7】RoPs特征(Rotational Projection Statistics) 描述子
0.在关键点出建立局部坐标系。
1.在一个给定的角度在当前坐标系下对关键点领域(局部表面) 进行旋转
2.把 局部表面 投影到 xy，yz，xz三个2维平面上
3.在每个投影平面上划分不同的盒子容器，把点分到不同的盒子里
4.根据落入每个盒子的数量，来计算每个投影面上的一系列数据分布
（熵值，低阶中心矩
5.M11,M12,M21,M22，E。E是信息熵。4*2+1=9）进行描述
计算值将会组成子特征。
盒子数量 × 旋转次数×9 得到特征维度
我们把上面这些步骤进行多次迭代。不同坐标轴的子特征将组成RoPS描述器
我们首先要找到目标模型:
points 包含点云
indices 点的下标
triangles包含了多边形
/*
RoPs特征(Rotational Projection Statistics) 描述子
0.在关键点出建立局部坐标系。
1.在一个给定的角度在当前坐标系下对关键点领域(局部表面) 进行旋转
2.把 局部表面 投影到 xy，yz，xz三个2维平面上
3.在每个投影平面上划分不同的盒子容器，把点分到不同的盒子里
4.根据落入每个盒子的数量，来计算每个投影面上的一系列数据分布
（熵值，低阶中心矩
5.M11,M12,M21,M22，E。E是信息熵。4*2+1=9）进行描述
计算值将会组成子特征。
盒子数量 × 旋转次数×9 得到特征维度
我们把上面这些步骤进行多次迭代。不同坐标轴的子特征将组成RoPS描述器
我们首先要找到目标模型:
points 包含点云
indices 点的下标
triangles包含了多边形
*/
#include <pcl/features/rops_estimation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_plotter.h>// 直方图的可视化 方法2
#include <pcl/visualization/cloud_viewer.h>//可是化
#include <boost/thread/thread.hpp>//boost::this_thread::sleep 多进程
int main (int argc, char** argv)
{
//======= points 包含点云===========
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ> ());
//========关键点索引文件===============
pcl::PointIndicesPtr indices = boost::shared_ptr <pcl::PointIndices> (new pcl::PointIndices ());//关键点 索引
std::ifstream indices_file;//文件输入留
//=====triangles包含了多边形===领域形状？？
std::vector <pcl::Vertices> triangles;
std::ifstream triangles_file;
if (argc != 4){
if (pcl::io::loadPCDFile ("../points.pcd", *cloud_ptr) == -1)
return (-1);
indices_file.open("../indices.txt", std::ifstream::in);
if(indices_file==NULL){
std::cout << "not found index.txt file"<<std::endl;
return (-1); }
triangles_file.open ("../triangles.txt", std::ifstream::in);
if(triangles_file==NULL){
std::cout << "not found triangles.txt file"<<std::endl;
return (-1); }
}
else if(argc == 4){
if (pcl::io::loadPCDFile (argv[1], *cloud_ptr) == -1) return (-1);
indices_file.open (argv[2], std::ifstream::in);
triangles_file.open (argv[3], std::ifstream::in);
}
//========关键点索引文件===============
for (std::string line; std::getline (indices_file, line);)//每一行为 一个点索引
{
std::istringstream in (line);
unsigned int index = 0;
in >> index;//索引
indices->indices.push_back (index - 1);
}
indices_file.close ();
//=====triangles包含了多边形===领域形状？？
for (std::string line; std::getline (triangles_file, line);)//每一行三个点索引
{
pcl::Vertices triangle;
std::istringstream in (line);
unsigned int vertex = 0;//索引
in >> vertex;
triangle.vertices.push_back (vertex - 1);
in >> vertex;
triangle.vertices.push_back (vertex - 1);
in >> vertex;
triangle.vertices.push_back (vertex - 1);
triangles.push_back (triangle);
}
float support_radius = 0.0285f;//局部表面裁剪支持的半径 (搜索半价)，
unsigned int number_of_partition_bins = 5;//以及用于组成分布矩阵的容器的数量
unsigned int number_of_rotations = 3;//和旋转的次数。最后的参数将影响描述器的长度。
//搜索方法
pcl::search::KdTree<pcl::PointXYZ>::Ptr search_method (new pcl::search::KdTree<pcl::PointXYZ>);
search_method->setInputCloud (cloud_ptr);
// rops 特征算法 对象 盒子数量 × 旋转次数×9 得到特征维度 3*5*9 =135
pcl::ROPSEstimation <pcl::PointXYZ, pcl::Histogram <135> > feature_estimator;
feature_estimator.setSearchMethod (search_method);//搜索算法
feature_estimator.setSearchSurface (cloud_ptr);//搜索平面
feature_estimator.setInputCloud (cloud_ptr);//输入点云
feature_estimator.setIndices (indices);//关键点索引
feature_estimator.setTriangles (triangles);//领域形状
feature_estimator.setRadiusSearch (support_radius);//搜索半径
feature_estimator.setNumberOfPartitionBins (number_of_partition_bins);//盒子数量
feature_estimator.setNumberOfRotations (number_of_rotations);//旋转次数
feature_estimator.setSupportRadius (support_radius);// 局部表面裁剪支持的半径
pcl::PointCloud<pcl::Histogram <135> >::Ptr histograms (new pcl::PointCloud <pcl::Histogram <135> > ());
feature_estimator.compute (*histograms);
// 可视化
pcl::visualization::PCLPlotter plotter;
plotter.addFeatureHistogram(*histograms, 300); //设置的很坐标长度，该值越大，则显示的越细致
plotter.plot();
// 可视化点云
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->setBackgroundColor (0, 0, 0);//背景黑色
viewer->addCoordinateSystem (1.0);//坐标系 尺寸
viewer->initCameraParameters ();//初始化相机参数
viewer->addPointCloud<pcl::PointXYZ> (cloud_ptr, "sample cloud");//输入点云可视化
while(!viewer->wasStopped())
{
viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
return (0);
}
【8】momentofinertiaestimation类获取基于惯性偏心矩 描述子。
这个类还允许提取云的轴对齐和定向包围盒。
但请记住，不可能的最小提取OBB包围盒。
首先计算点云的协方差矩阵，提取其特征值和向量。
你可以认为所得的特征向量是标准化的，
总是形成右手坐标系（主要特征向量表示x轴，而较小的向量表示z轴）。
在下一个步骤中，迭代过程发生。每次迭代时旋转主特征向量。
旋转顺序总是相同的，并且在其他特征向量周围执行，
这提供了点云旋转的不变性。
此后，我们将把这个旋转主向量作为当前轴。
然后在当前轴上计算转动惯量 和 将点云投影到以旋转向量为法线的平面上
计算偏心距
/*
moment_of_inertia.cpp
momentofinertiaestimation类获取基于惯性偏心矩 描述子。
这个类还允许提取云的轴对齐和定向包围盒。
但请记住，不可能的最小提取OBB包围盒。
首先计算点云的协方差矩阵，提取其特征值和向量。
你可以认为所得的特征向量是标准化的，
总是形成右手坐标系（主要特征向量表示x轴，而较小的向量表示z轴）。
在下一个步骤中，迭代过程发生。每次迭代时旋转主特征向量。
旋转顺序总是相同的，并且在其他特征向量周围执行，
这提供了点云旋转的不变性。
此后，我们将把这个旋转主向量作为当前轴。
然后在当前轴上计算转动惯量 和 将点云投影到以旋转向量为法线的平面上
计算偏心距
*/
#include <pcl/features/moment_of_inertia_estimation.h>
#include <vector>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <boost/thread/thread.hpp>
int main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZ> ());
if (argc != 2){
if (pcl::io::loadPCDFile ("../lamppost.pcd", *cloud_ptr) == -1)
return (-1);
}
else if (pcl::io::loadPCDFile (argv[1], *cloud_ptr) == -1)
return (-1);
pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;// 惯量 偏心距 特征提取
feature_extractor.setInputCloud (cloud_ptr);//输入点云
feature_extractor.compute ();//计算
std::vector <float> moment_of_inertia;// 惯量
std::vector <float> eccentricity;// 偏心距
pcl::PointXYZ min_point_AABB;
pcl::PointXYZ max_point_AABB;
pcl::PointXYZ min_point_OBB;
pcl::PointXYZ max_point_OBB;
pcl::PointXYZ position_OBB;
Eigen::Matrix3f rotational_matrix_OBB;
float major_value, middle_value, minor_value;
Eigen::Vector3f major_vector, middle_vector, minor_vector;
Eigen::Vector3f mass_center;
feature_extractor.getMomentOfInertia (moment_of_inertia);// 惯量
feature_extractor.getEccentricity (eccentricity);// 偏心距
// 以八个点的坐标 给出 包围盒
feature_extractor.getAABB (min_point_AABB, max_point_AABB);// AABB 包围盒坐标 八个点的坐标
// 以中心点 姿态 坐标轴范围 给出 包围盒
feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);// 位置和方向姿态
feature_extractor.getEigenValues (major_value, middle_value, minor_value);//特征值
feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);//特征向量
feature_extractor.getMassCenter (mass_center);//点云中心点
// 可视化
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->setBackgroundColor (0, 0, 0);//背景黑色
viewer->addCoordinateSystem (1.0);//坐标系 尺寸
viewer->initCameraParameters ();//初始化相机参数
viewer->addPointCloud<pcl::PointXYZ> (cloud_ptr, "sample cloud");//输入点云可视化
viewer->addCube (min_point_AABB.x, max_point_AABB.x, min_point_AABB.y, max_point_AABB.y, min_point_AABB.z, max_point_AABB.z, 1.0, 1.0, 0.0, "AABB");//包围盒
Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z);//位置
Eigen::Quaternionf quat (rotational_matrix_OBB);//方向姿态 四元数表示
viewer->addCube (position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, "OBB");
pcl::PointXYZ center (mass_center (0), mass_center (1), mass_center (2));
pcl::PointXYZ x_axis (major_vector (0) + mass_center (0), major_vector (1) + mass_center (1), major_vector (2) + mass_center (2));
pcl::PointXYZ y_axis (middle_vector (0) + mass_center (0), middle_vector (1) + mass_center (1), middle_vector (2) + mass_center (2));
pcl::PointXYZ z_axis (minor_vector (0) + mass_center (0), minor_vector (1) + mass_center (1), minor_vector (2) + mass_center (2));
viewer->addLine (center, x_axis, 1.0f, 0.0f, 0.0f, "major eigen vector");
viewer->addLine (center, y_axis, 0.0f, 1.0f, 0.0f, "middle eigen vector");
viewer->addLine (center, z_axis, 0.0f, 0.0f, 1.0f, "minor eigen vector");
Eigen::Vector3f p1 (min_point_OBB.x, min_point_OBB.y, min_point_OBB.z);
Eigen::Vector3f p2 (min_point_OBB.x, min_point_OBB.y, max_point_OBB.z);
Eigen::Vector3f p3 (max_point_OBB.x, min_point_OBB.y, max_point_OBB.z);
Eigen::Vector3f p4 (max_point_OBB.x, min_point_OBB.y, min_point_OBB.z);
Eigen::Vector3f p5 (min_point_OBB.x, max_point_OBB.y, min_point_OBB.z);
Eigen::Vector3f p6 (min_point_OBB.x, max_point_OBB.y, max_point_OBB.z);
Eigen::Vector3f p7 (max_point_OBB.x, max_point_OBB.y, max_point_OBB.z);
Eigen::Vector3f p8 (max_point_OBB.x, max_point_OBB.y, min_point_OBB.z);
p1 = rotational_matrix_OBB * p1 + position;
p2 = rotational_matrix_OBB * p2 + position;
p3 = rotational_matrix_OBB * p3 + position;
p4 = rotational_matrix_OBB * p4 + position;
p5 = rotational_matrix_OBB * p5 + position;
p6 = rotational_matrix_OBB * p6 + position;
p7 = rotational_matrix_OBB * p7 + position;
p8 = rotational_matrix_OBB * p8 + position;
pcl::PointXYZ pt1 (p1 (0), p1 (1), p1 (2));
pcl::PointXYZ pt2 (p2 (0), p2 (1), p2 (2));
pcl::PointXYZ pt3 (p3 (0), p3 (1), p3 (2));
pcl::PointXYZ pt4 (p4 (0), p4 (1), p4 (2));
pcl::PointXYZ pt5 (p5 (0), p5 (1), p5 (2));
pcl::PointXYZ pt6 (p6 (0), p6 (1), p6 (2));
pcl::PointXYZ pt7 (p7 (0), p7 (1), p7 (2));
pcl::PointXYZ pt8 (p8 (0), p8 (1), p8 (2));
viewer->addLine (pt1, pt2, 1.0, 0.0, 0.0, "1 edge");
viewer->addLine (pt1, pt4, 1.0, 0.0, 0.0, "2 edge");
viewer->addLine (pt1, pt5, 1.0, 0.0, 0.0, "3 edge");
viewer->addLine (pt5, pt6, 1.0, 0.0, 0.0, "4 edge");
viewer->addLine (pt5, pt8, 1.0, 0.0, 0.0, "5 edge");
viewer->addLine (pt2, pt6, 1.0, 0.0, 0.0, "6 edge");
viewer->addLine (pt6, pt7, 1.0, 0.0, 0.0, "7 edge");
viewer->addLine (pt7, pt8, 1.0, 0.0, 0.0, "8 edge");
viewer->addLine (pt2, pt3, 1.0, 0.0, 0.0, "9 edge");
viewer->addLine (pt4, pt8, 1.0, 0.0, 0.0, "10 edge");
viewer->addLine (pt3, pt4, 1.0, 0.0, 0.0, "11 edge");
viewer->addLine (pt3, pt7, 1.0, 0.0, 0.0, "12 edge");
while(!viewer->wasStopped())
{
viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
return (0);
}
【9】全局一致的空间分布描述子特征GASD
Globally Aligned Spatial Distribution (GASD) descriptors
可用于物体识别和姿态估计。
是对可以描述整个点云的参考帧的估计，
这是用来对准它的正则坐标系统
之后，根据其三维点在空间上的分布，计算出点云的描述符。
种描述符也可以扩展到整个点云的颜色分布。
匹配点云(icp)的全局对齐变换用于计算物体姿态。
使用主成分分析PCA来估计参考帧
三维点云P
计算其中心点位置P_
计算协方差矩阵 1/n × sum((pi - P_)*(pi - P_)转置)
奇异值分解得到 其 特征值eigen values 和特征向量eigen vectors
基于参考帧计算一个变换 [R t]
/*
pcl版本 >= 1.9才有
全局一致的空间分布描述子特征
Globally Aligned Spatial Distribution (GASD) descriptors
可用于物体识别和姿态估计。
是对可以描述整个点云的参考帧的估计，
这是用来对准它的正则坐标系统
之后，根据其三维点在空间上的分布，计算出点云的描述符。
种描述符也可以扩展到整个点云的颜色分布。
匹配点云(icp)的全局对齐变换用于计算物体姿态。
使用主成分分析PCA来估计参考帧
三维点云P
计算其中心点位置P_
计算协方差矩阵 1/n × sum((pi - P_)*(pi - P_)转置)
奇异值分解得到 其 特征值eigen values 和特征向量eigen vectors
基于参考帧计算一个变换 [R t]
*/
#include <iostream>
#include <pcl/point_types.h>
#include <pcl/features/gasd.h>
#include <pcl/io/pcd_io.h>//点云文件pcd 读写
#include <pcl/visualization/cloud_viewer.h>//点云可视化
#include <pcl/visualization/pcl_visualizer.h>// 高级可视化点云类
#include <pcl/visualization/pcl_plotter.h>// 直方图的可视化 方法2
#include <boost/thread/thread.hpp>
using namespace std;
// 别名
typedef pcl::PointCloud<pcl::PointXYZ> Cloud;
typedef pcl::PointXYZ PointType;
int
main (int argc, char** argv)
{
// 定义 点云对象 指针
Cloud::Ptr cloud_ptr (new Cloud);
// 读取点云文件 填充点云对象
pcl::PCDReader reader;
reader.read( "../../Filtering/table_scene_lms400.pcd", *cloud_ptr);
if(cloud_ptr==NULL) { cout << "pcd file read err" << endl; return -1;}
cout << "PointCLoud size() " << cloud_ptr->width * cloud_ptr->height
<< " data points ( " << pcl::getFieldsList (*cloud_ptr) << "." << endl;
// 创建 GASD 全局一致的空间分布描述子特征 传递 点云
// pcl::GASDColorEstimation<pcl::PointXYZRGBA, pcl::GASDSignature984> gasd;//包含颜色
pcl::GASDColorEstimation<pcl::PointXYZ, pcl::GASDSignature984> gasd;
gasd.setInputCloud (cloud_ptr);
// 输出描述子
pcl::PointCloud<pcl::GASDSignature984> descriptor;
// 计算描述子
gasd.compute (descriptor);
// 得到匹配 变换
Eigen::Matrix4f trans = gasd.getTransform();
// Unpack histogram bins
for (size_t i = 0; i < size_t( descriptor[0].descriptorSize ()); ++i)
{
descriptor[0].histogram[i];
}
// 可视化
// pcl::visualization::PCLPlotter plotter;
// plotter.addFeatureHistogram(descriptor[0].histogram, 300); //设置的很坐标长度，该值越大，则显示的越细致
// plotter.plot();
// 点云+法线 可视化
//pcl::visualization::PCLVisualizer viewer("pcd viewer");// 显示窗口的名字
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_ptr (new pcl::visualization::PCLVisualizer ("3D Viewer"));
//设置一个boost共享对象，并分配内存空间
viewer_ptr->setBackgroundColor(0.0, 0.0, 0.0);//背景黑色
//viewer.setBackgroundColor (1, 1, 1);//白色
viewer_ptr->addCoordinateSystem (1.0f, "global");//坐标系
// pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
//pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZ> rgb(cloud);
// pcl::visualization::PointCloudColorHandlerRandom<PointType> cloud_color_handler(cloud_ptr);
//该句的意思是：对输入的点云着色，Random表示的是随机上色，以上是其他两种渲染色彩的方式.
pcl::visualization::PointCloudColorHandlerCustom<PointType> cloud_color_handler (cloud_ptr, 255, 0, 0);//红色
//viewer->addPointCloud<pcl::PointXYZRGB>(cloud_ptr,cloud_color_handler,"sample cloud");//PointXYZRGB 类型点
viewer_ptr->addPointCloud<PointType>(cloud_ptr, cloud_color_handler, "original point cloud");//点云标签
viewer_ptr->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "original point cloud");
//渲染属性，可视化工具，3维数据， 其中PCL_VISUALIZER_POINT_SIZE表示设置点的大小为3
//viewer_ptr->addCoordinateSystem(1.0);//建立空间直角坐标系
//viewer_ptr->setCameraPosition(0,0,200); //设置坐标原点
viewer_ptr->initCameraParameters();//初始化相机参数
while (!viewer_ptr->wasStopped())
{
viewer_ptr->spinOnce ();
pcl_sleep(0.01);
}
return (0);
}