/
threedize.py
134 lines (116 loc) · 5.34 KB
/
threedize.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
# coding=utf-8
import numpy.matlib as np
import numpy
npfloat = numpy.float
# To use:
# - Call threedize(xs, ys, view, cameraposor, laserposor), where:
# - xs and ys are numpy arrays
# - view is created with View
# - the *posors are created with Posor(pos, theta, phi, psi)
# - pos is created with coords
class Posor(object):
def __init__(self, pos, theta, phi, psi):
self.pos = pos
self.theta = theta
self.phi = phi
self.psi = psi
class View(object):
def __init__(self, centerx, centery, dist, angle):
self.centerx = centerx
self.centery = centery
self.dist = dist
self.angle = angle
# data is a list of pairs whose first element is phi and whose second element is
# an array of x--y pairs. Return an array of x--y--z triples
def threedize_phi_angles(data, view, cameraposor, laserpos, lasertheta):
"""Call threedize on each element of data. Data is a list of pairs whose first
element is phi and whose second element is an array of x--y pairs. phi is
combined with laserpos and lasertheta to create laserposor."""
per_angles = [threedize(xys, view,
cameraposor,
Posor(laserpos, lasertheta, phi, 0))
for (phi, xys) in data]
return np.concatenate(per_angles)
# Take an array of pairs xys, along with the view, represented as an object
# with centerx, centery, dist, angle, and two objects camerapos, with
# pos, theta, phi, and psi, and laserpos, with pos, theta, and phi.
# Return an array of x--y--z triples, giving the corresponding points in
# absolute coördinates
# Laser starts out as the plane with normal y = z = 0. Orientation is created
# in the following way: Start by looking along the positive x axis, with z up.
# Rotate theta radians clockwise along the z axis, towards positive y. Rotate
# phi radians upwards along the new y axis, towards positive z. Finally, rotate
# psi radians counterclockwise along the new x axis, towards positive z.
# The transformation from camera coördinates to absolute coördinates is then
# given by the matrix product:
# [ cos th -sin th 0 ] [ cos ph 0 -sin ph ] [ 1 0 0 ]
# [ sin th cos th 0 ] [ 0 1 0 ] [ 0 cos ps -sin ps ]
# [ 0 0 1 ] [ sin ph 0 cos ph ] [ 0 sin ps cos ps ]
def threedize(xys, view, cameraposor, laserposor):
"""Calculate, as an array of x-y-z triples, the 3d points corresponding to the
pixels xys, an array of x-y pairs, as seen by a camera with view and posor
cameraposor and generated by a laser with posor laserposor."""
plane = calc_plane(laserposor)
return threedize_plane(xys, view, cameraposor, plane)
def threedize_plane(xys, view, cameraposor, plane):
"""Like threedize, but providing the laser plane explicitly instead of
calculating it from the laser posor."""
rays = calc_rays(xys, view)
rot_rays = rotate(rays, cameraposor)
threed_points = intersect(plane, cameraposor.pos, rot_rays)
return threed_points
class Plane(object):
def __init__(self, pos, normal):
self.pos = pos
self.normal = normal
# Take a posor object and return another object with point x, y, z and normal
# dx, dy, dz
def calc_plane(posor):
"""Calculate the plane associated with the laser posor"""
normal = rotate(coord(1, 0, 0), posor)
return Plane(posor.pos, normal)
# points is a matrix
def rotate(points, posor):
"""Rotate the matrix of column vectors points according to posor"""
rot_matrix = calc_rot_matrix(posor)
return rot_matrix * points
def unrotate(points, posor):
"""Rotate the matrix of column vectors points according to posor, i.e., from
absolute coordinates to camera coordinates"""
rot_matrix = calc_rot_matrix(posor)
return rot_matrix.I * points
def calc_rays(xys, view):
"""Return a matrix of column vectors of the rays corresponding to the pixels
xys, given as a list of pairs. view defines the camera. The results are in
camera coordinates."""
something = view_number(view)
cxys = xys - np.array([view.centerx, view.centery])
return np.mat([np.full(len(xys), something), cxys[:, 0], -cxys[:, 1]], dtype=npfloat)
def view_number(view):
return view.dist/np.tan(view.angle)
def calc_rot_matrix(posor):
"""Calculate the rotation matrix that takes a column vector from the camera
coordinates associated with posor to absolute coordinates"""
th = posor.theta
theta = np.mat([[np.cos(th), -np.sin(th), 0],
[np.sin(th), np.cos(th), 0],
[0, 0, 1]], dtype=npfloat)
ph = posor.phi
phi = np.mat([[np.cos(ph), 0, -np.sin(ph)],
[0, 1, 0],
[np.sin(ph), 0, np.cos(ph)]], dtype=npfloat)
ps = posor.psi
psi = np.mat([[1, 0, 0],
[0, np.cos(ps), -np.sin(ps)],
[0, np.sin(ps), np.cos(ps)]], dtype=npfloat)
return theta * phi * psi
def intersect(plane, ray_pos, rays):
"""Returns, as an array of triples, the x-y-z coordinates of the intersections
of rays originating at ray_pos with plane. Rays are given as a matrix of column
vectors"""
nt = plane.normal.T
rel = (np.array(rays) * ((nt * (plane.pos - ray_pos))[0, 0] / np.array(nt * rays)[0])).transpose()
return np.array(ray_pos.transpose())[0] + rel
def coord(*args):
"""Return a column vector with elements args"""
return np.mat([args], dtype=npfloat).T