-
Notifications
You must be signed in to change notification settings - Fork 0
/
planarTagTest.py
164 lines (148 loc) · 3.79 KB
/
planarTagTest.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
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
if __name__ != "__main__":
raise RuntimeError("Run this as a script")
from joy import JoyApp, progress
from joy.plans import AnimatorPlan
from joy.decl import KEYDOWN
from numpy import asarray,zeros_like,kron,concatenate,array,mean,dot,angle,pi,c_
from numpy.linalg import svd
from json import loads as json_loads
from sys import stdout
from socket import socket, AF_INET, SOCK_DGRAM, error as SocketError
try:
s.close()
except Exception:
pass
def skew( v ):
"""
Convert a 3-vector to a skew matrix such that
dot(skew(x),y) = cross(x,y)
The function is vectorized, such that:
INPUT:
v -- N... x 3 -- input vectors
OUTPUT:
N... x 3 x 3
For example:
>>> skew([[1,2,3],[0,0,1]])
array([[[ 0, 3, -2],
[-3, 0, 1],
[ 2, -1, 0]],
<BLANKLINE>
[[ 0, 1, 0],
[-1, 0, 0],
[ 0, 0, 0]]])
"""
v = asarray(v).T
z = zeros_like(v[0,...])
return array([
[ z, -v[2,...], v[1,...]],
[v[2,...], z, -v[0,...] ],
[-v[1,...], v[0,...], z ] ]).T
def fitHomography( x, y ):
"""Fit a homography mapping points x to points y"""
x = asarray(x)
assert x.shape == (len(x),3)
y = asarray(y)
assert y.shape == (len(y),3)
S = skew(y)
plan = [ kron(s,xi) for s,xi in zip(S,x) ]
#plan.append([[0]*8+[1]])
A = concatenate( plan, axis=0 )
U,s,V = svd(A)
res = V[-1,:].reshape(3,3)
return res.T
def _animation(fig):
global s
s = socket(AF_INET, SOCK_DGRAM )
s.bind(("",0xB00))
s.setblocking(0)
lst = []
msg = None
rh={}
i=0
# Corners of the arena, in order
corners = [26,23,27,22,29,24,28,25]
ref = array([
[-1,0,1,1,1,0,-1,-1],
[1,1,1,0,-1,-1,-1,0],
[1.0/100]*8]).T * 100
ax = array([
min(ref[:,0]),max(ref[:,0]),
min(ref[:,1]),max(ref[:,1])
])*1.2
allow = set(corners + [12,14,13,15])
fr = 0
while True: #number of samples
try:
while True:
# read data as fast as possible
msg = s.recv(1<<16)
except SocketError, se:
# until we've run out; last message remains in m
pass
# make sure we got something
if not msg:
yield
continue
# Parse tag information from UDP packet
dat = json_loads(msg)
# Make sure there are enough tags to make sense
if len(dat)<5:
yield
continue
# Collect allowed tags
c = {}
h = {}
acc = []
for d in dat:
nm = d['i']
if not nm in allow:
continue
p = asarray(d['p'])/100
c[nm] = mean(p,0)
h[nm] = p
acc.append(str(nm))
# Collect the corner tags
try:
roi = array( [ [c[nm][0], c[nm][1], 1] for nm in corners ] )
except KeyError, ck:
progress( "-- missing corner %d" % ck)
yield
continue
progress(",".join(acc))
# Homography mapping roi to ref
prj = fitHomography( roi, ref )
mrk = dot(roi,prj)
mrk = mrk[:,:2] / mrk[:,[-1]]
print
# display it
fig.clf()
fa = fig.gca()
# Mark the corner tags
fa.plot( mrk[:,0],mrk[:,1],'sc',ms=15)
ang0 = [-1+1j,1+1j,1-1j,-1-1j]
# Loop on all tags
for nm,p in h.iteritems():
# Project back
a = dot(c_[p,[1]*len(p)],prj)
a = a[:,:2]/a[:,[-1]]
# Compute position
z = a[:,0]+1j*a[:,1]
mz = mean(z)
# Compute angle
ang = angle(mean((z-mz) / ang0))
fa.plot( z[[0,1,2,3,0]].real, z[[0,1,2,3,0]].imag, '.-b' )
fa.plot( [z[0].real], [z[0].imag], 'og' )
fa.text( mean(a[:,0]), mean(a[:,1]),
"[%d] \n (%3d,%3d) %.0f" % (nm,mz.real,mz.imag,180/pi*ang),
ha='center',va='center' )
fa.axis(ax)
#
yield
class App(JoyApp):
def onStart(self):
AnimatorPlan(self,_animation).start()
def onEvent(self,evt):
if evt.type == KEYDOWN:
return JoyApp.onEvent(self,evt)
app = App()
app.run()