双边滤波算法

       今天再次查阅自己过去的博客，然而对具体的原理却不能一一道来，实属惭愧，发现写博客还是得像写科技文一样严谨，个人一直很推崇博客的原创性以及深度（毕竟网上千篇一律的东西太多太多），不过这得花费大量的时间以及精力，但是即使具备了这两个特质也不一定能引起读者的兴趣（可能读者的需求不再在这两点吧），当然把博客写成科技文的模式对于一个每天工作时间很长的人来说显然不现实。不过对于一些算法核心的原理我还是简单复述一下，希望能帮助大家快速了解其算法思想；

     其实双边滤波的作用就是用来解决点云厚度的问题（将点云压平变薄变光滑），这样的解释相信大家能理解吧。

     那对于远离主体点云的“噪点”怎么让他回到点云主体上呢？这就是双边滤波要解决的问题了。

     而且这又是一个涉及到局部邻域计算的问题，所以八叉树、kdtree的重要性不言而喻了（先前博客提供了源码，原理，比较详尽）；

       （1）基于其邻域估算法线，然后沿着该法向进行平移

           那么到底平移多少合适呢？

       （2）依据其局部邻域点进行加权计算；

           那怎么加权呢？单纯依据距离，构建一种线性加权关系？当然也可以，但是线性的处理方式对于复杂的曲面貌似不太高端，高斯函数对于这类问题貌似比较合理（距离越近的点对待处理点影响越大，也就是权重更高）

      （3）所以这个加权方式采用的就是高斯函数（也是重点）

        通过以上较为简洁的描述，我相信大家很快能了解其核心思想了吧，其实很多算法的原理通常都是采用一些较为简单常规的方法，解决问题的手段也通常是这样。越复杂的算法其可靠性个人感觉越差吧。

 

  

原始地形数据

 

                                                                                                                                                             双边滤波后的效果

 

         下面给大家上一段核心代码，稳定可用，当然这也是pcl库里剥离出来的，本可以做到完全不依赖于pcl库，不过其实意义也不大，反正一切工具皆可用，重点是掌握其核心原理即可。

  

bilateralFunc.h

//双边滤波算法
float sigma_s_ = 0.5;
float sigma_r_ = 0.5;
pcl::PointCloud<pcl::PointXYZ>::Ptr plcCloud1;
PointCloud<pcl::Normal>::Ptr cloud_normals;

double kernel(double x, double sigma)
{
    return (exp(-(x*x) / (2 * sigma*sigma)));
}

double computePointWeight(const int pid,const std::vector<int> &indices,const std::vector<float> &distances)
{
    double BF = 0, W = 0;

    // For each neighbor
    float up_v = 0;
    float down_v = 0;
    for (size_t n_id = 0; n_id < indices.size(); ++n_id)
    {
        int id = indices[n_id];
        // Compute the difference in intensity
        //double intensity_dist = fabs(input_->points[pid].intensity - input_->points[id].intensity);

        Eigen::Vector3f curPt(plcCloud1->points[pid].x, plcCloud1->points[pid].y, plcCloud1->points[pid].z);
        Eigen::Vector3f curNeiPt(plcCloud1->points[id].x, plcCloud1->points[id].y, plcCloud1->points[id].z);
        Eigen::Vector3f p_pi = curPt - curNeiPt;
        Eigen::Vector3f ptNormal(cloud_normals->points[pid].normal_x, 
            cloud_normals->points[pid].normal_y, cloud_normals->points[pid].normal_z);

        double dist = std::sqrt(distances[n_id]);//距离
        float paraS = abs(p_pi.dot(ptNormal));
        float para3 = p_pi.dot(ptNormal);

        up_v += kernel(dist, sigma_s_)* kernel(paraS, sigma_r_)*para3;
        down_v += kernel(dist, sigma_s_)* kernel(paraS, sigma_r_);
        //// Compute the Gaussian intensity weights both in Euclidean and in intensity space
        //double dist = std::sqrt(distances[n_id]);//距离
        //double weight = kernel(dist, sigma_s_) * kernel(intensity_dist, sigma_r_);

        //// Calculate the bilateral filter response
        //BF += weight * input_->points[id].intensity;
        //W += weight;
    }
    Eigen::Vector3f curPt1(plcCloud1->points[pid].x, plcCloud1->points[pid].y, plcCloud1->points[pid].z);
    Eigen::Vector3f ptNormal1(cloud_normals->points[pid].normal_x,
        cloud_normals->points[pid].normal_y, cloud_normals->points[pid].normal_z);

    Eigen::Vector3f curPt2 = curPt1 - (up_v / down_v)*ptNormal1;
    return (/*BF / W*/curPt2[2]);
}

 

void bilateralFunc()
{
    plcCloud1 = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    ifstream fin("181206_014447-09-54-34-876_export.txt", ios::in);
    if (!fin.is_open())
    {
        return;
    }

    while (!fin.eof())
    {
        float a, b, c;
        fin >> a >> b >> c;
        pcl::PointXYZ pt;
        pt.x = a;
        pt.y = b;
        pt.z = c;
        plcCloud1->points.push_back(pt);
    }

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_(new pcl::search::KdTree<pcl::PointXYZ>);
    tree_->setInputCloud(plcCloud1);//将点云塞入该树中


    //法线估计以及曲率计算
    cloud_normals = PointCloud<pcl::Normal>::Ptr(new PointCloud<pcl::Normal>);
    NormalEstimationOMP<pcl::PointXYZ, pcl::Normal>ne;
    ne.setNumberOfThreads(omp_get_max_threads());
    ne.setInputCloud(plcCloud1);
    ne.setSearchMethod(tree_);
    ne.setKSearch(50);
    ne.compute(*cloud_normals);

    // Check if sigma_s has been given by the user
    if (sigma_s_ == 0)
    {
        PCL_ERROR("[pcl::BilateralFilter::applyFilter] Need a sigma_s value given before continuing.\n");
        return;
    }

    std::vector<int> k_indices;
    std::vector<float> k_distances;

    // Copy the input data into the output

    // For all the indices given (equal to the entire cloud if none given)
    vector<float> zFilter;
    zFilter.resize(plcCloud1->points.size(), -9999);
    for (size_t i = 0; i < plcCloud1->points.size(); ++i)
    {
        // Perform a radius search to find the nearest neighbors
        tree_->radiusSearch(plcCloud1->points[i], sigma_s_ * 2, k_indices, k_distances);

        // Overwrite the intensity value with the computed average
        zFilter[i] = static_cast<float> (computePointWeight(i, k_indices, k_distances));
    }

    for (size_t i = 0; i < plcCloud1->points.size(); ++i)
    {
        plcCloud1->points[i].z = zFilter[i];
    }

    //输出结果
    ofstream outFile("双边滤波结果为" + to_string((long long)0) + ".txt", ios::out);
    for (size_t i = 0; i < plcCloud1->points.size(); ++i)
    {

        outFile << plcCloud1->points[i].x << " " << plcCloud1->points[i].y << " " << plcCloud1->points[i].z << endl;
    }
    outFile.close();

}