PCL库学习（13）_PCL中的法线估计对象与hdl-graph-slam中的法线滤波

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/visualization/pcl_visualizer.h>
int
main (int argc, char** argv)
{
        //加载点云模型
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
        if(pcl::io::loadPCDFile<pcl::PointXYZ> ("/media/sf_Linux_share/data-master/tutorials/table_scene_lms400.pcd", *cloud) == -1){
                
                PCL_ERROR("Could not read file \n");
        }
        //法向计算
        pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
        pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
        //构建kdtree来进行近邻点集搜索
        pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
        //为kdtree添加点云数据
        tree->setInputCloud (cloud);
        ne.setInputCloud (cloud);
        ne.setSearchMethod (tree);
        //点云法向计算时，需要搜索的近邻点大小
        ne.setKSearch (20);
        //开始进行法向计算
        ne.compute (*normals);
        //将点云数据与法向信息拼接
        pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>);
        pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
        
        /*显示*/
        //显示设置
        boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("Normal show"));
        //设置背景色
        viewer->setBackgroundColor (0, 0, 0.7);
        //设置点云颜色，该处为单一颜色设置
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 255, 0);
        //添加需要显示的点云数据
        viewer->addPointCloud<pcl::PointXYZ> (cloud, single_color, "sample cloud");
        //设置点显示大小
        viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 0.5, "sample cloud");
        //添加需要显示的点云法向。cloud为原始点云模型，normal为法向信息，10表示需要显示法向的点云间隔，即每500个点显示一次法向，0.2表示法线的长度。
        viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud, normals, 500, 0.2, "normals");
        while (!viewer->wasStopped ())
        {
                viewer->spinOnce (100);
                boost::this_thread::sleep (boost::posix_time::microseconds (100000));
        }
        return (0);
}

结果：
关于视点，默认坐标是（0，0，0）可使用如下函数替换：
setViewPoint (float vpx, float vpy, float vpz);
这里贴个VFH直方图的示图加深理解，而对于移动机器人，视点就是传感器的位置，hdl-graph-slam中是这样的：

ne.setViewPoint(0.0f, 0.0f, sensor_height);

hdl-graph-slam中对于地面点作法线滤波：

  /**
   * @brief filter points with non-vertical normals
   * @param cloud  input cloud
   * @return filtered cloud
   */
  pcl::PointCloud<PointT>::Ptr normal_filtering(const pcl::PointCloud<PointT>::Ptr& cloud) const 
  {
    pcl::NormalEstimation<PointT, pcl::Normal> ne;
    ne.setInputCloud(cloud);//输入点云

    pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
    ne.setSearchMethod(tree);//构建kdtree

    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    ne.setKSearch(10);//点云法向计算时，需要搜索的近邻点大小
    ne.setViewPoint(0.0f, 0.0f, sensor_height);//视点的位置
    ne.compute(*normals);

    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>);
    filtered->reserve(cloud->size());

    for (int i = 0; i < cloud->size(); i++) 
    {
    //这里其实是数量积公式a.b=|a||b|cosα，这里b为z轴平行的单位向量(0,0,1)，那么计算得到的dot就是每个点对应法向量的z值
      float dot = normals->at(i).getNormalVector3fMap().normalized().dot(Eigen::Vector3f::UnitZ());
      //normal_filter_thres为20度，即法线与Z轴法向量的夹角小于20度的点保留
      if (std::abs(dot) > std::cos(normal_filter_thresh * M_PI / 180.0)) 
      {
        filtered->push_back(cloud->at(i));
      }
    }
    filtered->width = filtered->size();
    filtered->height = 1;
    filtered->is_dense = false;
    return filtered;
  }