PCL学习（3）——点云平滑及法线估计

参考：https://mp.weixin.qq.com/s/GFDWOudJ08In6jFyrZ7hhg

通过重采样实现点云平滑

需要平滑的情况：

• 用RGB-D激光扫描仪等设备扫描物体，尤其是比较小的物体时，往往会有测量误差。这些误差所造成的不规则数据如果直接拿来曲面重建的话，会使得重建的曲面不光滑或者有漏洞，而且这种不规则数据很难用前面我们提到过的统计分析等滤波方法消除；

• 后处理过程中，对同一个物体从不同方向进行了多次扫描，然后把扫描结果进行配准，最后得到一个完整的模型，但是你配准的结果不一定准，比如，同一面墙壁由于配准误差变成了“两面墙”，并不能完全重叠。

点云重采样，我们实际上是通过一种叫做“移动最小二乘”（MLS， Moving Least Squares ）法来实现的，对应的类名叫做：pcl::MovingLeastSquares(官网）

// 对点云重采样  
pcl::search::KdTree::Ptr treeSampling (new pcl::search::KdTree); // 创建用于最近邻搜索的KD-Tree
pcl::PointCloud mls_points;   //输出MLS
pcl::MovingLeastSquares mls;  // 定义最小二乘实现的对象mls
mls.setComputeNormals (false);  //设置在最小二乘计算中是否需要存储计算的法线
mls.setInputCloud (cloud_filtered);        //设置待处理点云
mls.setPolynomialOrder(2);             // 拟合2阶多项式拟合
mls.setPolynomialFit (false);  // 设置为false可以 加速 smooth
mls.setSearchMethod (treeSampling);    // 设置KD-Tree作为搜索方法
mls.setSearchRadius (0.05); // 单位m.设置用于拟合的K近邻半径
mls.process (mls_points);        //输出

Kd-Tree是一种数据结构，是空间二分树的一种特殊情况，可以很方便的用于进行范围搜索。在这里用KD-Tree就是为了便于管理、搜索点云，这种结构来可以很方便的找到最近邻点。

估计点云的表面法线

点云的法线计算一般有两种方法：

• 使用曲面重建方法，从点云数据中得到采样点对应的曲面，然后再用曲面模型计算其表面的法线
• 直接使用近似值直接从点云数据集推断出曲面法线

用第2种方法来近似估计点云中每个点的表面法线。

// 法线估计
pcl::NormalEstimation normalEstimation;                    //创建法线估计的对象
normalEstimation.setInputCloud(cloud_smoothed);                                    //输入点云
pcl::search::KdTree::Ptr tree(new pcl::search::KdTree);          // 创建用于最近邻搜索的KD-Tree
normalEstimation.setSearchMethod(tree);
pcl::PointCloud::Ptr normals (new pcl::PointCloud);     // 定义输出的点云法线
// K近邻确定方法，使用k个最近点，或者确定一个以r为半径的圆内的点集来确定都可以，两者选1即可
normalEstimation.setKSearch(10);                    // 使用当前点周围最近的10个点
//normalEstimation.setRadiusSearch(0.03);            //对于每一个点都用半径为3cm的近邻搜索方式
normalEstimation.compute(*normals);                 //计算法线

实践

/****************************
 * 给定一个融合后的点云，对其进行下采样和滤波。
 * 再进行平滑（输出结果），然后计算法线，并讲法线显示在平滑后的点云上。
****************************/

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

typedef pcl::PointXYZRGB PointT;

int main(int argc, char** argv)
{

	// Load input file
	pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
    pcl::PointCloud<PointT>::Ptr cloud_downSampled(new pcl::PointCloud<PointT>);
	pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>);
	pcl::PointCloud<PointT>::Ptr cloud_smoothed(new pcl::PointCloud<PointT>);
	if (pcl::io::loadPCDFile("/home/xiaohu/learn_SLAM/zuoye15/作业15-点云平滑及法线估计及显示/data/fusedCloud.pcd", *cloud) == -1)
    {
        cout << "点云数据读取失败！" << endl;
    }

    std::cout << "Orginal points number: " << cloud->points.size() << std::endl;

    // 下采样，同时保持点云形状特征
    pcl::VoxelGrid<PointT> downSampled;  //创建滤波对象
    downSampled.setInputCloud (cloud);            //设置需要过滤的点云给滤波对象
    downSampled.setLeafSize (0.01f, 0.01f, 0.01f);  //设置滤波时创建的体素体积为1cm的立方体
    downSampled.filter (*cloud_downSampled);           //执行滤波处理，存储输出

    pcl::io::savePCDFile ("/home/xiaohu/learn_SLAM/zuoye15/作业15-点云平滑及法线估计及显示/data/downsampledPC.pcd", *cloud_downSampled);

	// 统计滤波
	pcl::StatisticalOutlierRemoval<PointT> statisOutlierRemoval;       //创建滤波器对象
    statisOutlierRemoval.setInputCloud (cloud_downSampled);            //设置待滤波的点云
    statisOutlierRemoval.setMeanK (50);                                //设置在进行统计时考虑查询点临近点数
    statisOutlierRemoval.setStddevMulThresh (3.0);                     //设置判断是否为离群点的阀值:均值+1.0*标准差
    statisOutlierRemoval.filter (*cloud_filtered);                     //滤波结果存储到cloud_filtered

    pcl::io::savePCDFile ("/home/xiaohu/learn_SLAM/zuoye15/作业15-点云平滑及法线估计及显示/data/filteredPC.pcd", *cloud_filtered);
	// ----------------------开始你的代码--------------------------//
	// 请参考PCL官网实现以下功能
	// 对点云重采样
    pcl::search::KdTree<PointT>::Ptr treeSampling (new pcl::search::KdTree<PointT>);// 创建用于最近邻搜索的KD-Tree
    pcl::PointCloud<PointT> mls_point;    //输出MLS
    pcl::MovingLeastSquares<PointT,PointT> mls; // 定义最小二乘实现的对象mls
    mls.setComputeNormals(false);  //设置在最小二乘计算中是否需要存储计算的法线
    mls.setInputCloud(cloud_filtered);         //设置待处理点云
    mls.setPolynomialOrder(2);            // 拟合2阶多项式拟合
    mls.setPolynomialFit(false);     // 设置为false可以 加速 smooth
    mls.setSearchMethod(treeSampling);         // 设置KD-Tree作为搜索方法
    mls.setSearchRadius(0.05);           // 单位m.设置用于拟合的K近邻半径
    mls.process(mls_point);                 //输出

    // 输出重采样结果
    cloud_smoothed = mls_point.makeShared();
    std::cout<<"cloud_smoothed: "<<cloud_smoothed->size() <<std::endl;

    //save cloud_smoothed
    pcl::io::savePCDFileASCII("/home/xiaohu/learn_SLAM/zuoye15/作业15-点云平滑及法线估计及显示/data/cloud_smoothed.pcd",*cloud_smoothed);


    // 法线估计
    pcl::NormalEstimation<PointT,pcl::Normal> normalEstimation;             //创建法线估计的对象
    normalEstimation.setInputCloud(cloud_smoothed);                         //输入点云
    pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);// 创建用于最近邻搜索的KD-Tree
    normalEstimation.setSearchMethod(tree);
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>); // 定义输出的点云法线
    // K近邻确定方法，使用k个最近点，或者确定一个以r为半径的圆内的点集来确定都可以，两者选1即可
    normalEstimation.setKSearch(10);// 使用当前点周围最近的10个点
    //normalEstimation.setRadiusSearch(0.03);            //对于每一个点都用半径为3cm的近邻搜索方式
    normalEstimation.compute(*normals);               //计算法线

    std::cout<<"normals: "<<normals->size()<<", "<<"normals fields: "<<pcl::getFieldsList(*normals)<<std::endl;
    pcl::io::savePCDFileASCII("/home/xiaohu/learn_SLAM/zuoye15/作业15-点云平滑及法线估计及显示/data/normals.pcd",*normals);



	// 显示结果
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("PCL Viewer"));
    viewer->setBackgroundColor (0, 0, 0);
    pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud_smoothed);
    viewer->addPointCloud<PointT> (cloud_smoothed, rgb, "smooth cloud");
    viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "smooth cloud");
    viewer->addPointCloudNormals<PointT, pcl::Normal> (cloud_smoothed, normals, 20, 0.05, "normals");

    viewer->initCameraParameters ();

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

    return 1;
}

平滑后：

平滑及法线估计：