Open3D (C++ API)  0.12.0
IoUImpl.h
Go to the documentation of this file.
1 // ----------------------------------------------------------------------------
2 // - Open3D: www.open3d.org -
3 // ----------------------------------------------------------------------------
4 // The MIT License (MIT)
5 //
6 // Copyright (c) 2019 www.open3d.org
7 //
8 // Permission is hereby granted, free of charge, to any person obtaining a copy
9 // of this software and associated documentation files (the "Software"), to deal
10 // in the Software without restriction, including without limitation the rights
11 // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12 // copies of the Software, and to permit persons to whom the Software is
13 // furnished to do so, subject to the following conditions:
14 //
15 // The above copyright notice and this permission notice shall be included in
16 // all copies or substantial portions of the Software.
17 //
18 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
23 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
24 // IN THE SOFTWARE.
25 // ----------------------------------------------------------------------------
26 
27 #pragma once
28 
29 #include <math.h>
30 
31 #include "open3d/core/CUDAUtils.h"
32 
33 namespace open3d {
34 namespace ml {
35 namespace contrib {
36 
37 constexpr int NMS_BLOCK_SIZE = sizeof(uint64_t) * 8;
38 constexpr float EPS = static_cast<float>(1e-8);
39 
40 struct Point {
42  OPEN3D_HOST_DEVICE Point(float x, float y) : x_(x), y_(y) {}
43  OPEN3D_HOST_DEVICE void set(float x, float y) {
44  x_ = x;
45  y_ = y;
46  }
48  return Point(x_ + b.x_, y_ + b.y_);
49  }
51  return Point(x_ - b.x_, y_ - b.y_);
52  }
53  float x_, y_;
54 };
55 
56 OPEN3D_HOST_DEVICE inline float Cross(const Point &a, const Point &b) {
57  return a.x_ * b.y_ - a.y_ * b.x_;
58 }
59 
60 OPEN3D_HOST_DEVICE inline float Cross(const Point &p1,
61  const Point &p2,
62  const Point &p0) {
63  return (p1.x_ - p0.x_) * (p2.y_ - p0.y_) -
64  (p2.x_ - p0.x_) * (p1.y_ - p0.y_);
65 }
66 
68  const Point &p2,
69  const Point &q1,
70  const Point &q2) {
71  int ret = fmin(p1.x_, p2.x_) <= fmax(q1.x_, q2.x_) &&
72  fmin(q1.x_, q2.x_) <= fmax(p1.x_, p2.x_) &&
73  fmin(p1.y_, p2.y_) <= fmax(q1.y_, q2.y_) &&
74  fmin(q1.y_, q2.y_) <= fmax(p1.y_, p2.y_);
75  return ret;
76 }
77 
78 OPEN3D_HOST_DEVICE inline int CheckInBox2D(const float *box, const Point &p) {
79  // box (5): [x1, y1, x2, y2, angle].
80  const float MARGIN = static_cast<float>(1e-5);
81 
82  float center_x = (box[0] + box[2]) / 2;
83  float center_y = (box[1] + box[3]) / 2;
84  // Rotate the point in the opposite direction of box.
85  float angle_cos = cos(-box[4]), angle_sin = sin(-box[4]);
86  float rot_x = (p.x_ - center_x) * angle_cos +
87  (p.y_ - center_y) * angle_sin + center_x;
88  float rot_y = -(p.x_ - center_x) * angle_sin +
89  (p.y_ - center_y) * angle_cos + center_y;
90  return (rot_x > box[0] - MARGIN && rot_x < box[2] + MARGIN &&
91  rot_y > box[1] - MARGIN && rot_y < box[3] + MARGIN);
92 }
93 
94 OPEN3D_HOST_DEVICE inline int Intersection(const Point &p1,
95  const Point &p0,
96  const Point &q1,
97  const Point &q0,
98  Point &ans) {
99  // Fast exclusion.
100  if (CheckRectCross(p0, p1, q0, q1) == 0) return 0;
101 
102  // Check Cross standing
103  float s1 = Cross(q0, p1, p0);
104  float s2 = Cross(p1, q1, p0);
105  float s3 = Cross(p0, q1, q0);
106  float s4 = Cross(q1, p1, q0);
107 
108  if (!(s1 * s2 > 0 && s3 * s4 > 0)) return 0;
109 
110  // Calculate Intersection of two lines.
111  float s5 = Cross(q1, p1, p0);
112  if (fabs(s5 - s1) > EPS) {
113  ans.x_ = (s5 * q0.x_ - s1 * q1.x_) / (s5 - s1);
114  ans.y_ = (s5 * q0.y_ - s1 * q1.y_) / (s5 - s1);
115 
116  } else {
117  float a0 = p0.y_ - p1.y_, b0 = p1.x_ - p0.x_,
118  c0 = p0.x_ * p1.y_ - p1.x_ * p0.y_;
119  float a1 = q0.y_ - q1.y_, b1 = q1.x_ - q0.x_,
120  c1 = q0.x_ * q1.y_ - q1.x_ * q0.y_;
121  float D = a0 * b1 - a1 * b0;
122 
123  ans.x_ = (b0 * c1 - b1 * c0) / D;
124  ans.y_ = (a1 * c0 - a0 * c1) / D;
125  }
126 
127  return 1;
128 }
129 
130 OPEN3D_HOST_DEVICE inline void RotateAroundCenter(const Point &center,
131  const float angle_cos,
132  const float angle_sin,
133  Point &p) {
134  float new_x = (p.x_ - center.x_) * angle_cos +
135  (p.y_ - center.y_) * angle_sin + center.x_;
136  float new_y = -(p.x_ - center.x_) * angle_sin +
137  (p.y_ - center.y_) * angle_cos + center.y_;
138  p.set(new_x, new_y);
139 }
140 
141 OPEN3D_HOST_DEVICE inline int PointCmp(const Point &a,
142  const Point &b,
143  const Point &center) {
144  return atan2(a.y_ - center.y_, a.x_ - center.x_) >
145  atan2(b.y_ - center.y_, b.x_ - center.x_);
146 }
147 
148 OPEN3D_HOST_DEVICE inline float BoxOverlap(const float *box_a,
149  const float *box_b) {
150  // box_a (5) [x1, y1, x2, y2, angle].
151  // box_b (5) [x1, y1, x2, y2, angle].
152  float a_x1 = box_a[0], a_y1 = box_a[1], a_x2 = box_a[2], a_y2 = box_a[3],
153  a_angle = box_a[4];
154  float b_x1 = box_b[0], b_y1 = box_b[1], b_x2 = box_b[2], b_y2 = box_b[3],
155  b_angle = box_b[4];
156 
157  Point center_a((a_x1 + a_x2) / 2, (a_y1 + a_y2) / 2);
158  Point center_b((b_x1 + b_x2) / 2, (b_y1 + b_y2) / 2);
159 
160  Point box_a_corners[5];
161  box_a_corners[0].set(a_x1, a_y1);
162  box_a_corners[1].set(a_x2, a_y1);
163  box_a_corners[2].set(a_x2, a_y2);
164  box_a_corners[3].set(a_x1, a_y2);
165 
166  Point box_b_corners[5];
167  box_b_corners[0].set(b_x1, b_y1);
168  box_b_corners[1].set(b_x2, b_y1);
169  box_b_corners[2].set(b_x2, b_y2);
170  box_b_corners[3].set(b_x1, b_y2);
171 
172  // Get oriented corners.
173  float a_angle_cos = cos(a_angle), a_angle_sin = sin(a_angle);
174  float b_angle_cos = cos(b_angle), b_angle_sin = sin(b_angle);
175 
176  for (int k = 0; k < 4; k++) {
177  RotateAroundCenter(center_a, a_angle_cos, a_angle_sin,
178  box_a_corners[k]);
179  RotateAroundCenter(center_b, b_angle_cos, b_angle_sin,
180  box_b_corners[k]);
181  }
182 
183  box_a_corners[4] = box_a_corners[0];
184  box_b_corners[4] = box_b_corners[0];
185 
186  // Get Intersection of lines.
187  Point cross_points[16];
188  Point poly_center;
189  int cnt = 0, flag = 0;
190 
191  poly_center.set(0, 0);
192  for (int i = 0; i < 4; i++) {
193  for (int j = 0; j < 4; j++) {
194  flag = Intersection(box_a_corners[i + 1], box_a_corners[i],
195  box_b_corners[j + 1], box_b_corners[j],
196  cross_points[cnt]);
197  if (flag) {
198  poly_center = poly_center + cross_points[cnt];
199  cnt++;
200  }
201  }
202  }
203 
204  // Check corners.
205  for (int k = 0; k < 4; k++) {
206  if (CheckInBox2D(box_a, box_b_corners[k])) {
207  poly_center = poly_center + box_b_corners[k];
208  cross_points[cnt] = box_b_corners[k];
209  cnt++;
210  }
211  if (CheckInBox2D(box_b, box_a_corners[k])) {
212  poly_center = poly_center + box_a_corners[k];
213  cross_points[cnt] = box_a_corners[k];
214  cnt++;
215  }
216  }
217 
218  poly_center.x_ /= cnt;
219  poly_center.y_ /= cnt;
220 
221  // Sort the points of polygon.
222  Point temp;
223  for (int j = 0; j < cnt - 1; j++) {
224  for (int i = 0; i < cnt - j - 1; i++) {
225  if (PointCmp(cross_points[i], cross_points[i + 1], poly_center)) {
226  temp = cross_points[i];
227  cross_points[i] = cross_points[i + 1];
228  cross_points[i + 1] = temp;
229  }
230  }
231  }
232 
233  // Get the overlap areas.
234  float area = 0;
235  for (int k = 0; k < cnt - 1; k++) {
236  area += Cross(cross_points[k] - cross_points[0],
237  cross_points[k + 1] - cross_points[0]);
238  }
239 
240  return static_cast<float>(fabs(area)) / 2.0f;
241 }
242 
245  const float *box_a,
246  const float *box_b,
247  bool intersection_only = false) {
248  // params: box_a (5) [x1, y1, x2, y2, angle].
249  // params: box_b (5) [x1, y1, x2, y2, angle].
250  float sa = (box_a[2] - box_a[0]) * (box_a[3] - box_a[1]);
251  float sb = (box_b[2] - box_b[0]) * (box_b[3] - box_b[1]);
252  float s_overlap = BoxOverlap(box_a, box_b);
253  if (intersection_only) {
254  return s_overlap;
255  } else {
256  return s_overlap / fmaxf(sa + sb - s_overlap, EPS);
257  }
258 }
259 
262  const float *box_a,
263  const float *box_b,
264  bool intersection_only = false) {
265  float box_a_new[5];
266  box_a_new[0] = box_a[0] - box_a[2] / 2;
267  box_a_new[1] = box_a[1] - box_a[3] / 2;
268  box_a_new[2] = box_a[0] + box_a[2] / 2;
269  box_a_new[3] = box_a[1] + box_a[3] / 2;
270  box_a_new[4] = box_a[4];
271 
272  float box_b_new[5];
273  box_b_new[0] = box_b[0] - box_b[2] / 2;
274  box_b_new[1] = box_b[1] - box_b[3] / 2;
275  box_b_new[2] = box_b[0] + box_b[2] / 2;
276  box_b_new[3] = box_b[1] + box_b[3] / 2;
277  box_b_new[4] = box_b[4];
278  return IoUBev2DWithMinAndMax(box_a_new, box_b_new, intersection_only);
279 }
280 
282 OPEN3D_HOST_DEVICE inline float IoU3DWithCenterAndSize(const float *box_a,
283  const float *box_b) {
284  float box_a_2d[5];
285  box_a_2d[0] = box_a[0];
286  box_a_2d[1] = box_a[2];
287  box_a_2d[2] = box_a[3];
288  box_a_2d[3] = box_a[5];
289  box_a_2d[4] = box_a[6];
290 
291  float box_b_2d[5];
292  box_b_2d[0] = box_b[0];
293  box_b_2d[1] = box_b[2];
294  box_b_2d[2] = box_b[3];
295  box_b_2d[3] = box_b[5];
296  box_b_2d[4] = box_b[6];
297  float intersection_2d = IoUBev2DWithCenterAndSize(box_a_2d, box_b_2d, true);
298 
299  float y_a_min = box_a[1] - box_a[4];
300  float y_a_max = box_a[1];
301  float y_b_min = box_b[1] - box_b[4];
302  float y_b_max = box_b[1];
303  float iw = (y_a_max < y_b_max ? y_a_max : y_b_max) -
304  (y_a_min > y_b_min ? y_a_min : y_b_min);
305  float iou_3d = 0;
306  if (iw > 0) {
307  float intersection_3d = intersection_2d * iw;
308  float volume_a = box_a[3] * box_a[4] * box_a[5];
309  float volume_b = box_b[3] * box_b[4] * box_b[5];
310  float union_3d = volume_a + volume_b - intersection_3d;
311  iou_3d = intersection_3d / union_3d;
312  }
313  return iou_3d;
314 }
315 
316 } // namespace contrib
317 } // namespace ml
318 } // namespace open3d
OPEN3D_HOST_DEVICE float IoUBev2DWithMinAndMax(const float *box_a, const float *box_b, bool intersection_only=false)
(x_min, z_min, x_max, z_max, y_rotate)
Definition: IoUImpl.h:244
OPEN3D_HOST_DEVICE float IoUBev2DWithCenterAndSize(const float *box_a, const float *box_b, bool intersection_only=false)
(x_center, z_center, x_size, z_size, y_rotate)
Definition: IoUImpl.h:261
OPEN3D_HOST_DEVICE int Intersection(const Point &p1, const Point &p0, const Point &q1, const Point &q0, Point &ans)
Definition: IoUImpl.h:94
OPEN3D_HOST_DEVICE Point operator+(const Point &b) const
Definition: IoUImpl.h:47
OPEN3D_HOST_DEVICE float BoxOverlap(const float *box_a, const float *box_b)
Definition: IoUImpl.h:148
constexpr float EPS
Definition: IoUImpl.h:38
const char const char value recording_handle imu_sample recording_handle uint8_t size_t data_size k4a_record_configuration_t config target_format k4a_capture_t capture_handle k4a_imu_sample_t imu_sample uint64_t
Definition: K4aPlugin.cpp:352
float y_
Definition: IoUImpl.h:53
#define OPEN3D_HOST_DEVICE
Definition: CUDAUtils.h:54
OPEN3D_HOST_DEVICE void RotateAroundCenter(const Point &center, const float angle_cos, const float angle_sin, Point &p)
Definition: IoUImpl.h:130
OPEN3D_HOST_DEVICE void set(float x, float y)
Definition: IoUImpl.h:43
OPEN3D_HOST_DEVICE int CheckRectCross(const Point &p1, const Point &p2, const Point &q1, const Point &q2)
Definition: IoUImpl.h:67
constexpr int NMS_BLOCK_SIZE
Definition: IoUImpl.h:37
OPEN3D_HOST_DEVICE Point()
Definition: IoUImpl.h:41
OPEN3D_HOST_DEVICE float IoU3DWithCenterAndSize(const float *box_a, const float *box_b)
(x_center, y_max, z_center, x_size, y_size, z_size, y_rotate)
Definition: IoUImpl.h:282
Definition: IoUImpl.h:40
Definition: PinholeCameraIntrinsic.cpp:35
OPEN3D_HOST_DEVICE int PointCmp(const Point &a, const Point &b, const Point &center)
Definition: IoUImpl.h:141
OPEN3D_HOST_DEVICE float Cross(const Point &a, const Point &b)
Definition: IoUImpl.h:56
Common CUDA utilities.
OPEN3D_HOST_DEVICE Point operator-(const Point &b) const
Definition: IoUImpl.h:50
float x_
Definition: IoUImpl.h:53
OPEN3D_HOST_DEVICE int CheckInBox2D(const float *box, const Point &p)
Definition: IoUImpl.h:78
OPEN3D_HOST_DEVICE Point(float x, float y)
Definition: IoUImpl.h:42