3D点云地图地面去除（3）：点云平面法向量估计

        在（1）中，利用RANSAC方法可以在室外提取部分地面特征点，这些特征点可以构成一个不完整的地面。计算出这个不完整地面的法向量，进一步求出 z 轴的变换矩阵 T ，以此可以将采集的点云数据的方向校正，使之成为与标准坐标轴平行的点云，方便后续的处理。激光雷达在采集数据时，其 z 轴与地面法向量不平行，而且这种情况随时都在发生。自动驾驶车辆或者机器人行驶过程中，路面随时都有小的颠簸，偶尔还会有较大颠簸，如通过城市道路中的减速带，转弯时速度过大等等情况。所以校准点云是很有必要的。

       点云平面法向量的计算涉及到PCL的一些库函数：

pcl::NormalEstimation< PointInT, PointOutT >：NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point.If PointOutT is specified as pcl::Normal, the normal is stored in the first 3 components (0-2), and the curvature is stored in component 3.

pcl::Normal：A point structure representing normal coordinates and the surface curvature estimate.

pcl::PointNormal：A point structure representing Euclidean xyz coordinates, together with normal coordinates and the surface curvature estimate.

 

具体的使用参考这里。另外也需要了解pcl的pcl::visualization::PCL::PCLVisualizer模块。代码如

/*************法向量计算****************/
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;//创建法线估计的对象
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);//创建法线的对象
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>); //建立kdtree来进行近邻点集搜索
tree->setInputCloud(cloud_filtered); //为kdtree添加点运数据
n.setInputCloud(cloud_filtered); //Provide a pointer to the input dataset.
n.setSearchMethod(tree); //Provide a pointer to the search object.
//点云法向计算时，需要所搜的近邻点大小
n.setKSearch(2000); //Set the number of k nearest neighbors to use for the feature estimation.
//开始进行法向计算
n.compute(*normals);

//将点云数据与法向信息拼接 ????
//pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>);
//pcl::concatenateFields (*cloud_filtered, *normals, *cloud_with_normals);

/*图形显示模块*/
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D viewer"));
//设置背景颜色
viewer->setBackgroundColor(0,0, 1);
//设置点云颜色，该处为单一颜色设置
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud_filtered, 0, 255, 0);
//添加需要显示的点云数据
viewer->addPointCloud<pcl::PointXYZ> (cloud_filtered, single_color, "sample cloud");
//设置点显示大小
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
//添加需要显示的点云法向。cloud为原始点云模型，normal为法向信息，10表示需要显示法向的点云间隔，即每10个点显示一次法向，５表示法向长度。
viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud_filtered, normals, 100, 2, "normals");
 

这个平面是在室外提取出来的不完整的地面点，白色的是其法向量。为了使用法向量求解其与z轴之间的变换T，还需要了解pcl::Normal，根据pcl::Normal的定义，当法向量是垂直向上的时候，points[i].normal_z的值是１,acos是０，为函数的最小值。根据acos函数的递减性质，非地面点的值应该都比地面点大。
其结构体定义如下： 

pcl::Normal在pcl官网中的定义:

/*brief A point structure representing normal coordinates and the surface curvature estimate. (SSE friendly)ingroup common*/

struct Normal : public _Normal
{
    inline Normal (const _Normal &p)
    {
     normal_x = p.normal_x; 
     normal_y = p.normal_y; 
     normal_z = p.normal_z;
     data_n[3] = 0.0f;
     curvature = p.curvature;
    }

    inline Normal ()
    {
     normal_x = normal_y = normal_z = data_n[3] = 0.0f;
     curvature = 0;
    }

    inline Normal (float n_x, float n_y, float n_z)
    {
     normal_x = n_x; normal_y = n_y; normal_z = n_z;
     curvature = 0;
     data_n[3] = 0.0f;
    }

    friend std::ostream& operator << (std::ostream& os, const Normal& p);
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
官网 tutorial中的资料:
union{
float data_n[4]
float normal[3];
struct
{
float normal_x;
float normal_y;
float normal_z;
};
};
union{
struct{
float curvature;
};
float data_c[4];
};