210 lines
8.5 KiB
Python
210 lines
8.5 KiB
Python
"""The Intersection Over Union (IoU) for 3D oriented bounding boxes.
|
|
|
|
Origin: https://github.com/OasisYang/Wild6D/blob/main/lib/iou.py
|
|
"""
|
|
|
|
import numpy as np
|
|
import scipy.spatial as sp
|
|
from core.utils.box import FACES, NUM_KEYPOINTS
|
|
|
|
_PLANE_THICKNESS_EPSILON = 0.000001
|
|
_POINT_IN_FRONT_OF_PLANE = 1
|
|
_POINT_ON_PLANE = 0
|
|
_POINT_BEHIND_PLANE = -1
|
|
|
|
|
|
class IoU(object):
|
|
"""General Intersection Over Union cost for Oriented 3D bounding boxes."""
|
|
|
|
def __init__(self, box1, box2):
|
|
self._box1 = box1
|
|
self._box2 = box2
|
|
self._intersection_points = []
|
|
|
|
def iou(self):
|
|
"""Computes the exact IoU using Sutherland-Hodgman algorithm."""
|
|
self._intersection_points = []
|
|
self._compute_intersection_points(self._box1, self._box2)
|
|
self._compute_intersection_points(self._box2, self._box1)
|
|
if self._intersection_points:
|
|
intersection_volume = sp.ConvexHull(self._intersection_points).volume
|
|
box1_volume = self._box1.volume
|
|
box2_volume = self._box2.volume
|
|
union_volume = box1_volume + box2_volume - intersection_volume
|
|
return intersection_volume / union_volume
|
|
else:
|
|
return 0.0
|
|
|
|
def iou_sampling(self, num_samples=10000):
|
|
"""Computes intersection over union by sampling points.
|
|
|
|
Generate n samples inside each box and check if those samples are inside
|
|
the other box. Each box has a different volume, therefore the number o
|
|
samples in box1 is estimating a different volume than box2. To address
|
|
this issue, we normalize the iou estimation based on the ratio of the
|
|
volume of the two boxes.
|
|
|
|
Args:
|
|
num_samples: Number of generated samples in each box
|
|
|
|
Returns:
|
|
IoU Estimate (float)
|
|
"""
|
|
p1 = [self._box1.sample() for _ in range(num_samples)]
|
|
p2 = [self._box2.sample() for _ in range(num_samples)]
|
|
box1_volume = self._box1.volume
|
|
box2_volume = self._box2.volume
|
|
box1_intersection_estimate = 0
|
|
box2_intersection_estimate = 0
|
|
for point in p1:
|
|
if self._box2.inside(point):
|
|
box1_intersection_estimate += 1
|
|
for point in p2:
|
|
if self._box1.inside(point):
|
|
box2_intersection_estimate += 1
|
|
# We are counting the volume of intersection twice.
|
|
intersection_volume_estimate = (
|
|
box1_volume * box1_intersection_estimate + box2_volume * box2_intersection_estimate
|
|
) / 2.0
|
|
union_volume_estimate = (box1_volume * num_samples + box2_volume * num_samples) - intersection_volume_estimate
|
|
iou_estimate = intersection_volume_estimate / union_volume_estimate
|
|
return iou_estimate
|
|
|
|
def _compute_intersection_points(self, box_src, box_template):
|
|
"""Computes the intersection of two boxes."""
|
|
# Transform the source box to be axis-aligned
|
|
inv_transform = np.linalg.inv(box_src.transformation)
|
|
box_src_axis_aligned = box_src.apply_transformation(inv_transform)
|
|
template_in_src_coord = box_template.apply_transformation(inv_transform)
|
|
for face in range(len(FACES)):
|
|
indices = FACES[face, :]
|
|
poly = [template_in_src_coord.vertices[indices[i], :] for i in range(4)]
|
|
clip = self.intersect_box_poly(box_src_axis_aligned, poly)
|
|
for point in clip:
|
|
# Transform the intersection point back to the world coordinate
|
|
point_w = np.matmul(box_src.rotation, point) + box_src.translation
|
|
self._intersection_points.append(point_w)
|
|
|
|
for point_id in range(NUM_KEYPOINTS):
|
|
v = template_in_src_coord.vertices[point_id, :]
|
|
if box_src_axis_aligned.inside(v):
|
|
point_w = np.matmul(box_src.rotation, v) + box_src.translation
|
|
self._intersection_points.append(point_w)
|
|
|
|
def intersect_box_poly(self, box, poly):
|
|
"""Clips the polygon against the faces of the axis-aligned box."""
|
|
for axis in range(3):
|
|
poly = self._clip_poly(poly, box.vertices[1, :], 1.0, axis)
|
|
poly = self._clip_poly(poly, box.vertices[8, :], -1.0, axis)
|
|
return poly
|
|
|
|
def _clip_poly(self, poly, plane, normal, axis):
|
|
"""Clips the polygon with the plane using the Sutherland-Hodgman algorithm.
|
|
|
|
See en.wikipedia.org/wiki/Sutherland-Hodgman_algorithm for the overview of
|
|
the Sutherland-Hodgman algorithm. Here we adopted a robust implementation
|
|
from "Real-Time Collision Detection", by Christer Ericson, page 370.
|
|
|
|
Args:
|
|
poly: List of 3D vertices defining the polygon.
|
|
plane: The 3D vertices of the (2D) axis-aligned plane.
|
|
normal: normal
|
|
axis: A tuple defining a 2D axis.
|
|
|
|
Returns:
|
|
List of 3D vertices of the clipped polygon.
|
|
"""
|
|
# The vertices of the clipped polygon are stored in the result list.
|
|
result = []
|
|
if len(poly) <= 1:
|
|
return result
|
|
|
|
# polygon is fully located on clipping plane
|
|
poly_in_plane = True
|
|
|
|
# Test all the edges in the polygon against the clipping plane.
|
|
for i, current_poly_point in enumerate(poly):
|
|
prev_poly_point = poly[(i + len(poly) - 1) % len(poly)]
|
|
d1 = self._classify_point_to_plane(prev_poly_point, plane, normal, axis)
|
|
d2 = self._classify_point_to_plane(current_poly_point, plane, normal, axis)
|
|
if d2 == _POINT_BEHIND_PLANE:
|
|
poly_in_plane = False
|
|
if d1 == _POINT_IN_FRONT_OF_PLANE:
|
|
intersection = self._intersect(plane, prev_poly_point, current_poly_point, axis)
|
|
result.append(intersection)
|
|
elif d1 == _POINT_ON_PLANE:
|
|
if not result or (not np.array_equal(result[-1], prev_poly_point)):
|
|
result.append(prev_poly_point)
|
|
elif d2 == _POINT_IN_FRONT_OF_PLANE:
|
|
poly_in_plane = False
|
|
if d1 == _POINT_BEHIND_PLANE:
|
|
intersection = self._intersect(plane, prev_poly_point, current_poly_point, axis)
|
|
result.append(intersection)
|
|
elif d1 == _POINT_ON_PLANE:
|
|
if not result or (not np.array_equal(result[-1], prev_poly_point)):
|
|
result.append(prev_poly_point)
|
|
|
|
result.append(current_poly_point)
|
|
else:
|
|
if d1 != _POINT_ON_PLANE:
|
|
result.append(current_poly_point)
|
|
|
|
if poly_in_plane:
|
|
return poly
|
|
else:
|
|
return result
|
|
|
|
def _intersect(self, plane, prev_point, current_point, axis):
|
|
"""Computes the intersection of a line with an axis-aligned plane.
|
|
|
|
Args:
|
|
plane: Formulated as two 3D points on the plane.
|
|
prev_point: The point on the edge of the line.
|
|
current_point: The other end of the line.
|
|
axis: A tuple defining a 2D axis.
|
|
|
|
Returns:
|
|
A 3D point intersection of the poly edge with the plane.
|
|
"""
|
|
alpha = (current_point[axis] - plane[axis]) / (current_point[axis] - prev_point[axis])
|
|
# Compute the intersecting points using linear interpolation (lerp)
|
|
intersection_point = alpha * prev_point + (1.0 - alpha) * current_point
|
|
return intersection_point
|
|
|
|
def _inside(self, plane, point, axis):
|
|
"""Check whether a given point is on a 2D plane."""
|
|
# Cross products to determine the side of the plane the point lie.
|
|
x, y = axis
|
|
u = plane[0] - point
|
|
v = plane[1] - point
|
|
|
|
a = u[x] * v[y]
|
|
b = u[y] * v[x]
|
|
return a >= b
|
|
|
|
def _classify_point_to_plane(self, point, plane, normal, axis):
|
|
"""Classify position of a point w.r.t the given plane.
|
|
|
|
See Real-Time Collision Detection, by Christer Ericson, page 364.
|
|
|
|
Args:
|
|
point: 3x1 vector indicating the point
|
|
plane: 3x1 vector indicating a point on the plane
|
|
normal: scalar (+1, or -1) indicating the normal to the vector
|
|
axis: scalar (0, 1, or 2) indicating the xyz axis
|
|
|
|
Returns:
|
|
Side: which side of the plane the point is located.
|
|
"""
|
|
signed_distance = normal * (point[axis] - plane[axis])
|
|
if signed_distance > _PLANE_THICKNESS_EPSILON:
|
|
return _POINT_IN_FRONT_OF_PLANE
|
|
elif signed_distance < -_PLANE_THICKNESS_EPSILON:
|
|
return _POINT_BEHIND_PLANE
|
|
else:
|
|
return _POINT_ON_PLANE
|
|
|
|
@property
|
|
def intersection_points(self):
|
|
return self._intersection_points
|