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个点显示一次法向,5表示法向长度。
viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud_filtered, normals, 100, 2, "normals");
 

这个平面是在室外提取出来的不完整的地面点,白色的是其法向量。为了使用法向量求解其与z轴之间的变换T,还需要了解pcl::Normal,根据pcl::Normal的定义,当法向量是垂直向上的时候,points[i].normal_z的值是1,acos是0,为函数的最小值。根据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];
};
 

 

  • 0
    点赞
  • 13
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值