-
Notifications
You must be signed in to change notification settings - Fork 622
/
geometry_utils.py
145 lines (112 loc) · 5.32 KB
/
geometry_utils.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
# nuScenes dev-kit.
# Code written by Oscar Beijbom and Alex Lang, 2018.
from enum import IntEnum
from typing import Tuple
import numpy as np
from pyquaternion import Quaternion
class BoxVisibility(IntEnum):
""" Enumerates the various level of box visibility in an image """
ALL = 0 # Requires all corners are inside the image.
ANY = 1 # Requires at least one corner visible in the image.
NONE = 2 # Requires no corners to be inside, i.e. box can be fully outside the image.
def view_points(points: np.ndarray, view: np.ndarray, normalize: bool) -> np.ndarray:
"""
This is a helper class that maps 3d points to a 2d plane. It can be used to implement both perspective and
orthographic projections. It first applies the dot product between the points and the view. By convention,
the view should be such that the data is projected onto the first 2 axis. It then optionally applies a
normalization along the third dimension.
For a perspective projection the view should be a 3x3 camera matrix, and normalize=True
For an orthographic projection with translation the view is a 3x4 matrix and normalize=False
For an orthographic projection without translation the view is a 3x3 matrix (optionally 3x4 with last columns
all zeros) and normalize=False
:param points: <np.float32: 3, n> Matrix of points, where each point (x, y, z) is along each column.
:param view: <np.float32: n, n>. Defines an arbitrary projection (n <= 4).
The projection should be such that the corners are projected onto the first 2 axis.
:param normalize: Whether to normalize the remaining coordinate (along the third axis).
:return: <np.float32: 3, n>. Mapped point. If normalize=False, the third coordinate is the height.
"""
assert view.shape[0] <= 4
assert view.shape[1] <= 4
assert points.shape[0] == 3
viewpad = np.eye(4)
viewpad[:view.shape[0], :view.shape[1]] = view
nbr_points = points.shape[1]
# Do operation in homogenous coordinates.
points = np.concatenate((points, np.ones((1, nbr_points))))
points = np.dot(viewpad, points)
points = points[:3, :]
if normalize:
points = points / points[2:3, :].repeat(3, 0).reshape(3, nbr_points)
return points
def box_in_image(box, intrinsic: np.ndarray, imsize: Tuple[int, int], vis_level: int = BoxVisibility.ANY) -> bool:
"""
Check if a box is visible inside an image without accounting for occlusions.
:param box: The box to be checked.
:param intrinsic: <float: 3, 3>. Intrinsic camera matrix.
:param imsize: (width, height).
:param vis_level: One of the enumerations of <BoxVisibility>.
:return True if visibility condition is satisfied.
"""
corners_3d = box.corners()
corners_img = view_points(corners_3d, intrinsic, normalize=True)[:2, :]
visible = np.logical_and(corners_img[0, :] > 0, corners_img[0, :] < imsize[0])
visible = np.logical_and(visible, corners_img[1, :] < imsize[1])
visible = np.logical_and(visible, corners_img[1, :] > 0)
visible = np.logical_and(visible, corners_3d[2, :] > 1)
in_front = corners_3d[2, :] > 0.1 # True if a corner is at least 0.1 meter in front of the camera.
if vis_level == BoxVisibility.ALL:
return all(visible) and all(in_front)
elif vis_level == BoxVisibility.ANY:
return any(visible) and all(in_front)
elif vis_level == BoxVisibility.NONE:
return True
else:
raise ValueError("vis_level: {} not valid".format(vis_level))
def transform_matrix(translation: np.ndarray = np.array([0, 0, 0]),
rotation: Quaternion = Quaternion([1, 0, 0, 0]),
inverse: bool = False) -> np.ndarray:
"""
Convert pose to transformation matrix.
:param translation: <np.float32: 3>. Translation in x, y, z.
:param rotation: Rotation in quaternions (w ri rj rk).
:param inverse: Whether to compute inverse transform matrix.
:return: <np.float32: 4, 4>. Transformation matrix.
"""
tm = np.eye(4)
if inverse:
rot_inv = rotation.rotation_matrix.T
trans = np.transpose(-np.array(translation))
tm[:3, :3] = rot_inv
tm[:3, 3] = rot_inv.dot(trans)
else:
tm[:3, :3] = rotation.rotation_matrix
tm[:3, 3] = np.transpose(np.array(translation))
return tm
def points_in_box(box: 'Box', points: np.ndarray, wlh_factor: float = 1.0):
"""
Checks whether points are inside the box.
Picks one corner as reference (p1) and computes the vector to a target point (v).
Then for each of the 3 axes, project v onto the axis and compare the length.
Inspired by: https://math.stackexchange.com/a/1552579
:param box: <Box>.
:param points: <np.float: 3, n>.
:param wlh_factor: Inflates or deflates the box.
:return: <np.bool: n, >.
"""
corners = box.corners(wlh_factor=wlh_factor)
p1 = corners[:, 0]
p_x = corners[:, 4]
p_y = corners[:, 1]
p_z = corners[:, 3]
i = p_x - p1
j = p_y - p1
k = p_z - p1
v = points - p1.reshape((-1, 1))
iv = np.dot(i, v)
jv = np.dot(j, v)
kv = np.dot(k, v)
mask_x = np.logical_and(0 <= iv, iv <= np.dot(i, i))
mask_y = np.logical_and(0 <= jv, jv <= np.dot(j, j))
mask_z = np.logical_and(0 <= kv, kv <= np.dot(k, k))
mask = np.logical_and(np.logical_and(mask_x, mask_y), mask_z)
return mask