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