pcl里面的法线估计

法线估计是一个很重要的特征，常常在被使用在很多计算机视觉的应用里面，比如可以用来推出光源的位置，通过阴影与其他视觉影响。

给一个几何表面，去推断给定点的法线方向，即垂直向量的方向往往是不容易的。然而，在我们获取物体表面的点云数据后，有两大选择:

1.从已获取的点云数据集中得到潜在表面，并用表面网格化技术，来计算网格的表面法线。

2.使用近似值来推断点云数据集的表面法线。

尽管有很多法线估计的方法存在，但是我们这次将会讲的是最简单的方法。表面法线的问题可以近似化解为切面的问题，这个切面的问题又会变成最小二乘法拟合平面的问题。

解决表面法线估计的问题可以最终化简为对一个协方差矩阵的特征向量和特征值的分析(或者也叫PCA-Principal Component Analysis 主成分分析)，这个协方差矩阵是由查询点的最近邻产生的。对于每个点Pi,我们假设协方差矩阵C如下:

这里K指的是离点的最近的K个点，是最近邻的中心，是第j个特征值，是第j个特征向量。

下面是一段用来估计协方差矩阵的代码

// Placeholder for the 3x3 covariance matrix at each surface patchEigen::Matrix3f covariance_matrix;// 16-bytes aligned placeholder for the XYZ centroid of a surface patchEigen::Vector4f xyz_centroid;// Estimate the XYZ centroidcompute3DCentroid (cloud, xyz_centroid);// Compute the 3x3 covariance matrixcomputeCovarianceMatrix (cloud, xyz_centroid, covariance_matrix);

总的来说，因为数学上没有方法解决法线的符号，比如一个球面，法线可以指向球心，也可以背向球心。下面的两幅图像是描述厨房的点云图，右边的图像是通过高斯扩展图(EGI Extended Gaussian Image)，也常常叫做法线球。法线球是一个描述点云里面所有法线方向的一种方式。因为数据集是2.5D的，何为2.5D，你可以把上下，左右，前后看成一个D，然后现实生活里面往往不可能每个方向都兼顾，比如摄像机只能拍到前面的，所有是1(上下)+1(左右)+0.5(前)叫2.5D，当然这是建立在摄像机为单一视角的情况下，即摄像机不会动，一直是固定的。所以理论上，EGI图，即高斯球也应该是2.5D的，因为你摄像机是向前拍摄的，所以物体的法线也是向前的，然而因为这个算法的原因。主成分分析这个算法，不能解决法线的符号，所以导致了法线指向可能往前，也可能往后，导致整个球里面各个方向都可能存在着法线。

解决上面的法线方向不定的问题，我们得知道视角的向量，这就很简单了，只要法线和视角与点的连线，这两条线的夹角是锐角，即这两个向量的点积大于0即可。

我们经过上述的处理后，图片就变成了这样

可以看到左边的那副图，法线的指向全都变成一个方向了，同时高斯扩展图只有前半个球面是有法线的，即我们的方法是有效的。

我们可以使用下面的方法去改变法线的方向

flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z, Eigen::Vector4f &normal);

上面的这个方法就像我们刚才说的，只适用于单一视角的情况下。

选择正确的比例

就像前面说的，预测一个表面法线需要最近邻的方法，那么如何设置最近邻所需要的半径与点的个数呢？

这个问题是非常重要的，是对点的特征自动化评估的重要因素。为了更好的阐述这个问题，下面的图将显示选择一个小比例和大比例的影响。左边的图是比例(r和k)比较小的情况，我们发现它的法线是另人满意的，而右边就不尽人意了，你看那个桌角的位置，有一个法线出轨了，是一个小三，不属于上一个表面也不属于侧面，这就是比例选择太大的弊端，所以小比例往往更注重细节，更适合描述比较复杂的物体。

我们得根据不同的细节来选取比例，简单的说，如果边缘的曲率在杯子的把柄和圆柱体之间的时候，比例需要比较小来获取足够的细节。

下面是官方的一段代码段:

#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>{pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);... read, pass in or create a point cloud ...// Create the normal estimation class, and pass the input dataset to itpcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;ne.setInputCloud (cloud);// Create an empty kdtree representation, and pass it to the normal estimation object.// Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());ne.setSearchMethod (tree);// Output datasetspcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);// Use all neighbors in a sphere of radius 3cmne.setRadiusSearch (0.03);// Compute the featuresne.compute (*cloud_normals);// cloud_normals->points.size () should have the same size as the input cloud->points.size ()*
}

NormalEstimation这个类

做了以下3件事

1.得到p的最近邻

2.计算p的表面法线n

3.检查法线的朝向，然后拨乱反正。

默认的视角是(0,0,0)，可以通过下面的方法来更改

setViewPoint (float vpx, float vpy, float vpz);

计算一个点的法线

computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices, Eigen::Vector4f &plane_parameters, float &curvature);

前面两个参数很好理解，plane_parameters包含了4个参数，前面三个是法线的(nx,ny,nz)坐标，加上一个 nc . p_plane (centroid here) + p的坐标，然后最后一个参数是曲率。

接下去是我写的一个代码，先通过从磁盘里面加载一个PCD文件，然后进行降低采样和滤波等操作，最后通过PCLVisulizer显示出来.

#include <iostream>
#include <pcl/visualization/cloud_viewer.h>
#include<pcl/io/pcd_io.h>
#include<pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/features/normal_3d.h>
#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/filters/statistical_outlier_removal.h>int main ()
{pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_old (new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_downsampled (new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);pcl::io::loadPCDFile ("test_pcd.pcd", *cloud_old);//Use a voxelSampler to downsamplepcl::VoxelGrid<pcl::PointXYZ> voxelSampler;voxelSampler.setInputCloud(cloud_old);voxelSampler.setLeafSize(0.01f, 0.01f, 0.01f);voxelSampler.filter(*cloud_downsampled);//Use a filter to reduce noisepcl::StatisticalOutlierRemoval<pcl::PointXYZ> statFilter;statFilter.setInputCloud(cloud_downsampled);statFilter.setMeanK(10);statFilter.setStddevMulThresh(0.2);statFilter.filter(*cloud);// Create the normal estimation class, and pass the input dataset to itpcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;ne.setInputCloud (cloud);// Create an empty kdtree representation, and pass it to the normal estimation object.// Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());ne.setSearchMethod (tree);// Output datasetspcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);// Use all neighbors in a sphere of radius 1cmne.setRadiusSearch(0.01);// Compute the featuresne.compute (*normals);boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));viewer->setBackgroundColor (0, 0, 0);viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud, normals, 10, 0.2, "normals");viewer->addCoordinateSystem (1.0);viewer->initCameraParameters ();while (!viewer->wasStopped()){viewer->spinOnce (100);boost::this_thread::sleep (boost::posix_time::microseconds (100000));}return 0;
}

因为我的PCD的点云文件里面的点是PointXYZ类型，所以显示得时候，都是白色的，下面上一张效果图。