/
camera_to_threedize.py
75 lines (68 loc) · 3.67 KB
/
camera_to_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
import threedize as ddd
import numpy.matlib as np
import numpy.linalg as npl
# Prepare data from the camera for use with threedize.threedize_phi_angles
def threedize_phi_angles(data, ref_half_plane, view, cameraposor, laserpos, lasertheta):
"""Using ref_half_plane, tag data and pass it to threedize.threedize_phi_angles"""
return ddd.threedize_phi_angles(tag_data(data, ref_half_plane, view, cameraposor, laserpos, lasertheta),
view, cameraposor, laserpos, lasertheta)
# x is in the half-plane if, in addition to dot(n,x)=0, we have dot(side,x)>=0
class HalfPlane(object):
def __init__(self, pos, normal, side):
self.pos = pos
self.normal = normal
self.side = side
class NoReferenceException(Exception): pass
def calc_phi(xys, ref_half_plane, view, cameraposor, laserpos, lasertheta):
"""Given an array of pixel pairs xys from a camera with view and cameraposor and
laser with laserpos and lasertheta, calculate the laser inclination based on a
known half-plane ref_half_plane. Throws a NoReferenceException if no pixels are
in the reference half-plane."""
cref_pos = ddd.unrotate(ref_half_plane.pos - cameraposor.pos, cameraposor)
cref_side = ddd.unrotate(ref_half_plane.side, cameraposor)
cref_line = np.cross(cref_side, ddd.unrotate(ref_half_plane.normal, cameraposor), axis = 0)
# TODO less copy-pasta
cpos = np.array([cref_pos[1, 0], -cref_pos[2, 0]]) / cref_pos[0, 0] * ddd.view_number(view) \
+ np.array([view.centerx, view.centery])
cline_ = cref_pos / cref_pos[0, 0] - cref_line / cref_line[0, 0]
cside_ = np.array([cref_side[1, 0], -cref_side[2, 0]])
cside = np.array([cline_[2, 0], cline_[1, 0]])
if np.dot(cside, cside_) < 0:
cside = - cside
dxys = xys - cpos
dot_products = np.array(np.mat([cside]) * np.mat(dxys).T)[0]
good_xys = xys[dot_products >= 0]
print("say "+str(np.average(good_xys[:,1])))
if len(good_xys) == 0:
raise NoReferenceException()
threepoints = ddd.threedize_plane(good_xys, view, cameraposor, ref_half_plane)
return calc_phi_points(threepoints, laserpos, lasertheta)
def calc_phi_points(points, laserpos, lasertheta):
"""Given an array of triples points that should be in the plane generated by a
laser at laserpos with theta lasertheta, calculate the inclination of the laser
plane's normal vector."""
plane_line = ddd.coord(-np.sin(lasertheta), np.cos(lasertheta), 0)
normals = np.cross(np.array(plane_line.T)[0], points - np.array(laserpos.T)[0])
return calc_phi_norm(np.average((normals.T / npl.norm(normals, axis = 1)).T, axis = 0), lasertheta)
def calc_phi_norm(norm, lasertheta):
"""Given norm, the normal vector to the plane of the laser, and lasertheta, the
angle it is rotated along z from the positive x-axis, calculate phi, the angle
of inclination of the norm."""
return np.arctan2(norm[2], norm[0]*np.cos(lasertheta) + norm[1]*np.sin(lasertheta))
def tag_data(data, ref_half_plane, view, cameraposor, laserpos, lasertheta):
"""Given a list of arrays of pixels data, return a list of tuples whose first
element is the calculated laser phi and whose second element is the associated
original data. The other parameters are passed to calc_phi. Pixel arrays that
cause NoReferenceException to be thrown are ignored."""
result = []
for xys in data:
if len(xys) == 0:
continue
try:
result.append((calc_phi(xys, ref_half_plane, view,
cameraposor, laserpos, lasertheta),
xys))
print("say "+str(result[-1][0]*180/np.pi))
except NoReferenceException:
pass
return result