| 61 | } |
| 62 | |
| 63 | bool Instance::computeVertexProperties(CloudT::Ptr &pc, Slash& filteredPoints, PointT& median_point, Scalar& radius) { |
| 64 | // Compute median in each x,y,z |
| 65 | int num_points = pc->points.size(); |
| 66 | int middle_point = (int)(num_points / 2.0); |
| 67 | Scalar median_x = 0; |
| 68 | Scalar median_y = 0; |
| 69 | Scalar median_z = 0; |
| 70 | |
| 71 | std::sort(pc->points.begin(), pc->points.end(), |
| 72 | [](const PointT &p1, const PointT &p2) { return p1.x < p2.x; }); |
| 73 | |
| 74 | median_x = pc->points[middle_point].x; |
| 75 | |
| 76 | std::sort(pc->points.begin(), pc->points.end(), |
| 77 | [](const PointT &p1, const PointT &p2) { return p1.y < p2.y; }); |
| 78 | median_y = pc->points[middle_point].y; |
| 79 | |
| 80 | std::sort(pc->points.begin(), pc->points.end(), |
| 81 | [](const PointT &p1, const PointT &p2) { return p1.z < p2.z; }); |
| 82 | median_z = pc->points[middle_point].z; |
| 83 | |
| 84 | // PointT median_point; |
| 85 | median_point.x = median_x; |
| 86 | median_point.y = median_y; |
| 87 | median_point.z = median_z; |
| 88 | |
| 89 | for (const auto &point : pc->points) { |
| 90 | if (euclideanDistance(point, median_point) < params_.max_dist_to_centroid) { |
| 91 | filteredPoints.push_back(point); |
| 92 | } |
| 93 | } |
| 94 | |
| 95 | if(filteredPoints.size() > 1){ |
| 96 | PointT pointA = filteredPoints[0]; |
| 97 | PointT pointB = filteredPoints[filteredPoints.size() - 1]; |
| 98 | radius = euclideanDistance(pointA, pointB); |
| 99 | return true; |
| 100 | } |
| 101 | return false; |
| 102 | } |
| 103 | |
| 104 | void Instance::findTrees(const CloudT::Ptr pc, |
| 105 | pcl::PointCloud<pcl::Label>& euclidean_labels, |
nothing calls this directly
no outgoing calls
no test coverage detected