| 26 | |
| 27 | |
| 28 | std::tuple<py::array_t<float>, py::array_t<int>> extract_features_with_segment(py::array_t<float> range_image, py::array_t<int> seg_idx, int feature_region, int segments, int sharp_num, int less_sharp_num, int flat_num) { |
| 29 | // input array |
| 30 | py::buffer_info ri_buf = range_image.request(); |
| 31 | py::buffer_info seg_idx_buf = seg_idx.request(); |
| 32 | float *ri_ptr = (float *) ri_buf.ptr; |
| 33 | int *seg_idx_ptr = (int *) seg_idx_buf.ptr; |
| 34 | int h = ri_buf.shape[0]; |
| 35 | int w = ri_buf.shape[1]; |
| 36 | |
| 37 | // return array |
| 38 | py::array_t<float> sharp_feat_map = py::array_t<float>(ri_buf.size); |
| 39 | py::array_t<int> key_point_map = py::array_t<int>(ri_buf.size); |
| 40 | py::buffer_info feat_buf = sharp_feat_map.request(); |
| 41 | py::buffer_info kp_buf = key_point_map.request(); |
| 42 | float *feat_ptr = (float *) feat_buf.ptr; |
| 43 | int *kp_ptr = (int *) kp_buf.ptr; |
| 44 | |
| 45 | vector<vector<int>> cloud_neighbors_picked(h, vector<int>(w, 0)); |
| 46 | |
| 47 | for (int h_i = 0; h_i < h; h_i++) { |
| 48 | vector<float> valid_ri; |
| 49 | vector<int> valid_idx; |
| 50 | |
| 51 | for (int w_i = 0; w_i < w; w_i++){ |
| 52 | if (seg_idx_ptr[h_i * w + w_i] != 0 && seg_idx_ptr[h_i * w + w_i] != 1){ |
| 53 | valid_ri.push_back(ri_ptr[h_i * w + w_i]); |
| 54 | valid_idx.push_back(w_i); |
| 55 | } |
| 56 | } |
| 57 | int valid_length = valid_idx.size(); |
| 58 | |
| 59 | if (valid_length < segments + feature_region * 2 + 1) |
| 60 | continue; |
| 61 | |
| 62 | vector<float> valid_feat(valid_length, 0); |
| 63 | vector<pair<float, int> > valid_feat_map; |
| 64 | for (int s_i = feature_region; s_i < valid_length - feature_region; s_i++){ |
| 65 | for (int k = -feature_region; k <= feature_region; k++){ |
| 66 | valid_feat[s_i] += valid_ri[s_i + k] - valid_ri[s_i]; |
| 67 | } |
| 68 | valid_feat[s_i] = valid_feat[s_i] * valid_feat[s_i]; |
| 69 | valid_feat[s_i] /= 2 * feature_region; |
| 70 | valid_feat[s_i] /= valid_ri[s_i]; |
| 71 | feat_ptr[h_i * w + valid_idx[s_i]] = valid_feat[s_i]; |
| 72 | valid_feat_map.push_back(make_pair(valid_feat[s_i], s_i)); |
| 73 | } |
| 74 | |
| 75 | for (int j = 0; j < segments; j++){ |
| 76 | int sp = floor(valid_feat_map.size() / segments) * j; |
| 77 | int ep = floor(valid_feat_map.size() / segments) * (j + 1); |
| 78 | |
| 79 | int largest_picked_num = 0; |
| 80 | sort(valid_feat_map.begin() + sp, valid_feat_map.begin() + ep); |
| 81 | for (int i = ep - 1; i >= sp; i--){ |
| 82 | int idx = valid_feat_map[i].second; |
| 83 | valid_feat_map[i].first = 0; |
| 84 | if (cloud_neighbors_picked[h_i][valid_idx[idx]] == 0) |
| 85 | if (mark_as_picked(ri_ptr, cloud_neighbors_picked, h_i, valid_idx[idx], feature_region)){ |
nothing calls this directly
no test coverage detected