PCL只获取点云中一个点的法向量之computePointNormal

PCL只获取点云中一个点的法向量computePointNormal

最近用点云图做应用的时候想只获取点云中一个点的法向量，然后就在网络上搜索，搜索了半天只能找到一些看似成功，实则语焉不详的文章，甚至是纯照搬、抄袭的文章。所以写下这篇文章供有这个需要的人进行参考。

首先，关于点云图的法向量这个知识点我就不做重复说明，网上有很多人写的很好，大家可以去看一看。这里我重点讲pcl中computePointNormal这个函数的使用。

 computePointNormal (const pcl::PointCloud<PointInT> &cloud, const pcl::Indices &indices,
                          Eigen::Vector4f &plane_parameters, float &curvature)
 computePointNormal (const pcl::PointCloud<PointInT> &cloud, const pcl::Indices &indices,
                          float &nx, float &ny, float &nz, float &curvature)

这是该方法的函数参数说明：

• cloud是我们要输入的点云图，这里可以输入进你完整的点云图，函数在计算的时候只会计算你想要计算的那个点的法向量。
• indices是pcl::Indices类型，这个类型实际上你可以看成是std::vector< int >，里面包含的是你要计算的那个点的邻点，这些点可以用KNN进行计算获取到。
• plane_parameters是得到的结果的平面参数，包含4个变量，分别是一个三维空间中平面方程 A x + B y + C z + D = 0 Ax+By+Cz+D=0 Ax+By+Cz+D=0中的A、B、C、D。而法向量就是(A, B, C)
• curvature为求得的平面的曲率

第二个函数中，里面的nx, ny, nz和上面的A，B，C等同。

使用这个函数的最难点在于，如何获取一个能够满足函数要求的indices从而能使他成功运行。在文章点击这里ROS社区中指出，我们可以通过使用KNN，PCL中使用名为KdTree的类进行求取。

基本的函数流程：

1. 实例化一个NormalEstimation的求解器
2. 实例化一个PointCloud类的法向量
3. 设置我们要求取的点云中的一个点searchPoint
4. 使用KdTree从searchPoint中获取indices
5. 计算并显示

粘贴出代码：

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/cloud_viewer.h>
#include <boost/thread/thread.hpp>

int main (int argc, char** argv)
{
    //加载点云图
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    //这里的点云图设置为你自己的
    if (pcl::io::loadPCDFile<pcl::PointXYZ> ("../../Demo/data/biwi_face_database/model.pcd",
                                           *cloud) == -1) //* load the file
    {
        PCL_ERROR ("Couldn't read file pcd \n");
        return (-1);
    }
    std::cout << "Loaded "
            << cloud->width * cloud->height
            << " data points from pcd."
            << std::endl;

    /*求取法线->对输入的点云图像中的一个点进行法向量求解*/
    //法向量求解器
    pcl::NormalEstimation<pcl::PointXYZ, pcl::PointNormal> pointNormalEstimation;
    //法向量
    pcl::PointCloud<pcl::PointNormal>::Ptr pointNormal(new pcl::PointCloud<pcl::PointNormal>);
    //我们要求解的点,这个点的index你可以自己设置
    pcl::PointXYZ searchPoint = cloud->points[100];
    //KNN
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    //设置KdTree要求解的点云参数
    tree->setInputCloud(cloud);
    //这是K近邻的半径
    float radius = 0.03;
    //关键的数据indices
    std::vector<int> indices;
    //每个点到searchPoint的距离(暂时用不到)
    std::vector<float> distance;
    tree->radiusSearch(searchPoint, radius, indices, distance);

    //输出参数1>平面数据(可以转化为法向量)
    Eigen::Vector4f planeParams;
    //输出参数2>平面曲率
    float curvature;
    //进行单个点的法向量求解
    pointNormalEstimation.computePointNormal(*cloud, indices, planeParams, curvature);
    std::cout << planeParams << std::endl;  //输出平面的数据

    //创建视窗对象，定义标题栏名称“3D Viewer”
    boost::shared_ptr<pcl::visualization::PCLVisualizer>
            viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    //将点云添加到视窗对象中，并定义一个唯一的ID“original_cloud”
    viewer->addPointCloud<pcl::PointXYZ>(cloud, "Cloud");
    //点云附色，三个字段，每个字段范围0-1
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0.5, "Cloud");
    //构造该点对应的法向量
    pointNormal->push_back(pcl::PointNormal(
                                searchPoint._PointXYZ::x, searchPoint._PointXYZ::y, searchPoint._PointXYZ::z,
                                planeParams[0], planeParams[1], planeParams[2],
                                curvature)
                            );
    //将获取到的单个点的法向量添加到图中
    viewer->addPointCloudNormals<pcl::PointNormal>(pointNormal);

    //显示
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }

    return (0);
}

结果显示

至此暂且结束，有问题请在评论留言。