PCL点云学习 十二(RoPs 特征与惯性矩描述子)

RoPs(Rotational Projection Statistics)

pcl官网地址
来自：
《pcl_点云从入门到精通》：详见这里
旋装投影特征

• 旋装平移不变性
• 抗干扰性良好

关键点

• 给定关键点$P$和其支撑半径内$r$内的点，得到$N$个三角形和$M$个顶点
• 对于第i个三角形见上图，三角形中任意一个顶点可以表示为:
  $$p_i(s,t)=p_{i1}+s(p_{i2}-p_{i1})+t(p_{i3}-p_{p_{i1}})(0\le s,t\le 1;s+t\le 1)$$
• 第i个三角形上的单点的散布矩阵$C_i$为：
  $$C_i=\frac{1}{12}\sum _{j=1}^3\sum _{k=1}^3(p_{ij}-p)(p_{ik}-p{)}^T+\frac{1}{12}\sum _{j=1}^3(p_{ij}-p)(p_{ij}-p{)}^T$$
  总散布矩阵为:
  $w_{i1}:$三角形面积与局部表面$S$总面积的比值
  $w_{i2}:$关键点到第i个三角形中心距离，形成的权值
  $$\begin{gathered} C=\sum _{i=1}^N{w}_{i1}{w}_{i2}{C}_i \\ {w}_{i1}=\frac{|(p_{i2}-p_{i1})\times (p_{i3}-p_{i1})|}{{\sum }_{i=1}^N|(p_{i2}-p_{i1})\times (p_{i3}-p_{i1})||} \\ {w}_{i2}=(r-|p-\frac{p_{i1}+p_{i2}+p_{i3}}{3}|{)}^2 \end{gathered}$$
• 在对特征矩阵进行分解
  $E:$特征值矩阵 降序排列
  $V:$特征向量 对应特征值
  $$CE=EV$$
• 特征向量方向：
  多数散布向量所指向的方向，可以根据特征向量与散布向量的内积得到一个模糊特征向量,其局部坐标为：
  $$\begin{array}{c}\sim \\ v_1\end{array}=v_1\ast sign(\sum _{i=1}^N{w}_{i1}{w}_{i2}(\frac{1}{6}\sum _{j=1}^3(p_{ij}-p)v_1))$$
  $$\begin{array}{c}\sim \\ v_3\end{array}=v_3\ast sign(\sum _{i=1}^N{w}_{i1}{w}_{i2}(\frac{1}{6}\sum _{j=1}^3(p_{ij}-p)v_3))$$
  $$v_2=v_1\times v_3$$

描述符

输入点云或者其mesh，计算点$p$及其支撑半径为$r$的局部表面，再进算其局部参考坐标系，再将局部表面$S$上的三角形顶点构成点云$Q$,映射到局部参考坐标系$Q^{\prime }$.
具体步骤：
$Q:$原始点云
$Q^{\prime }:$转换到局部坐标系的点云

1. 旋装点云$Q^{\prime }$绕x轴旋转${\theta }_k$,得到旋转后的点云$Q^{\prime }({\theta }_k)$,再将$Q^{\prime }({\theta }_k)$获得其的三视图$xy,xz,yz$及其对应投影点云$\widetilde{Q_i^{\prime }({\theta }_k)};i=1,2,3$
2. 对于每个$\widetilde{Q_i^{\prime }({\theta }_k)};i=1,2,3$，将其二位包围框均匀划分为$L\times L$个单元格，统计落入每个单元格点的个数，得到一个$L\times L$的矩阵$D$,将矩阵$D$归一化使所有单元格数量为1，使其对于分辨率具有不变性。
3. 提取分布矩阵$D$的中心矩和香农熵。
   中心矩公式$u_{mn}$:
   $$u_{mn}=\sum _{i=1}^L\sum _{j=1}^L(i-\overline{i}{)}^m(j-\overline{j}{)}^{n}D(i,j)$$
   香农熵为$e$:
   $$e=-\sum _{i=1}^L\sum _{j=1}^{L}D(i,j)log(D(i,j))$$
   在将三视图$xy,xz,yz$上的三个统计向量组合起来的得到$f_x({\theta }_k),k:表示绕x轴旋装k次$
4. 分别绕x轴旋主k次$k=1,..,T$，其角度分别为${\theta }_k$,得到其k次的关于3个平面的投影$f_x({\theta }_k),f_y({\theta }_k),f_z({\theta }_k)$,最终得到描述符:
   f = { f x ( θ k ) , f y ( θ k ) , f z ( θ k ) } f=\{f_x{(\theta_k)},f_y{(\theta_k)},f_z{(\theta_k)}\} f={fx​(θk​),fy​(θk​),fz​(θk​)}
5. 上述参数中$L（划分区域）=5$，$T（旋转次数）=3$，如果输入是点云需要将其三角化/采用其他算法的局部参考坐标模块

pcl中使用

其他文件详见pcl点云从入门到精通

#include <pcl/features/rops_estimation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/histogram_visualizer.h>
#include <pcl/visualization/pcl_plotter.h>
#include <iostream>
using namespace pcl;
using namespace std;
int main(int argc, char const *argv[])
{
    pcl::PointCloud<PointXYZ>::Ptr  cloud(new  pcl::PointCloud<PointXYZ>());//
    pcl::io::loadPCDFile("/home/n1/notes/pcl/RoPS/test.pcd",*cloud);
    pcl::PointIndicesPtr   indices =   boost::shared_ptr<PointIndices>(new pcl::PointIndices());
    std::ifstream   indices_file;
    std::cout<<cloud->size()<<endl;
    indices_file.open("/home/n1/notes/pcl/RoPS/indices.txt",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();

    std::vector<pcl::Vertices>  triangles;// std::vector<uint32_t>
    std::ifstream   triangles_file;
    triangles_file.open("/home/n1/notes/pcl/RoPS/triangles.txt",std::ifstream::in);//读取三角化信息
    for(std::string line;std::getline(triangles_file,line);)
    {
        pcl::Vertices   triangle;
        std::istringstream  in(line);
        unsigned    int vertex  =0;//为每个id添加索引
        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<PointXYZ>::Ptr  search_method(new  pcl::search::KdTree<PointXYZ>());
    search_method->setInputCloud(cloud);
    pcl::ROPSEstimation <pcl::PointXYZ,pcl::Histogram<135> > Feature_estimator;//估计特征点
    Feature_estimator.setSearchMethod(search_method);   //设置搜索方法
    Feature_estimator.setSearchSurface(cloud);          //设置搜索平面
    Feature_estimator.setInputCloud(cloud);             //设置输入点云
    Feature_estimator.setIndices(indices);              //对应的id索引
    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);
    std::cout<<histograms->header<<endl;
    pcl::visualization::PCLPlotter *plotter = new pcl::visualization::PCLPlotter ("test");
    plotter->setShowLegend(true);
    plotter->addFeatureHistogram<pcl::Histogram<135> >(*histograms,135,"rops");//点云,描述子长度，对应的id， 第0个点对应索引
    plotter->spin();
    return 0;
}

基于惯性矩和偏心率的描述子

官网链接
使用 pcl::MomentOfInertiaEstimation 类获取，惯性矩和偏心率描述子同时产生OBB（不一定为最小）和AABB边框

1. 面积矩：来自
   $S_Z:$平面图形对于z轴的面积距
   $A:$图形面积，位于$yoz$平面
   $yc:$面积中心到z轴的距离
   $S_Z=A\times yc(单位m^3)$

2. 惯性矩:

   • 物理量：用作描述物体抵抗扭动转动的能力，单位($m^4$)：地址
   • 详细定义：面积与与轴的距离（形心到坐标轴）的平方和的乘机:地址
   • $I_x:$平面图形对于z轴的面积矩,$I_y:$平面图形对于y轴的面积矩
     $I_x={\int }_A{y}^{2}dA,I_y={\int }_A{x}^{2}dA$

3. 偏心率（离心率）

   • 描述圆锥曲线轨道形状的数学量：详见

4. OBB（Oriented Bounding Box）方向包围盒，AaBB(Axis-Aligned Bounding Box)坐标对齐包围盒
   简介：地址1，地址2
   常用做作快速碰撞检测

5. PCL中的原理简介

   • 先计算点云的协防差矩阵，分解求解特征值和特征向量（同时归一化）
   • 建立坐标系：特征向量从大到小对应的特征向量为:$X,Y,Z$,同时符合右手定则
   • X轴绕其他Y,Z轴旋转，一般是绕y轴旋转$0-90$,绕z轴旋转$0-360$,pcl可以设置,没旋转一次，
     • 惯性矩：每旋转一次，以X作为当前轴计算一次
     • 偏心率：每旋转一次，以X轴旋转后的正方向，作为假设平面的法向量，将点云投影到该假设平面，长轴与短轴由投影后点云的协防差矩阵得到

6. pcl中代码(来自pcl点云入门到精通)

#include <pcl/features/moment_of_inertia_estimation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/point_cloud.h>
using namespace pcl;
using namespace std;


int main(int argc, char const *argv[])
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());

    pcl::io::loadPCDFile("/home/n1/notes/pcl/inertia/test.pcd",*cloud);

    pcl::MomentOfInertiaEstimation<pcl::PointXYZ> feature_extractor;//特征提取
    feature_extractor.setInputCloud(cloud); //输入点云
    feature_extractor.compute ();           //计算
    std::vector<float>  moment_of_inertia;//惯性值
    std::vector<float>  eccentricity;     //偏心率
    pcl::PointXYZ min_point_AABB;           //AABB左下角坐标
    pcl::PointXYZ max_point_AABB;           //AABB右上角坐标
    pcl::PointXYZ min_point_OBB;            //OBB最小角坐标
    pcl::PointXYZ max_point_OBB;            //OBB右上角坐标
    pcl::PointXYZ position_OBB;             //OBB 局部坐标中心点
    Eigen::Matrix3f rotational_matrix_OBB;  //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);   //获取对应边框
    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 ("MomentOfInertiaEstimation"));
    viewer->setBackgroundColor (1, 1, 1);
    viewer->addCoordinateSystem (1.0);
    viewer->initCameraParameters ();
    viewer->addPointCloud<pcl::PointXYZ> (cloud,pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloud,0,255,0), "sample cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,5,"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, 0.0, 0.0, "AABB");
    viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY,0.1,"AABB"); //不透明度
    viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH,4,"AABB");    //线宽

    Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z);
    std::cout<<"局部坐标中心点: "<<position_OBB<<endl;
    std::cout<<"质心: "<<mass_center<<endl;
    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");
    viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,0,0,1,"OBB");
    viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY,0.1,"OBB");
    viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH,4,"OBB");
    //viewer->setRepresentationToWireframeForAllActors(); //改变为线框表示

    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");
    std::cout<<"size of cloud :"<<cloud->points.size()<<endl;
    std::cout<<"moment_of_inertia :"<<moment_of_inertia.size()<<endl;
    std::cout<<"eccentricity :"<<eccentricity.size()<<endl;
    while(!viewer->wasStopped())
    {
        viewer->spinOnce (100);
        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }

    return 0;
}