Open3D (C++ API)  0.16.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) 2018-2021 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/Macro.h"
33
34namespace open3d {
35namespace ml {
36namespace contrib {
37
38constexpr int NMS_BLOCK_SIZE = sizeof(uint64_t) * 8;
39constexpr float EPS = static_cast<float>(1e-8);
40
41struct Point {
43 OPEN3D_HOST_DEVICE Point(float x, float y) : x_(x), y_(y) {}
44 OPEN3D_HOST_DEVICE void set(float x, float y) {
45 x_ = x;
46 y_ = y;
47 }
49 return Point(x_ + b.x_, y_ + b.y_);
50 }
52 return Point(x_ - b.x_, y_ - b.y_);
53 }
54 float x_ = 0.0f;
55 float y_ = 0.0f;
56};
57
58OPEN3D_HOST_DEVICE inline float Cross(const Point &a, const Point &b) {
59 return a.x_ * b.y_ - a.y_ * b.x_;
60}
61
62OPEN3D_HOST_DEVICE inline float Cross(const Point &p1,
63 const Point &p2,
64 const Point &p0) {
65 return (p1.x_ - p0.x_) * (p2.y_ - p0.y_) -
66 (p2.x_ - p0.x_) * (p1.y_ - p0.y_);
67}
68
70 const Point &p2,
71 const Point &q1,
72 const Point &q2) {
73 int ret = fmin(p1.x_, p2.x_) <= fmax(q1.x_, q2.x_) &&
74 fmin(q1.x_, q2.x_) <= fmax(p1.x_, p2.x_) &&
75 fmin(p1.y_, p2.y_) <= fmax(q1.y_, q2.y_) &&
76 fmin(q1.y_, q2.y_) <= fmax(p1.y_, p2.y_);
77 return ret;
78}
79
80OPEN3D_HOST_DEVICE inline int CheckInBox2D(const float *box, const Point &p) {
81 // box (5): [x1, y1, x2, y2, angle].
82 const float MARGIN = static_cast<float>(1e-5);
83
84 float center_x = (box[0] + box[2]) / 2;
85 float center_y = (box[1] + box[3]) / 2;
86 // Rotate the point in the opposite direction of box.
87 float angle_cos = cos(-box[4]), angle_sin = sin(-box[4]);
88 float rot_x = (p.x_ - center_x) * angle_cos +
89 (p.y_ - center_y) * angle_sin + center_x;
90 float rot_y = -(p.x_ - center_x) * angle_sin +
91 (p.y_ - center_y) * angle_cos + center_y;
92 return (rot_x > box[0] - MARGIN && rot_x < box[2] + MARGIN &&
93 rot_y > box[1] - MARGIN && rot_y < box[3] + MARGIN);
94}
95
97 const Point &p0,
98 const Point &q1,
99 const Point &q0,
100 Point &ans) {
101 // Fast exclusion.
102 if (CheckRectCross(p0, p1, q0, q1) == 0) return 0;
103
104 // Check Cross standing
105 float s1 = Cross(q0, p1, p0);
106 float s2 = Cross(p1, q1, p0);
107 float s3 = Cross(p0, q1, q0);
108 float s4 = Cross(q1, p1, q0);
109
110 if (!(s1 * s2 > 0 && s3 * s4 > 0)) return 0;
111
112 // Calculate Intersection of two lines.
113 float s5 = Cross(q1, p1, p0);
114 if (fabs(s5 - s1) > EPS) {
115 ans.x_ = (s5 * q0.x_ - s1 * q1.x_) / (s5 - s1);
116 ans.y_ = (s5 * q0.y_ - s1 * q1.y_) / (s5 - s1);
117
118 } else {
119 float a0 = p0.y_ - p1.y_, b0 = p1.x_ - p0.x_,
120 c0 = p0.x_ * p1.y_ - p1.x_ * p0.y_;
121 float a1 = q0.y_ - q1.y_, b1 = q1.x_ - q0.x_,
122 c1 = q0.x_ * q1.y_ - q1.x_ * q0.y_;
123 float D = a0 * b1 - a1 * b0;
124
125 ans.x_ = (b0 * c1 - b1 * c0) / D;
126 ans.y_ = (a1 * c0 - a0 * c1) / D;
127 }
128
129 return 1;
130}
131
133 const float angle_cos,
134 const float angle_sin,
135 Point &p) {
136 float new_x = (p.x_ - center.x_) * angle_cos +
137 (p.y_ - center.y_) * angle_sin + center.x_;
138 float new_y = -(p.x_ - center.x_) * angle_sin +
139 (p.y_ - center.y_) * angle_cos + center.y_;
140 p.set(new_x, new_y);
141}
142
144 const Point &b,
145 const Point &center) {
146 return atan2(a.y_ - center.y_, a.x_ - center.x_) >
147 atan2(b.y_ - center.y_, b.x_ - center.x_);
148}
149
150OPEN3D_HOST_DEVICE inline float BoxOverlap(const float *box_a,
151 const float *box_b) {
152 // box_a (5) [x1, y1, x2, y2, angle].
153 // box_b (5) [x1, y1, x2, y2, angle].
154 float a_x1 = box_a[0], a_y1 = box_a[1], a_x2 = box_a[2], a_y2 = box_a[3],
155 a_angle = box_a[4];
156 float b_x1 = box_b[0], b_y1 = box_b[1], b_x2 = box_b[2], b_y2 = box_b[3],
157 b_angle = box_b[4];
158
159 Point center_a((a_x1 + a_x2) / 2, (a_y1 + a_y2) / 2);
160 Point center_b((b_x1 + b_x2) / 2, (b_y1 + b_y2) / 2);
161
162 Point box_a_corners[5];
163 box_a_corners[0].set(a_x1, a_y1);
164 box_a_corners[1].set(a_x2, a_y1);
165 box_a_corners[2].set(a_x2, a_y2);
166 box_a_corners[3].set(a_x1, a_y2);
167
168 Point box_b_corners[5];
169 box_b_corners[0].set(b_x1, b_y1);
170 box_b_corners[1].set(b_x2, b_y1);
171 box_b_corners[2].set(b_x2, b_y2);
172 box_b_corners[3].set(b_x1, b_y2);
173
174 // Get oriented corners.
175 float a_angle_cos = cos(a_angle), a_angle_sin = sin(a_angle);
176 float b_angle_cos = cos(b_angle), b_angle_sin = sin(b_angle);
177
178 for (int k = 0; k < 4; k++) {
179 RotateAroundCenter(center_a, a_angle_cos, a_angle_sin,
180 box_a_corners[k]);
181 RotateAroundCenter(center_b, b_angle_cos, b_angle_sin,
182 box_b_corners[k]);
183 }
184
185 box_a_corners[4] = box_a_corners[0];
186 box_b_corners[4] = box_b_corners[0];
187
188 // Get Intersection of lines.
189 Point cross_points[16];
190 Point poly_center;
191 int cnt = 0, flag = 0;
192
193 poly_center.set(0, 0);
194 for (int i = 0; i < 4; i++) {
195 for (int j = 0; j < 4; j++) {
196 flag = Intersection(box_a_corners[i + 1], box_a_corners[i],
197 box_b_corners[j + 1], box_b_corners[j],
198 cross_points[cnt]);
199 if (flag) {
200 poly_center = poly_center + cross_points[cnt];
201 cnt++;
202 }
203 }
204 }
205
206 // Check corners.
207 for (int k = 0; k < 4; k++) {
208 if (CheckInBox2D(box_a, box_b_corners[k])) {
209 poly_center = poly_center + box_b_corners[k];
210 cross_points[cnt] = box_b_corners[k];
211 cnt++;
212 }
213 if (CheckInBox2D(box_b, box_a_corners[k])) {
214 poly_center = poly_center + box_a_corners[k];
215 cross_points[cnt] = box_a_corners[k];
216 cnt++;
217 }
218 }
219
220 OPEN3D_ASSERT(cnt != 0 && "Invalid value: cnt==0.");
221
222 poly_center.x_ /= cnt;
223 poly_center.y_ /= cnt;
224
225 // Sort the points of polygon.
226 Point temp;
227 for (int j = 0; j < cnt - 1; j++) {
228 for (int i = 0; i < cnt - j - 1; i++) {
229 if (PointCmp(cross_points[i], cross_points[i + 1], poly_center)) {
230 temp = cross_points[i];
231 cross_points[i] = cross_points[i + 1];
232 cross_points[i + 1] = temp;
233 }
234 }
235 }
236
237 // Get the overlap areas.
238 float area = 0;
239 for (int k = 0; k < cnt - 1; k++) {
240 area += Cross(cross_points[k] - cross_points[0],
241 cross_points[k + 1] - cross_points[0]);
242 }
243
244 return static_cast<float>(fabs(area)) / 2.0f;
245}
246
249 const float *box_a,
250 const float *box_b,
251 bool intersection_only = false) {
252 // params: box_a (5) [x1, y1, x2, y2, angle].
253 // params: box_b (5) [x1, y1, x2, y2, angle].
254 float sa = (box_a[2] - box_a[0]) * (box_a[3] - box_a[1]);
255 float sb = (box_b[2] - box_b[0]) * (box_b[3] - box_b[1]);
256 float s_overlap = BoxOverlap(box_a, box_b);
257 if (intersection_only) {
258 return s_overlap;
259 } else {
260 return s_overlap / fmaxf(sa + sb - s_overlap, EPS);
261 }
262}
263
266 const float *box_a,
267 const float *box_b,
268 bool intersection_only = false) {
269 float box_a_new[5];
270 box_a_new[0] = box_a[0] - box_a[2] / 2;
271 box_a_new[1] = box_a[1] - box_a[3] / 2;
272 box_a_new[2] = box_a[0] + box_a[2] / 2;
273 box_a_new[3] = box_a[1] + box_a[3] / 2;
274 box_a_new[4] = box_a[4];
275
276 float box_b_new[5];
277 box_b_new[0] = box_b[0] - box_b[2] / 2;
278 box_b_new[1] = box_b[1] - box_b[3] / 2;
279 box_b_new[2] = box_b[0] + box_b[2] / 2;
280 box_b_new[3] = box_b[1] + box_b[3] / 2;
281 box_b_new[4] = box_b[4];
282 return IoUBev2DWithMinAndMax(box_a_new, box_b_new, intersection_only);
283}
284
286OPEN3D_HOST_DEVICE inline float IoU3DWithCenterAndSize(const float *box_a,
287 const float *box_b) {
288 float box_a_2d[5];
289 box_a_2d[0] = box_a[0];
290 box_a_2d[1] = box_a[2];
291 box_a_2d[2] = box_a[3];
292 box_a_2d[3] = box_a[5];
293 box_a_2d[4] = box_a[6];
294
295 float box_b_2d[5];
296 box_b_2d[0] = box_b[0];
297 box_b_2d[1] = box_b[2];
298 box_b_2d[2] = box_b[3];
299 box_b_2d[3] = box_b[5];
300 box_b_2d[4] = box_b[6];
301 float intersection_2d = IoUBev2DWithCenterAndSize(box_a_2d, box_b_2d, true);
302
303 float y_a_min = box_a[1] - box_a[4];
304 float y_a_max = box_a[1];
305 float y_b_min = box_b[1] - box_b[4];
306 float y_b_max = box_b[1];
307 float iw = (y_a_max < y_b_max ? y_a_max : y_b_max) -
308 (y_a_min > y_b_min ? y_a_min : y_b_min);
309 float iou_3d = 0;
310 if (iw > 0) {
311 float intersection_3d = intersection_2d * iw;
312 float volume_a = box_a[3] * box_a[4] * box_a[5];
313 float volume_b = box_b[3] * box_b[4] * box_b[5];
314 float union_3d = volume_a + volume_b - intersection_3d;
315 iou_3d = intersection_3d / union_3d;
316 }
317 return iou_3d;
318}
319
320} // namespace contrib
321} // namespace ml
322} // namespace open3d
Common CUDA utilities.
#define OPEN3D_HOST_DEVICE
Definition: CUDAUtils.h:63
#define OPEN3D_ASSERT(...)
Definition: Macro.h:67
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:362
constexpr int NMS_BLOCK_SIZE
Definition: IoUImpl.h:38
OPEN3D_HOST_DEVICE float BoxOverlap(const float *box_a, const float *box_b)
Definition: IoUImpl.h:150
OPEN3D_HOST_DEVICE int Intersection(const Point &p1, const Point &p0, const Point &q1, const Point &q0, Point &ans)
Definition: IoUImpl.h:96
constexpr float EPS
Definition: IoUImpl.h:39
OPEN3D_HOST_DEVICE void RotateAroundCenter(const Point &center, const float angle_cos, const float angle_sin, Point &p)
Definition: IoUImpl.h:132
OPEN3D_HOST_DEVICE int CheckRectCross(const Point &p1, const Point &p2, const Point &q1, const Point &q2)
Definition: IoUImpl.h:69
OPEN3D_HOST_DEVICE int PointCmp(const Point &a, const Point &b, const Point &center)
Definition: IoUImpl.h:143
OPEN3D_HOST_DEVICE float Cross(const Point &a, const Point &b)
Definition: IoUImpl.h:58
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:265
OPEN3D_HOST_DEVICE int CheckInBox2D(const float *box, const Point &p)
Definition: IoUImpl.h:80
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:248
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:286
Definition: PinholeCameraIntrinsic.cpp:35
Definition: IoUImpl.h:41
OPEN3D_HOST_DEVICE Point operator+(const Point &b) const
Definition: IoUImpl.h:48
OPEN3D_HOST_DEVICE Point operator-(const Point &b) const
Definition: IoUImpl.h:51
float x_
Definition: IoUImpl.h:54
float y_
Definition: IoUImpl.h:55
OPEN3D_HOST_DEVICE Point()
Definition: IoUImpl.h:42
OPEN3D_HOST_DEVICE void set(float x, float y)
Definition: IoUImpl.h:44
OPEN3D_HOST_DEVICE Point(float x, float y)
Definition: IoUImpl.h:43