双边滤波算法

 

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();

}