forked from dblanchet/Bug-Arena
-
Notifications
You must be signed in to change notification settings - Fork 0
/
kinect.py
281 lines (223 loc) · 8.4 KB
/
kinect.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
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
"""
Kinect.py
Interface to the kinect hardware.
"""
from collections import namedtuple
import numpy
try:
import freenect
except ImportError:
freenect = None
print "Kinect module not found. Faking it"
__all__ = ['get_buffers',
'set_default_data',
'z_to_cm',
'x_to_cm',
'y_to_cm',
'extract_obstacles',
'get_obstacles',
'UNDEF_DEPTH',
'UNDEF_DISTANCE',
'_MIN_DISTANCE',
'_MAX_DISTANCE']
_DEFAULT_ANALYSIS_BAND = (37, 196, 566, 85)
_DEFAULT_SURFACE = (-9999, -9999, 9999, 9999)
UNDEF_DEPTH = _UNDEF_DEPTH = 2047
UNDEF_DISTANCE = _UNDEF_DISTANCE = 2000.0
# Look up table for depth calculations
# Formula from http://vvvv.org/forum/the-kinect-thread.
_dist_values = numpy.tan(numpy.arange(2048) / 1024.0 + 0.5) * 33.825 + 5.7
# XBox 360 Kinect is said to be OK with
# depth values between 80 cm and 4 meters.
# saturate all inputs/outputs to those values
_MIN_DISTANCE = 80.0 # cm
_MAX_DISTANCE = 400.0 # cm
_DIST_ARRAY = numpy.where(
_MIN_DISTANCE < _dist_values,
_dist_values,
_UNDEF_DISTANCE)
_DIST_ARRAY = numpy.where(
_DIST_ARRAY < _MAX_DISTANCE,
_DIST_ARRAY,
_UNDEF_DISTANCE)
# ----------------------------------------------
# Returned by get_buffers
KinectData = namedtuple('KinectData', 'real_kinect rgb depth')
def get_buffers():
'''get_buffers(): returns a KinectData object
KinectData members:
- real_kinect (boolean) (true if data comes fro ma real kinect)
- rgb array
- depth array
(buffers=numpy array)
the input is taken from a file if the kinect is missing or the library not
present. No memorization is done.
'''
found_kinect = False
if freenect: # module has been imported
try:
# Try to obtain Kinect images.
(depth, _), (rgb, _) = \
freenect.sync_get_depth(), freenect.sync_get_video()
found_kinect = True
except TypeError:
pass
if found_kinect:
return KinectData(real_kinect=True, rgb=rgb, depth=depth)
else:
# Use local data files. not defined if not initialized
return _DEFAULT_DATA
def set_default_data(filename):
'''Sets default fake input file to use, without extension
ex: 2012-03-02_14-36-48'''
global _DEFAULT_FILE, _DEFAULT_DATA
print "loaded fake data %s" % filename
_DEFAULT_FILE = filename
_DEFAULT_DATA = KinectData(
real_kinect=False,
rgb=numpy.load(filename + '_rgb.npy'),
depth=numpy.load(filename + '_depth.npy')
)
def z_to_cm(depth):
"from a depth (or depth buffer), convert to depth in centimeters"
return _DIST_ARRAY[depth]
def x_to_cm(x, z):
"from a depth in cm and x, converts to x in centimeters"
coeff = 0.001734 # Measured constant.
return (320.0 - x) * z * coeff
def y_to_cm(y, z):
'from a depth in cm and y, converts to y height in centimeters'
coeff = 0.001734 # Measured constant.
dev = 9 / coeff / 200 # Horizon is not at y = 0.
h = 6.0 # Kinect captor is not at y = 0.
return ((480.0 - y) - 240.0 - dev) * z * coeff + h
# Returned by analyzer object.
#
# bounds Rectangle that contains the obstacle. Tuple (x, y, w, h) (y au
# sens Z)
# min_height Minimal y value detected in the obstacle. Int
# raw_data Detected data. Numpy Array
_Obstacle = namedtuple('Obstacle', 'x y width height z raw_data')
class Obstacle (_Obstacle):
def __str__(self):
return "Obstacle at (%.1f,%.1f) size : (%.1fx%.1f), height:%.1f %s" % (
self.x, self.y,
self.width, self.height,
self.z,
"(has raw data)" if self.raw_data else '(no raw data)'
)
# patch the class ...
#Obstacle.__str__ = show_obstacle
def extract_obstacles(
depth,
band=_DEFAULT_ANALYSIS_BAND,
surface=_DEFAULT_SURFACE,
provide_raw=False):
'''Returns obstacles from pixel depth
extract_obstacles(depth, band=..., surface=..., provide_raw=False):
depth: depth array
band: an optional analysis band in pixels (x, y, w, h) and
surface: an optional analysis band in cm within the game area
(x, z, w, p) - in top view, z is depth
provide_raw : whether to provide raw data in the returned object or
None
returns a list of Obstacles objects
Obstacle objects members:
x:
y:
width:
height:
coordinates of the bounding rectangle in top view *in
centimeters* (0,0) : center in front of kinect
z: minimal height of the rectangle from the ground 0 => on the
ground
raw_data: the raw data for analysis (x,y in pixels, z in cm)
'''
MAX_DEPTH = 300.0 # 3 meters. FIXME Depends on Gaming Zone size.
MAX_BORDER_HEIGHT = 5 # cm. a foot can never be higher than this.
# Restrict accordingly.
MAX_Z_CHANGE = 10 # cm. consider discutinued foot if Z varies this much or
# more
dist = z_to_cm(depth)
# -- Extract borders (lower Y where Z is in range)
bx, by, bw, bh = band
borders = [] # list of (x, ymax, z@ymax) of non-empty columns. ymax :
# max Y where z is not null
# x,y in pixels ; z in cm
zone = dist[by:by + bh, bx:bx + bw] # extract zone from which Data
# is considered
# ymax: for each x: maximum Y for the given X
# on the zone where Z is in range
for x in xrange(zone.shape[1]):
non_null_y = numpy.argwhere(zone[:, x] <= MAX_DEPTH) # y in range ?
if non_null_y.size: # is there any z in the range ?
ymax = numpy.max(non_null_y)
# split to new if discontinuity (y ou z) ?
borders.append((bx + x, by + ymax, zone[ymax, x]))
# else: new foot
# -- Analysis :
# Analyze from the borders array the list of feets
feet = [] # foot : (x,y,z) points in the foot, one per X
x, _, z = borders[0] # initialization
prev_x = x
prev_z = z
foot = [] # current foot
for x, y, z in borders:
# Separate disconnected feet.
# connected foot : contiguous X and not too abrupt z change
if x - prev_x <= 1 and abs(prev_z - z) < MAX_Z_CHANGE:
foot.append((x, y, z))
else:
feet.append(foot)
foot = [(x, y, z)]
prev_x = x
prev_z = z
if foot:
feet.append(foot)
# Limit zone height : distance between base and top must be restricted.
# shrink foot accordingly (...)
# put back results to feet
result = []
for foot in feet:
m = min(y_to_cm(y, z) for x, y, z in foot) # bas du pied actuel,
# including corresponding z
result.append([(x, y, z) for x, y, z in foot
if y_to_cm(y, z) - m <= MAX_BORDER_HEIGHT])
feet = result
final = []
for foot in feet:
left = min(x_to_cm(x, z) for x, y, z in foot)
right = max(x_to_cm(x, z) for x, y, z in foot)
# z is already in cm
close = min(z for x, y, z in foot)
far = max(z for x, y, z in foot)
bottom = min(y_to_cm(y, z) for x, y, z in foot)
# swap coordinates Y and Z here (y was height,
# becomes depth ; invert for z)
final.append(Obstacle(
x=left,
y=close,
width=right - left,
height=far - close,
z=min(y for x, y, z in foot),
raw_data=foot if provide_raw else None
))
return final
def get_obstacles(provide_raw=False):
"""Get buffers from the Kinect and extract obstacles.
See extract_obstacles for obstacle definition."""
k = get_buffers()
if not k.real_kinect:
print "Using Fake Data..."
return extract_obstacles(k.depth, provide_raw=provide_raw)
set_default_data('data/2012-03-02_14-36-48')
if __name__ == '__main__':
"test the library, don't execute if imported"
for f in ('2012-03-02_14-36-48',
'2012-03-23_12-55-38',
'2012-03-23_13-21-48',
'2012-03-30_14-14-43'):
set_default_data('data/' + f)
print 'Testing Kinect library ...'
for foot in get_obstacles():
print foot