| 102 | } |
| 103 | |
| 104 | void Instance::findTrees(const CloudT::Ptr pc, |
| 105 | pcl::PointCloud<pcl::Label>& euclidean_labels, |
| 106 | std::vector<pcl::PointIndices>& label_indices, std::vector<std::vector<TreeVertex>>& landmarks){ |
| 107 | |
| 108 | for (size_t i = 0; i < label_indices.size(); i++){ |
| 109 | if (label_indices.at(i).indices.size () > 80){ |
| 110 | std::vector<TreeVertex> tree; |
| 111 | for (int row_idx = pc->height - 1; row_idx >= 0; --row_idx) { |
| 112 | CloudT::Ptr beam(new CloudT); |
| 113 | for (size_t col_idx = 0; col_idx < pc->width; ++col_idx) { |
| 114 | if(euclidean_labels.points[row_idx * pc->width + col_idx].label == i){ |
| 115 | PointT p = pc->at(col_idx, row_idx); |
| 116 | beam->points.push_back(p); |
| 117 | } |
| 118 | } |
| 119 | if(beam->points.size() > 3){ |
| 120 | TreeVertex v = computeTreeVertex(beam, i); |
| 121 | if(v.isValid) tree.push_back(v); |
| 122 | } |
| 123 | } |
| 124 | if(tree.size() > 16){ |
| 125 | if(tree.size() > 56) { |
| 126 | tree.resize(56); |
| 127 | } |
| 128 | landmarks.push_back(tree); |
| 129 | } |
| 130 | } |
| 131 | } |
| 132 | } |
| 133 | |
| 134 | void Instance::computeGraph(const CloudT::Ptr cloud, const CloudT::Ptr tree_cloud, |
| 135 | std::vector<std::vector<TreeVertex>> &landmarks) { |
nothing calls this directly
no outgoing calls
no test coverage detected