基于法线的点云双边滤波

pcl是根据强度值进行双边滤波，这里用法线计算。
双边滤波具有一定的保留边缘特征的功能，本质上是一种平滑算法（滤波前后点云数量不变），算法原理：
具体参考《The Bilateral Filter for Point Clouds》
作者：Julie Digne, Carlo de Franchis

代码：

#include 
#include
#include
#include
#include
#include
#include
using point = pcl::PointXYZ;
using normal = pcl::Normal;
using cloud = pcl::PointCloud<point>;
using cloudNormal = pcl::PointCloud<normal>;
int main()
{
    //三个参数
    const double dist_weigh =0.04;
    const double normal_weigh = 0.02;
    const int k = 40;
    cloud::Ptr cloud_in(new cloud), cloud_out(new cloud);
    std::string file_name("noisedchair.pcd");
    pcl::io::loadPCDFile<point>(file_name, *cloud_in);
    //kdtree
    pcl::search::KdTree<point>::Ptr tree(new pcl::search::KdTree<point>);
    tree->setInputCloud(cloud_in);
    //计算法线
    pcl::NormalEstimation<point, normal> ne;
    cloudNormal cloud_normals;
    ne.setInputCloud(cloud_in);
    ne.setSearchMethod(tree);
    ne.setKSearch(k);
    ne.compute(cloud_normals);

    for (size_t i = 0; i < cloud_in->size(); ++i)
    {
        std::vector<int> indices;
        std::vector<float>dist_square;
        indices.reserve(k);
        indices.reserve(k);
        //
        double zeta = 0.0;
        double sum = 0.0;
        if (tree->nearestKSearch(cloud_in->points[i], k, indices, dist_square) > 0)
        {
            for (size_t j = 1; j < indices.size(); ++j)
            {
                double diffx = cloud_in->points[indices[j]].x - cloud_in->points[i].x;
                double diffy = cloud_in->points[indices[j]].y - cloud_in->points[i].y;
                double diffz = cloud_in->points[indices[j]].z - cloud_in->points[i].z;
                double dn = cloud_normals.at(i).normal_x * diffx +
                    cloud_normals.at(i).normal_y * diffy +
                    cloud_normals.at(i).normal_z * diffz;
                double w = exp(-1*dn * dn / (2*normal_weigh*normal_weigh)) * exp(-1*dist_square[j] /(2*dist_weigh*dist_weigh));
                zeta += w * dn;
                sum += w;
            }

        }
        if (sum < 1e-10)
        {
            zeta = 0.0;
        }
        else
        {
            zeta /= sum;
        }
        point smoothed_point;
        smoothed_point.x = cloud_in->points[i].x + cloud_normals.at(i).normal_x * zeta;
        smoothed_point.y = cloud_in->points[i].y + cloud_normals.at(i).normal_y * zeta;
        smoothed_point.z = cloud_in->points[i].z + cloud_normals.at(i).normal_z * zeta;
        cloud_out->push_back(smoothed_point);
    }
    cloud_out->width = cloud_out->size();
    cloud_out->height = 1;
    cloud_out->resize(cloud_out->width * cloud_out->height);
    pcl::io::savePCDFile("smoothed" + file_name, *cloud_out);
    std::cout << "Hello World!\n";
}

效果：
原始

滤波后的效果：