首页 > 编程语言 >双边滤波算法

双边滤波算法

时间:2023-12-06 19:44:38浏览次数:34  
标签:pid 滤波 算法 points plcCloud1 sigma id cloud 双边

 

 H:\CodeSet\vcg完善1\pclPrj\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();

}

 

标签:pid,滤波,算法,points,plcCloud1,sigma,id,cloud,双边
From: https://www.cnblogs.com/z-web-2017/p/17534987.html

相关文章

  • 【数据结构和算法】搜索算法
    ①搜索最小值python的min函数返回列表中的最小项1defindexOfMin(lyst):2minIndex=03currentIndex=14whilecurrentIndex<len(lyst):5iflyst[currentIndex]<lyst[minIndex]:6minIndex=currentIndex7currentI......
  • 【数据结构和算法】排序算法
    使用swap函数来交换列表中的两项的位置1defswap(lyst,i,j):2'''交换列表中两项的位置'''3temp=lyst[i]4lyst[i]=lyst[j]5lyst[j]=temp①选择排序处于列表第一项,先找到最小项的位置,如果该位置不是列表的第一项,算法会交换这两个位置的项,然后......
  • 路径规划算法 - 求解最短路径 - Dijkstra算法
    Dijkstra算法的思想是广度优先搜索(BFS)贪心策略。是从一个顶点到其余各顶点的最短路径算法,节点边是不各自不同的权重,但都必须是正数如果是负数,则需要Bellman-Ford算法如果想求任意两点之间的距离,就需要用Floyd算法求节点0->4的最短路径每次从未标记的节点中选择距离......
  • 拒绝算法推荐,使用rss订阅消息与新闻!
    算法推荐的弊端就不说了借用RSSHub镜像网站如果你实在不会,又或者觉得麻烦,那你还可以搭其他网友的“便车”。我收集了 9 个公开的 RSShub镜像网站,它们用的都是用自己的服务器,所以在流量方面也不会有问题。服务器1 :https://rsshub.rssforever.com 服务器2 :https://rss......
  • java与算法Day1 Scanner的应用(一)
    java中使用输入需要用到java.util.Scanner。Scanner有next,nextInt,nextString,hasNext,hasNextLine等方法。使用XXX variable=Scanner.NextXXX就可以获取一个输入值。next系列的方法,他们的作用都是从键盘中接收数据。当程序执行到他们的时候,在命令行中就开始等待键盘输入了,而......
  • 文心一言 VS 讯飞星火 VS chatgpt (130)-- 算法导论11.2 2题
    二、用go语言,对于一个用链接法解决冲突的散列表,说明将关键字5,28,19,15,20,33,12,17,10插入到该表中的过程。设该表中有9个槽位,并设其散列函数为h(k)=kmod9。文心一言:在这个问题中,我们使用Go语言来解决一个散列(Hash)表的问题,该问题需要我们使用链接法解决冲突。我们的散列......
  • 文心一言 VS 讯飞星火 VS chatgpt (129)-- 算法导论11.1 4题
    四、用go语言,我们希望在一个非常大的数组上,通过利用直接寻址的方式来实现一个字典。开始时该数组中可能包含一些无用信息,但要对整个数组进行初始化是不太实际的,因为该数组的规模太大。请给出在大数组上实现直接寻址字典的方案。每个存储对象占用O(1)空间;SEARCH、INSERT和DELETE操......
  • Xlinx FPGA for DSP CORDIC 算法
    https://wenku.baidu.com/view/6c623aa8910ef12d2bf9e732.html?_wkts_=1701401816748&needWelcomeRecommand=1           ......
  • 单调栈与单调队列算法总结
    单调栈知识概览单调栈最常见的应用是找到每一个数离它最近的且比它小的数。单调栈考虑的方式和双指针类似,都是先想一下暴力做法是什么,然后再挖掘一些性质如单调性,最终可以把目光集中在比较少的状态中,从而达到降低时间复杂度的作用,都是算法优化的一种手段。对于的情况,更有可能......
  • 代码随想录算法训练营第六天| 454.四数相加 15.三数之和 18.四数之和
    LeetCode454.四数相加题目链接:LeetCode454思路: 将两个数组中的数存放到一个map中,用另外两个数组的值在map中去减 classSolution{public:intfourSumCount(vector<int>&A,vector<int>&B,vector<int>&C,vector<int>&D){unordered_map&l......