点云统计滤波（基于中值距离）

‌效果对比:// ---------- 2. 参数设置 ---------- const float radius 0.1f; // 搜索半径米决定每个点的邻域大小。 // 调试技巧 // - 增大 radius 可在稀疏点云中找到更多邻居但会增加计算量并可能把不同结构的点混在一起。 // - 减小 radius 可提高局部精度适用于高密度点云或希望滤除远离点但会使某些边界点被视为孤立点。 // 建议范围0.05 ~ 1.0根据点云分辨率调整。 const int min_neighbors 100; // 最少邻居数执行统计/中位数计算所需的邻居数量阈值。 // 调试技巧 // - 增大 min_neighbors 会让稀疏点更容易被跳过有助于移除孤立噪声但可能丢失真实的稀疏结构点。 // - 减小 min_neighbors 会保留更多点但可能使噪声对统计结果产生更大影响。 // 建议范围10 ~ 200随点云密度增减。 const float multiplier 0.1f; // 阈值倍率用于判断点是否保留的阈值为 global_median * multiplier。 // 调试技巧 // - 减小 multiplier如 0.3~0.5会更严格地筛选点移除更多离群点/边缘噪声。 // - 增大 multiplier1.0会更宽松地保留点适用于需要保全更多细节的场景。 // 建议范围0.3 ~ 2.0。根据可视化结果反复微调。算法步骤‌对每个点搜索半径radius内的邻居点计算每个点的邻域距离中位数计算所有点中位数的全局中位数以global_median * multiplier为阈值筛选离群点#include iostream #include vector #include numeric #include algorithm #include cmath #include sstream #include pcl/io/ply_io.h #include pcl/io/pcd_io.h #include pcl/point_types.h #include pcl/search/kdtree.h // KD-Tree用于邻域搜索 #include pcl/features/normal_3d.h // 法线估计头文件 #include pcl/ModelCoefficients.h // 模型系数头文件 #include pcl/sample_consensus/ransac.h // RANSAC #include pcl/sample_consensus/method_types.h // 随机采样一致性方法 #include pcl/sample_consensus/model_types.h // 模型类型定义 #include pcl/segmentation/sac_segmentation.h // RANSAC 分割头文件 #include pcl/sample_consensus/sac_model_cylinder.h// 圆柱模型定义 #include pcl/filters/extract_indices.h #include pcl/console/print.h // 日志输出 #include stdexcept // 异常处理 #include boost/thread/thread.hpp #include boost/date_time/posix_time/posix_time.hpp #include pcl/visualization/pcl_visualizer.h // 可视化 #include pcl/common/common.h #include chrono #include thread #include pcl/io/ply_io.h #include pcl/filters/extract_indices.h std::string current_time() { auto now std::chrono::system_clock::now(); auto in_time_t std::chrono::system_clock::to_time_t(now); std::stringstream ss; ss std::put_time(std::localtime(in_time_t), %Y-%m-%d %H:%M:%S); return ss.str(); } int main(int argc, char** argv) { // ---------- 1. 读取点云 ---------- std::cout [ current_time() ] start load cloud file: inside.ply std::endl; pcl::PointCloudpcl::PointXYZ::Ptr cloud(new pcl::PointCloudpcl::PointXYZ); if (pcl::io::loadPLYFilepcl::PointXYZ(inside.ply, *cloud) 0) { std::cerr [ current_time() ] ERROR: Failed to load point cloud file inside.ply. std::endl; return -1; } std::cout [ current_time() ] success load cloudPoint, Size: cloud-size() std::endl; if (cloud-empty()) { std::cerr [ current_time() ] ERROR: Point cloud is empty. std::endl; return -1; } // ---------- 2. 参数设置 ---------- const float radius 0.1f; // 搜索半径米决定每个点的邻域大小。 // 调试技巧 // - 增大 radius 可在稀疏点云中找到更多邻居但会增加计算量并可能把不同结构的点混在一起。 // - 减小 radius 可提高局部精度适用于高密度点云或希望滤除远离点但会使某些边界点被视为孤立点。 // 建议范围0.05 ~ 1.0根据点云分辨率调整。 const int min_neighbors 100; // 最少邻居数执行统计/中位数计算所需的邻居数量阈值。 // 调试技巧 // - 增大 min_neighbors 会让稀疏点更容易被跳过有助于移除孤立噪声但可能丢失真实的稀疏结构点。 // - 减小 min_neighbors 会保留更多点但可能使噪声对统计结果产生更大影响。 // 建议范围10 ~ 200随点云密度增减。 const float multiplier 0.1f; // 阈值倍率用于判断点是否保留的阈值为 global_median * multiplier。 // 调试技巧 // - 减小 multiplier如 0.3~0.5会更严格地筛选点移除更多离群点/边缘噪声。 // - 增大 multiplier1.0会更宽松地保留点适用于需要保全更多细节的场景。 // 建议范围0.3 ~ 2.0。根据可视化结果反复微调。 // ---------- 3. 构建 KD-Tree ---------- // 在范围查询如“半径为5米内的所有点”或光线追踪中划分空间可以快速跳过无用的区域 std::cout [ current_time() ] Building KD-Tree... std::endl; pcl::KdTreeFLANNpcl::PointXYZ kdtree; kdtree.setInputCloud(cloud); // ---------- 4. Compute median neighborhood distance ---------- std::cout [ current_time() ] Start computing median neighborhood distance... std::endl; std::vectorint nn_indices; std::vectorfloat nn_distances; std::vectorfloat median_distances; median_distances.reserve(cloud-size()); size_t skipped_points 0; for (size_t i 0; i cloud-size(); i) { if (!std::isfinite(cloud-at(i).x)) { median_distances.push_back(0.0f); skipped_points; continue; } //--------------对每个点搜索半径radius内的邻居点---------------- // nn_indices存储‌邻居点的索引‌列表 // nn_distances存储‌邻居点到目标点的距离‌列表 kdtree.radiusSearch(i, radius, nn_indices, nn_distances); if (nn_indices.size() static_castsize_t(min_neighbors)) { median_distances.push_back(0.0f); skipped_points; continue; } std::sort(nn_distances.begin(), nn_distances.end()); float median_dist nn_distances[nn_indices.size() / 2]; median_distances.push_back(median_dist); } std::cout [ current_time() ] Median distance computed, valid points: cloud-size() - skipped_points , skipped points: skipped_points std::endl; // ---------- 5. 计算全局中心距离 ---------- std::vectorfloat sorted_medians median_distances; std::sort(sorted_medians.begin(), sorted_medians.end()); float global_median sorted_medians[sorted_medians.size() / 2]; std::cout [ current_time() ] Global median distance: global_median std::endl; // ---------- 6. Select inliers ---------- pcl::PointIndices::Ptr inliers(new pcl::PointIndices); for (size_t i 0; i cloud-size(); i) { if (median_distances[i] global_median * multiplier) { //inliers-indices.push_back(i); } else { inliers-indices.push_back(i); } } std::cout [ current_time() ] Inlier count: inliers-indices.size() std::endl; // ---------- 7. Extract filtered result ---------- pcl::ExtractIndicespcl::PointXYZ extract; extract.setInputCloud(cloud); extract.setIndices(inliers); pcl::PointCloudpcl::PointXYZ::Ptr filtered_cloud(new pcl::PointCloudpcl::PointXYZ); extract.filter(*filtered_cloud); std::cout [ current_time() ] Filtering finished, original points: cloud-size() , filtered points: filtered_cloud-size() std::endl; // 8. Left-right split visualization std::cout [ current_time() ] Creating left-right comparison window... std::endl; // Print point-cloud bounds. auto print_bounds [](const pcl::PointCloudpcl::PointXYZ::Ptr pc, const char* name) { if (!pc) { std::cout name nullptr\n; return; } std::cout [ current_time() ] name size pc-size(); if (pc-empty()) { std::cout (empty)\n; return; } Eigen::Vector4f minPt, maxPt; pcl::getMinMax3D(*pc, minPt, maxPt); std::cout min( minPt[0] , minPt[1] , minPt[2] ) max( maxPt[0] , maxPt[1] , maxPt[2] )\n; }; print_bounds(cloud, original_cloud); print_bounds(filtered_cloud, filtered_cloud); // Create the visualizer window. boost::shared_ptrpcl::visualization::PCLVisualizer viewer(new pcl::visualization::PCLVisualizer(Point Cloud Filter Comparison - Left: Original | Right: Filtered)); viewer-initCameraParameters(); // Create left and right viewports. int v1, v2; viewer-createViewPort(0.0, 0.0, 0.5, 1.0, v1); // Left viewport viewer-createViewPort(0.5, 0.0, 1.0, 1.0, v2); // Right viewport // Use a dark background for better contrast. viewer-setBackgroundColor(0.05, 0.05, 0.05, v1); viewer-setBackgroundColor(0.05, 0.05, 0.05, v2); // Add a coordinate system. viewer-addCoordinateSystem(0.5); // Configure point-cloud colors. pcl::visualization::PointCloudColorHandlerCustompcl::PointXYZ original_h(cloud, 0, 200, 200); // Cyan pcl::visualization::PointCloudColorHandlerCustompcl::PointXYZ filtered_h(filtered_cloud, 255, 150, 0); // Orange // Add the point clouds to the viewports. if (cloud !cloud-empty()) { viewer-addPointCloud(cloud, original_h, original_cloud, v1); viewer-setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, original_cloud); viewer-addText(Original Cloud, 10, 10, v1_text, v1); } if (filtered_cloud !filtered_cloud-empty()) { viewer-addPointCloud(filtered_cloud, filtered_h, filtered_cloud, v2); viewer-setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, filtered_cloud); viewer-addText(Filtered Cloud, 10, 10, v2_text, v2); } // Reset the camera for a reasonable initial view. viewer-resetCamera(); // Enter the render loop. while (!viewer-wasStopped()) { viewer-spinOnce(50); boost::this_thread::sleep(boost::posix_time::milliseconds(10)); } std::cout [ current_time() ] Comparison window closed. Program finished. std::endl; return 0; }多线程优化#include iostream #include vector #include numeric #include algorithm #include cmath #include sstream #include pcl/io/ply_io.h #include pcl/io/pcd_io.h #include pcl/point_types.h #include pcl/search/kdtree.h // KD-Tree用于邻域搜索 #include pcl/features/normal_3d.h // 法线估计头文件 #include pcl/ModelCoefficients.h // 模型系数头文件 #include pcl/sample_consensus/ransac.h // RANSAC #include pcl/sample_consensus/method_types.h // 随机采样一致性方法 #include pcl/sample_consensus/model_types.h // 模型类型定义 #include pcl/segmentation/sac_segmentation.h // RANSAC 分割头文件 #include pcl/sample_consensus/sac_model_cylinder.h// 圆柱模型定义 #include pcl/filters/extract_indices.h #include pcl/console/print.h // 日志输出 #include stdexcept // 异常处理 #include boost/thread/thread.hpp #include boost/date_time/posix_time/posix_time.hpp #include pcl/visualization/pcl_visualizer.h // 可视化 #include pcl/common/common.h #include chrono #include thread #include atomic #include mutex #include pcl/io/ply_io.h #include pcl/filters/extract_indices.h std::string current_time() { auto now std::chrono::system_clock::now(); auto in_time_t std::chrono::system_clock::to_time_t(now); std::stringstream ss; ss std::put_time(std::localtime(in_time_t), %Y-%m-%d %H:%M:%S); return ss.str(); } int main(int argc, char** argv) { // ---------- 1. 读取点云 ---------- std::cout [ current_time() ] start load cloud file: inside.ply std::endl; pcl::PointCloudpcl::PointXYZ::Ptr cloud(new pcl::PointCloudpcl::PointXYZ); if (pcl::io::loadPLYFilepcl::PointXYZ(inside.ply, *cloud) 0) { std::cerr [ current_time() ] ERROR: Failed to load point cloud file inside.ply. std::endl; return -1; } std::cout [ current_time() ] success load cloudPoint, Size: cloud-size() std::endl; if (cloud-empty()) { std::cerr [ current_time() ] ERROR: Point cloud is empty. std::endl; return -1; } // ---------- 2. 参数设置 ---------- const float radius 0.1f; // 搜索半径米决定每个点的邻域大小。 // 调试技巧 // - 增大 radius 可在稀疏点云中找到更多邻居但会增加计算量并可能把不同结构的点混在一起。 // - 减小 radius 可提高局部精度适用于高密度点云或希望滤除远离点但会使某些边界点被视为孤立点。 // 建议范围0.05 ~ 1.0根据点云分辨率调整。 const int min_neighbors 100; // 最少邻居数执行统计/中位数计算所需的邻居数量阈值。 // 调试技巧 // - 增大 min_neighbors 会让稀疏点更容易被跳过有助于移除孤立噪声但可能丢失真实的稀疏结构点。 // - 减小 min_neighbors 会保留更多点但可能使噪声对统计结果产生更大影响。 // 建议范围10 ~ 200随点云密度增减。 const float multiplier 0.1f; // 阈值倍率用于判断点是否保留的阈值为 global_median * multiplier。 // 调试技巧 // - 减小 multiplier如 0.3~0.5会更严格地筛选点移除更多离群点/边缘噪声。 // - 增大 multiplier1.0会更宽松地保留点适用于需要保全更多细节的场景。 // 建议范围0.3 ~ 2.0。根据可视化结果反复微调。 // 可配置线程数 unsigned int kd_tree_instances std::thread::hardware_concurrency(); if (kd_tree_instances 0) kd_tree_instances 4; // fallback unsigned int median_threads std::thread::hardware_concurrency(); if (median_threads 0) median_threads 4; // fallback // ---------- 3. 构建 KD-Tree ---------- // 在范围查询如“半径为5米内的所有点”或光线追踪中划分空间可以快速跳过无用的区域 std::cout [ current_time() ] Building KD-Tree(s)... std::endl; auto t_kd_start std::chrono::high_resolution_clock::now(); // 构建多个 KD-Tree 实例供各线程复用避免在线程中重复构建或加锁访问单一实例 unsigned int num_kdtrees kd_tree_instances; std::vectorpcl::KdTreeFLANNpcl::PointXYZ kdtrees; kdtrees.resize(num_kdtrees); try { for (unsigned int k 0; k num_kdtrees; k) { kdtrees[k].setInputCloud(cloud); } } catch (const std::exception e) { std::cerr [ current_time() ] ERROR: KD-Tree build failed: e.what() std::endl; return -1; } catch (...) { std::cerr [ current_time() ] ERROR: KD-Tree build failed: unknown error std::endl; return -1; } auto t_kd_end std::chrono::high_resolution_clock::now(); auto kd_ms std::chrono::duration_caststd::chrono::milliseconds(t_kd_end - t_kd_start).count(); std::cout [ current_time() ] KD-Tree(s) built in kd_ms ms std::endl; std::cout [ current_time() ] KD-Tree instances(threads): num_kdtrees std::endl; // ---------- 4. Compute median neighborhood distance ---------- std::cout [ current_time() ] Start computing median neighborhood distance (multithreaded)... std::endl; std::vectorfloat median_distances; median_distances.resize(cloud-size(), 0.0f); std::atomic_size_t skipped_points_atomic{0}; std::atomic_bool processing_error{false}; std::string processing_error_msg; std::mutex processing_error_mutex; unsigned int num_threads median_threads; std::cout [ current_time() ] Using num_threads threads for median computation std::endl; auto t_med_start std::chrono::high_resolution_clock::now(); auto worker [](unsigned int thread_id, size_t start_idx, size_t end_idx) { try { // 每线程复用对应的 KD-Tree 实例避免重复构建预分配容器以减少内存分配开销 pcl::KdTreeFLANNpcl::PointXYZ local_kdtree kdtrees[thread_id % kdtrees.size()]; std::vectorint local_indices; std::vectorfloat local_distances; local_indices.reserve(min_neighbors * 2); local_distances.reserve(min_neighbors * 2); for (size_t i start_idx; i end_idx; i) { if (!std::isfinite(cloud-at(i).x)) { median_distances[i] 0.0f; skipped_points_atomic; continue; } local_indices.clear(); local_distances.clear(); local_kdtree.radiusSearch(i, radius, local_indices, local_distances); if (local_distances.size() static_castsize_t(min_neighbors)) { median_distances[i] 0.0f; skipped_points_atomic; continue; } size_t m local_distances.size() / 2; std::nth_element(local_distances.begin(), local_distances.begin() m, local_distances.end()); float median_dist local_distances[m]; median_distances[i] median_dist; } } catch (const std::exception e) { std::lock_guardstd::mutex lg(processing_error_mutex); processing_error_msg std::string(Worker thread exception: ) e.what(); processing_error true; } catch (...) { std::lock_guardstd::mutex lg(processing_error_mutex); processing_error_msg Worker thread unknown exception; processing_error true; } }; std::vectorstd::thread threads; threads.reserve(num_threads); size_t N cloud-size(); size_t chunk (N num_threads - 1) / num_threads; for (unsigned int t 0; t num_threads; t) { size_t start t * chunk; if (start N) break; size_t end std::min(N, start chunk); threads.emplace_back(worker, t, start, end); } for (auto th : threads) th.join(); // 如果线程中发生异常输出并安全退出不做可视化 if (processing_error) { std::lock_guardstd::mutex lg(processing_error_mutex); std::cerr [ current_time() ] ERROR: processing_error_msg std::endl; std::cerr [ current_time() ] Aborting processing due to worker thread error. std::endl; return -1; } auto t_med_end std::chrono::high_resolution_clock::now(); auto med_ms std::chrono::duration_caststd::chrono::milliseconds(t_med_end - t_med_start).count(); size_t skipped_points skipped_points_atomic.load(); std::cout [ current_time() ] Median distance computed (multithreaded) in med_ms ms, valid points: (cloud-size() - skipped_points) , skipped points: skipped_points std::endl; // ---------- 5. 计算全局中心距离只使用有效中值并使用 nth_element 加速 ---------- auto t_global_start std::chrono::high_resolution_clock::now(); std::vectorfloat valid_medians; valid_medians.reserve(cloud-size() - skipped_points); for (const auto m : median_distances) { if (m 0.0f) valid_medians.push_back(m); } float global_median 0.0f; if (valid_medians.empty()) { std::cerr [ current_time() ] WARNING: No valid median distances found, global median set to 0. std::endl; } else { size_t mid valid_medians.size() / 2; std::nth_element(valid_medians.begin(), valid_medians.begin() mid, valid_medians.end()); global_median valid_medians[mid]; } auto t_global_end std::chrono::high_resolution_clock::now(); auto global_ms std::chrono::duration_caststd::chrono::milliseconds(t_global_end - t_global_start).count(); std::cout [ current_time() ] Global median distance: global_median (computed in global_ms ms) std::endl; // ---------- 6. Select inliers ---------- auto t_select_start std::chrono::high_resolution_clock::now(); pcl::PointIndices::Ptr inliers(new pcl::PointIndices); inliers-indices.reserve(cloud-size() / 4); for (size_t i 0; i cloud-size(); i) { if (median_distances[i] global_median * multiplier) { // keep } else { inliers-indices.push_back(i); } } auto t_select_end std::chrono::high_resolution_clock::now(); auto select_ms std::chrono::duration_caststd::chrono::milliseconds(t_select_end - t_select_start).count(); std::cout [ current_time() ] Inlier count: inliers-indices.size() (selection in select_ms ms) std::endl; // ---------- 7. Extract filtered result ---------- pcl::ExtractIndicespcl::PointXYZ extract; extract.setInputCloud(cloud); extract.setIndices(inliers); pcl::PointCloudpcl::PointXYZ::Ptr filtered_cloud(new pcl::PointCloudpcl::PointXYZ); auto t_extract_start std::chrono::high_resolution_clock::now(); try { extract.filter(*filtered_cloud); } catch (const std::exception e) { std::cerr [ current_time() ] ERROR: extract.filter failed: e.what() std::endl; return -1; } catch (...) { std::cerr [ current_time() ] ERROR: extract.filter failed: unknown error std::endl; return -1; } auto t_extract_end std::chrono::high_resolution_clock::now(); auto extract_ms std::chrono::duration_caststd::chrono::milliseconds(t_extract_end - t_extract_start).count(); std::cout [ current_time() ] Filtering finished, original points: cloud-size() , filtered points: filtered_cloud-size() (extraction in extract_ms ms) std::endl; // 8. Left-right split visualization std::cout [ current_time() ] Creating left-right comparison window... std::endl; // Print point-cloud bounds. auto print_bounds [](const pcl::PointCloudpcl::PointXYZ::Ptr pc, const char* name) { if (!pc) { std::cout name nullptr\n; return; } std::cout [ current_time() ] name size pc-size(); if (pc-empty()) { std::cout (empty)\n; return; } Eigen::Vector4f minPt, maxPt; pcl::getMinMax3D(*pc, minPt, maxPt); std::cout min( minPt[0] , minPt[1] , minPt[2] ) max( maxPt[0] , maxPt[1] , maxPt[2] )\n; }; print_bounds(cloud, original_cloud); print_bounds(filtered_cloud, filtered_cloud); // Create the visualizer window. boost::shared_ptrpcl::visualization::PCLVisualizer viewer(new pcl::visualization::PCLVisualizer(Point Cloud Filter Comparison - Left: Original | Right: Filtered)); viewer-initCameraParameters(); // Create left and right viewports. int v1, v2; viewer-createViewPort(0.0, 0.0, 0.5, 1.0, v1); // Left viewport viewer-createViewPort(0.5, 0.0, 1.0, 1.0, v2); // Right viewport // Use a dark background for better contrast. viewer-setBackgroundColor(0.05, 0.05, 0.05, v1); viewer-setBackgroundColor(0.05, 0.05, 0.05, v2); // Add a coordinate system. viewer-addCoordinateSystem(0.5); // Configure point-cloud colors. pcl::visualization::PointCloudColorHandlerCustompcl::PointXYZ original_h(cloud, 0, 200, 200); // Cyan pcl::visualization::PointCloudColorHandlerCustompcl::PointXYZ filtered_h(filtered_cloud, 255, 150, 0); // Orange // Add the point clouds to the viewports. if (cloud !cloud-empty()) { viewer-addPointCloud(cloud, original_h, original_cloud, v1); viewer-setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, original_cloud); viewer-addText(Original Cloud, 10, 10, v1_text, v1); } if (filtered_cloud !filtered_cloud-empty()) { viewer-addPointCloud(filtered_cloud, filtered_h, filtered_cloud, v2); viewer-setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, filtered_cloud); viewer-addText(Filtered Cloud, 10, 10, v2_text, v2); } // Reset the camera for a reasonable initial view. viewer-resetCamera(); // Enter the render loop. while (!viewer-wasStopped()) { viewer-spinOnce(50); boost::this_thread::sleep(boost::posix_time::milliseconds(10)); } std::cout [ current_time() ] Comparison window closed. Program finished. std::endl; return 0; }