|
| 1 | +// Copyright (c) OpenMMLab. All rights reserved |
| 2 | +#include "nms_rotated.h" |
| 3 | + |
| 4 | +#include <assert.h> |
| 5 | + |
| 6 | +#include <algorithm> |
| 7 | +#include <cassert> |
| 8 | +#include <cmath> |
| 9 | +#include <iostream> |
| 10 | +#include <iterator> |
| 11 | +#include <numeric> // std::iota |
| 12 | +#include <vector> |
| 13 | + |
| 14 | +#include "ort_utils.h" |
| 15 | + |
| 16 | +namespace mmdeploy { |
| 17 | + |
| 18 | +namespace { |
| 19 | +struct RotatedBox { |
| 20 | + float x_ctr, y_ctr, w, h, a; |
| 21 | +}; |
| 22 | +struct Point { |
| 23 | + float x, y; |
| 24 | + Point(const float& px = 0, const float& py = 0) : x(px), y(py) {} |
| 25 | + Point operator+(const Point& p) const { return Point(x + p.x, y + p.y); } |
| 26 | + Point& operator+=(const Point& p) { |
| 27 | + x += p.x; |
| 28 | + y += p.y; |
| 29 | + return *this; |
| 30 | + } |
| 31 | + Point operator-(const Point& p) const { return Point(x - p.x, y - p.y); } |
| 32 | + Point operator*(const float coeff) const { return Point(x * coeff, y * coeff); } |
| 33 | +}; |
| 34 | + |
| 35 | +float dot_2d(const Point& A, const Point& B) { return A.x * B.x + A.y * B.y; } |
| 36 | + |
| 37 | +float cross_2d(const Point& A, const Point& B) { return A.x * B.y - B.x * A.y; } |
| 38 | +} // namespace |
| 39 | + |
| 40 | +void get_rotated_vertices(const RotatedBox& box, Point (&pts)[4]) { |
| 41 | + // M_PI / 180. == 0.01745329251 |
| 42 | + // double theta = box.a * 0.01745329251; |
| 43 | + // MODIFIED |
| 44 | + double theta = box.a; |
| 45 | + float cosTheta2 = (float)cos(theta) * 0.5f; |
| 46 | + float sinTheta2 = (float)sin(theta) * 0.5f; |
| 47 | + |
| 48 | + // y: top --> down; x: left --> right |
| 49 | + pts[0].x = box.x_ctr - sinTheta2 * box.h - cosTheta2 * box.w; |
| 50 | + pts[0].y = box.y_ctr + cosTheta2 * box.h - sinTheta2 * box.w; |
| 51 | + pts[1].x = box.x_ctr + sinTheta2 * box.h - cosTheta2 * box.w; |
| 52 | + pts[1].y = box.y_ctr - cosTheta2 * box.h - sinTheta2 * box.w; |
| 53 | + pts[2].x = 2 * box.x_ctr - pts[0].x; |
| 54 | + pts[2].y = 2 * box.y_ctr - pts[0].y; |
| 55 | + pts[3].x = 2 * box.x_ctr - pts[1].x; |
| 56 | + pts[3].y = 2 * box.y_ctr - pts[1].y; |
| 57 | +} |
| 58 | + |
| 59 | +int get_intersection_points(const Point (&pts1)[4], const Point (&pts2)[4], |
| 60 | + Point (&intersections)[24]) { |
| 61 | + // Line vector |
| 62 | + // A line from p1 to p2 is: p1 + (p2-p1)*t, t=[0,1] |
| 63 | + Point vec1[4], vec2[4]; |
| 64 | + for (int i = 0; i < 4; i++) { |
| 65 | + vec1[i] = pts1[(i + 1) % 4] - pts1[i]; |
| 66 | + vec2[i] = pts2[(i + 1) % 4] - pts2[i]; |
| 67 | + } |
| 68 | + |
| 69 | + // Line test - test all line combos for intersection |
| 70 | + int num = 0; // number of intersections |
| 71 | + for (int i = 0; i < 4; i++) { |
| 72 | + for (int j = 0; j < 4; j++) { |
| 73 | + // Solve for 2x2 Ax=b |
| 74 | + float det = cross_2d(vec2[j], vec1[i]); |
| 75 | + |
| 76 | + // This takes care of parallel lines |
| 77 | + if (fabs(det) <= 1e-14) { |
| 78 | + continue; |
| 79 | + } |
| 80 | + |
| 81 | + auto vec12 = pts2[j] - pts1[i]; |
| 82 | + |
| 83 | + float t1 = cross_2d(vec2[j], vec12) / det; |
| 84 | + float t2 = cross_2d(vec1[i], vec12) / det; |
| 85 | + |
| 86 | + if (t1 >= 0.0f && t1 <= 1.0f && t2 >= 0.0f && t2 <= 1.0f) { |
| 87 | + intersections[num++] = pts1[i] + vec1[i] * t1; |
| 88 | + } |
| 89 | + } |
| 90 | + } |
| 91 | + |
| 92 | + // Check for vertices of rect1 inside rect2 |
| 93 | + { |
| 94 | + const auto& AB = vec2[0]; |
| 95 | + const auto& DA = vec2[3]; |
| 96 | + auto ABdotAB = dot_2d(AB, AB); |
| 97 | + auto ADdotAD = dot_2d(DA, DA); |
| 98 | + for (int i = 0; i < 4; i++) { |
| 99 | + // assume ABCD is the rectangle, and P is the point to be judged |
| 100 | + // P is inside ABCD iff. P's projection on AB lies within AB |
| 101 | + // and P's projection on AD lies within AD |
| 102 | + |
| 103 | + auto AP = pts1[i] - pts2[0]; |
| 104 | + |
| 105 | + auto APdotAB = dot_2d(AP, AB); |
| 106 | + auto APdotAD = -dot_2d(AP, DA); |
| 107 | + |
| 108 | + if ((APdotAB >= 0) && (APdotAD >= 0) && (APdotAB <= ABdotAB) && (APdotAD <= ADdotAD)) { |
| 109 | + intersections[num++] = pts1[i]; |
| 110 | + } |
| 111 | + } |
| 112 | + } |
| 113 | + |
| 114 | + // Reverse the check - check for vertices of rect2 inside rect1 |
| 115 | + { |
| 116 | + const auto& AB = vec1[0]; |
| 117 | + const auto& DA = vec1[3]; |
| 118 | + auto ABdotAB = dot_2d(AB, AB); |
| 119 | + auto ADdotAD = dot_2d(DA, DA); |
| 120 | + for (int i = 0; i < 4; i++) { |
| 121 | + auto AP = pts2[i] - pts1[0]; |
| 122 | + |
| 123 | + auto APdotAB = dot_2d(AP, AB); |
| 124 | + auto APdotAD = -dot_2d(AP, DA); |
| 125 | + |
| 126 | + if ((APdotAB >= 0) && (APdotAD >= 0) && (APdotAB <= ABdotAB) && (APdotAD <= ADdotAD)) { |
| 127 | + intersections[num++] = pts2[i]; |
| 128 | + } |
| 129 | + } |
| 130 | + } |
| 131 | + |
| 132 | + return num; |
| 133 | +} |
| 134 | + |
| 135 | +int convex_hull_graham(const Point (&p)[24], const int& num_in, Point (&q)[24], |
| 136 | + bool shift_to_zero = false) { |
| 137 | + assert(num_in >= 2); |
| 138 | + |
| 139 | + // Step 1: |
| 140 | + // Find point with minimum y |
| 141 | + // if more than 1 points have the same minimum y, |
| 142 | + // pick the one with the minimum x. |
| 143 | + int t = 0; |
| 144 | + for (int i = 1; i < num_in; i++) { |
| 145 | + if (p[i].y < p[t].y || (p[i].y == p[t].y && p[i].x < p[t].x)) { |
| 146 | + t = i; |
| 147 | + } |
| 148 | + } |
| 149 | + auto& start = p[t]; // starting point |
| 150 | + |
| 151 | + // Step 2: |
| 152 | + // Subtract starting point from every points (for sorting in the next step) |
| 153 | + for (int i = 0; i < num_in; i++) { |
| 154 | + q[i] = p[i] - start; |
| 155 | + } |
| 156 | + |
| 157 | + // Swap the starting point to position 0 |
| 158 | + auto tmp = q[0]; |
| 159 | + q[0] = q[t]; |
| 160 | + q[t] = tmp; |
| 161 | + |
| 162 | + // Step 3: |
| 163 | + // Sort point 1 ~ num_in according to their relative cross-product values |
| 164 | + // (essentially sorting according to angles) |
| 165 | + // If the angles are the same, sort according to their distance to origin |
| 166 | + float dist[24]; |
| 167 | + for (int i = 0; i < num_in; i++) { |
| 168 | + dist[i] = dot_2d(q[i], q[i]); |
| 169 | + } |
| 170 | + |
| 171 | + // CPU version |
| 172 | + std::sort(q + 1, q + num_in, [](const Point& A, const Point& B) -> bool { |
| 173 | + float temp = cross_2d(A, B); |
| 174 | + if (fabs(temp) < 1e-6) { |
| 175 | + return dot_2d(A, A) < dot_2d(B, B); |
| 176 | + } else { |
| 177 | + return temp > 0; |
| 178 | + } |
| 179 | + }); |
| 180 | + // compute distance to origin after sort, since the points are now different. |
| 181 | + for (int i = 0; i < num_in; i++) { |
| 182 | + dist[i] = dot_2d(q[i], q[i]); |
| 183 | + } |
| 184 | + |
| 185 | + // Step 4: |
| 186 | + // Make sure there are at least 2 points (that don't overlap with each other) |
| 187 | + // in the stack |
| 188 | + int k; // index of the non-overlapped second point |
| 189 | + for (k = 1; k < num_in; k++) { |
| 190 | + if (dist[k] > 1e-8) { |
| 191 | + break; |
| 192 | + } |
| 193 | + } |
| 194 | + if (k == num_in) { |
| 195 | + // We reach the end, which means the convex hull is just one point |
| 196 | + q[0] = p[t]; |
| 197 | + return 1; |
| 198 | + } |
| 199 | + q[1] = q[k]; |
| 200 | + int m = 2; // 2 points in the stack |
| 201 | + // Step 5: |
| 202 | + // Finally we can start the scanning process. |
| 203 | + // When a non-convex relationship between the 3 points is found |
| 204 | + // (either concave shape or duplicated points), |
| 205 | + // we pop the previous point from the stack |
| 206 | + // until the 3-point relationship is convex again, or |
| 207 | + // until the stack only contains two points |
| 208 | + for (int i = k + 1; i < num_in; i++) { |
| 209 | + while (m > 1 && cross_2d(q[i] - q[m - 2], q[m - 1] - q[m - 2]) >= 0) { |
| 210 | + m--; |
| 211 | + } |
| 212 | + q[m++] = q[i]; |
| 213 | + } |
| 214 | + |
| 215 | + // Step 6 (Optional): |
| 216 | + // In general sense we need the original coordinates, so we |
| 217 | + // need to shift the points back (reverting Step 2) |
| 218 | + // But if we're only interested in getting the area/perimeter of the shape |
| 219 | + // We can simply return. |
| 220 | + if (!shift_to_zero) { |
| 221 | + for (int i = 0; i < m; i++) { |
| 222 | + q[i] += start; |
| 223 | + } |
| 224 | + } |
| 225 | + |
| 226 | + return m; |
| 227 | +} |
| 228 | + |
| 229 | +float polygon_area(const Point (&q)[24], const int& m) { |
| 230 | + if (m <= 2) { |
| 231 | + return 0; |
| 232 | + } |
| 233 | + |
| 234 | + float area = 0; |
| 235 | + for (int i = 1; i < m - 1; i++) { |
| 236 | + area += fabs(cross_2d(q[i] - q[0], q[i + 1] - q[0])); |
| 237 | + } |
| 238 | + |
| 239 | + return area / 2.0; |
| 240 | +} |
| 241 | + |
| 242 | +float rotated_boxes_intersection(const RotatedBox& box1, const RotatedBox& box2) { |
| 243 | + // There are up to 4 x 4 + 4 + 4 = 24 intersections (including dups) returned |
| 244 | + // from rotated_rect_intersection_pts |
| 245 | + Point intersectPts[24], orderedPts[24]; |
| 246 | + |
| 247 | + Point pts1[4]; |
| 248 | + Point pts2[4]; |
| 249 | + get_rotated_vertices(box1, pts1); |
| 250 | + get_rotated_vertices(box2, pts2); |
| 251 | + |
| 252 | + int num = get_intersection_points(pts1, pts2, intersectPts); |
| 253 | + |
| 254 | + if (num <= 2) { |
| 255 | + return 0.0; |
| 256 | + } |
| 257 | + |
| 258 | + // Convex Hull to order the intersection points in clockwise order and find |
| 259 | + // the contour area. |
| 260 | + int num_convex = convex_hull_graham(intersectPts, num, orderedPts, true); |
| 261 | + return polygon_area(orderedPts, num_convex); |
| 262 | +} |
| 263 | + |
| 264 | +NMSRotatedKernel::NMSRotatedKernel(OrtApi api, const OrtKernelInfo* info) |
| 265 | + : api_(api), ort_(api_), info_(info) { |
| 266 | + iou_threshold_ = ort_.KernelInfoGetAttribute<float>(info, "iou_threshold"); |
| 267 | + |
| 268 | + // create allocator |
| 269 | + allocator_ = Ort::AllocatorWithDefaultOptions(); |
| 270 | +} |
| 271 | + |
| 272 | +void NMSRotatedKernel::Compute(OrtKernelContext* context) { |
| 273 | + const float iou_threshold = iou_threshold_; |
| 274 | + |
| 275 | + const OrtValue* boxes = ort_.KernelContext_GetInput(context, 0); |
| 276 | + const float* boxes_data = reinterpret_cast<const float*>(ort_.GetTensorData<float>(boxes)); |
| 277 | + const OrtValue* scores = ort_.KernelContext_GetInput(context, 1); |
| 278 | + const float* scores_data = reinterpret_cast<const float*>(ort_.GetTensorData<float>(scores)); |
| 279 | + |
| 280 | + OrtTensorDimensions boxes_dim(ort_, boxes); |
| 281 | + OrtTensorDimensions scores_dim(ort_, scores); |
| 282 | + |
| 283 | + int64_t nboxes = boxes_dim[0]; |
| 284 | + assert(boxes_dim[1] == 5); //(cx,cy,w,h,theta) |
| 285 | + |
| 286 | + // allocate tmp memory |
| 287 | + float* tmp_boxes = (float*)allocator_.Alloc(sizeof(float) * nboxes * 5); |
| 288 | + float* sc = (float*)allocator_.Alloc(sizeof(float) * nboxes); |
| 289 | + bool* select = (bool*)allocator_.Alloc(sizeof(bool) * nboxes); |
| 290 | + for (int64_t i = 0; i < nboxes; i++) { |
| 291 | + select[i] = true; |
| 292 | + } |
| 293 | + |
| 294 | + memcpy(tmp_boxes, boxes_data, sizeof(float) * nboxes * 5); |
| 295 | + memcpy(sc, scores_data, sizeof(float) * nboxes); |
| 296 | + |
| 297 | + // sort scores |
| 298 | + std::vector<float> tmp_sc; |
| 299 | + for (int i = 0; i < nboxes; i++) { |
| 300 | + tmp_sc.push_back(sc[i]); |
| 301 | + } |
| 302 | + std::vector<int64_t> order(tmp_sc.size()); |
| 303 | + std::iota(order.begin(), order.end(), 0); |
| 304 | + std::sort(order.begin(), order.end(), |
| 305 | + [&tmp_sc](int64_t id1, int64_t id2) { return tmp_sc[id1] > tmp_sc[id2]; }); |
| 306 | + |
| 307 | + for (int64_t _i = 0; _i < nboxes; _i++) { |
| 308 | + if (select[_i] == false) continue; |
| 309 | + auto i = order[_i]; |
| 310 | + |
| 311 | + for (int64_t _j = _i + 1; _j < nboxes; _j++) { |
| 312 | + if (select[_j] == false) continue; |
| 313 | + auto j = order[_j]; |
| 314 | + RotatedBox box1, box2; |
| 315 | + auto center_shift_x = (tmp_boxes[i * 5] + tmp_boxes[j * 5]) / 2.0; |
| 316 | + auto center_shift_y = (tmp_boxes[i * 5 + 1] + tmp_boxes[j * 5 + 1]) / 2.0; |
| 317 | + box1.x_ctr = tmp_boxes[i * 5] - center_shift_x; |
| 318 | + box1.y_ctr = tmp_boxes[i * 5 + 1] - center_shift_y; |
| 319 | + box1.w = tmp_boxes[i * 5 + 2]; |
| 320 | + box1.h = tmp_boxes[i * 5 + 3]; |
| 321 | + box1.a = tmp_boxes[i * 5 + 4]; |
| 322 | + box2.x_ctr = tmp_boxes[j * 5] - center_shift_x; |
| 323 | + box2.y_ctr = tmp_boxes[j * 5 + 1] - center_shift_y; |
| 324 | + box2.w = tmp_boxes[j * 5 + 2]; |
| 325 | + box2.h = tmp_boxes[j * 5 + 3]; |
| 326 | + box2.a = tmp_boxes[j * 5 + 4]; |
| 327 | + auto area1 = box1.w * box1.h; |
| 328 | + auto area2 = box2.w * box2.h; |
| 329 | + auto intersection = rotated_boxes_intersection(box1, box2); |
| 330 | + float baseS = 1.0; |
| 331 | + baseS = (area1 + area2 - intersection); |
| 332 | + auto ovr = intersection / baseS; |
| 333 | + if (ovr > iou_threshold) select[_j] = false; |
| 334 | + } |
| 335 | + } |
| 336 | + std::vector<int64_t> res_order; |
| 337 | + for (int i = 0; i < nboxes; i++) { |
| 338 | + if (select[i]) { |
| 339 | + res_order.push_back(order[i]); |
| 340 | + } |
| 341 | + } |
| 342 | + |
| 343 | + std::vector<int64_t> inds_dims({(int64_t)res_order.size()}); |
| 344 | + |
| 345 | + OrtValue* res = ort_.KernelContext_GetOutput(context, 0, inds_dims.data(), inds_dims.size()); |
| 346 | + int64_t* res_data = ort_.GetTensorMutableData<int64_t>(res); |
| 347 | + |
| 348 | + memcpy(res_data, res_order.data(), sizeof(int64_t) * res_order.size()); |
| 349 | +} |
| 350 | + |
| 351 | +REGISTER_ONNXRUNTIME_OPS(mmdeploy, NMSRotatedOp); |
| 352 | +} // namespace mmdeploy |
0 commit comments