1.计算点云最近点的平均距离(点云的平均距离)http://pointclouds.org/documentation/tutorials/correspondence_grouping.php
1 double computeCloudResolution (const pcl::PointCloud<PointType>::ConstPtr &cloud) 2 { 3 double res = 0.0; 4 int n_points = 0; 5 int nres; 6 std::vector<int> indices (2); 7 std::vector<float> sqr_distances (2); 8 pcl::search::KdTree<PointType> tree; 9 tree.setInputCloud (cloud); 10 11 for (size_t i = 0; i < cloud->size (); ++i) 12 { 13 if (! pcl_isfinite ((*cloud)[i].x)) 14 { 15 continue; 16 } 17 //Considering the second neighbor since the first is the point itself. 18 nres = tree.nearestKSearch (i, 2, indices, sqr_distances); 19 if (nres == 2) 20 { 21 res += sqrt (sqr_distances[1]); 22 ++n_points; 23 } 24 } 25 if (n_points != 0) 26 { 27 res /= n_points; 28 } 29 return res; 30 }
时间: 2024-11-08 17:39:49