| 123 | |
| 124 | |
| 125 | std::tuple<py::array_t<float>, py::array_t<int>> extract_features(py::array_t<float> range_image, int feature_region, int segments, int sharp_num, int less_sharp_num, int flat_num) { |
| 126 | // input array |
| 127 | py::buffer_info ri_buf = range_image.request(); |
| 128 | float *ri_ptr = (float *) ri_buf.ptr; |
| 129 | int h = ri_buf.shape[0]; |
| 130 | int w = ri_buf.shape[1]; |
| 131 | |
| 132 | // return array |
| 133 | py::array_t<float> sharp_feat_map = py::array_t<float>(ri_buf.size); |
| 134 | py::array_t<int> key_point_map = py::array_t<int>(ri_buf.size); |
| 135 | py::buffer_info feat_buf = sharp_feat_map.request(); |
| 136 | py::buffer_info kp_buf = key_point_map.request(); |
| 137 | float *feat_ptr = (float *) feat_buf.ptr; |
| 138 | int *kp_ptr = (int *) kp_buf.ptr; |
| 139 | |
| 140 | // key_point_map initialization |
| 141 | for (int h_i = 0; h_i < h; h_i++) { |
| 142 | for (int w_i = 0; w_i < w; w_i++){ |
| 143 | kp_ptr[h_i * w + w_i] = 0; |
| 144 | } |
| 145 | } |
| 146 | |
| 147 | vector<vector<int>> cloud_neighbors_picked(h, vector<int>(w, 0)); |
| 148 | |
| 149 | for (int h_i = 0; h_i < h; h_i++) { |
| 150 | vector<float> valid_ri; |
| 151 | vector<int> valid_idx; |
| 152 | |
| 153 | for (int w_i = 0; w_i < w; w_i++){ |
| 154 | if (ri_ptr[h_i * w + w_i] != 0){ |
| 155 | valid_ri.push_back(ri_ptr[h_i * w + w_i]); |
| 156 | valid_idx.push_back(w_i); |
| 157 | } |
| 158 | } |
| 159 | int valid_length = valid_idx.size(); |
| 160 | |
| 161 | if (valid_length < segments + feature_region * 2 + 1) |
| 162 | continue; |
| 163 | |
| 164 | vector<float> valid_feat(valid_length, 0); |
| 165 | vector<pair<float, int> > valid_feat_map; |
| 166 | for (int s_i = feature_region; s_i < valid_length - feature_region; s_i++){ |
| 167 | for (int k = -feature_region; k <= feature_region; k++){ |
| 168 | valid_feat[s_i] += valid_ri[s_i + k] - valid_ri[s_i]; |
| 169 | } |
| 170 | valid_feat[s_i] = valid_feat[s_i] * valid_feat[s_i]; |
| 171 | valid_feat[s_i] /= 2 * feature_region; |
| 172 | valid_feat[s_i] /= valid_ri[s_i]; |
| 173 | feat_ptr[h_i * w + valid_idx[s_i]] = valid_feat[s_i]; |
| 174 | valid_feat_map.push_back(make_pair(valid_feat[s_i], s_i)); |
| 175 | } |
| 176 | |
| 177 | for (int j = 0; j < segments; j++){ |
| 178 | int sp = floor(valid_feat_map.size() / segments) * j; |
| 179 | int ep = floor(valid_feat_map.size() / segments) * (j + 1); |
| 180 | |
| 181 | int largest_picked_num = 0; |
| 182 | sort(valid_feat_map.begin() + sp, valid_feat_map.begin() + ep); |
nothing calls this directly
no test coverage detected