def show_depth(depth=depth,name='depth'):
"""Although opencv supports numpy directly,
i.e. cv.ShowImage('depth',depth), it leaks memory. This is a workaround
"""
im = cv.CreateImage((640,480),8,3)
cv.SetData(im, np.ascontiguousarray(np.dstack(\
3*[(depth % 256).astype('u1')])).tostring())
if len(clickpts)==1:
pt1 = np.array(clickpts[0],'i4')
cv.Rectangle(im, tuple(pt1-measure.sample_side/2),
tuple(pt1+measure.sample_side/2), (255,255,0))
if len(clickpts)==2:
pt1 = np.array(clickpts[0],'i4')
pt2 = np.array(clickpts[1],'i4')
cv.Rectangle(im, tuple(pt1-measure.sample_side/2),
tuple(pt1+measure.sample_side/2), (255,0,255))
cv.Rectangle(im, tuple(pt2-measure.sample_side/2),
tuple(pt2+measure.sample_side/2), (255,0,255))
cv.Line(im, tuple(pt1),tuple(pt2), (255,0,255))
dist = measure.estimate_distance(depth, *clickpts)
cv.PutText(im, '%.3f meters' % dist,
tuple(np.minimum(pt1,pt2)-(10,10)),
cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 0.7, 0.7),
(255,255,255))
cv.ShowImage(name,im)
def run_one_file(filename,method='kmeans'):
dirname, fname = os.path.split(filename)
# Chop off the file extension, split the name into important parts
i,_,x1,y1,x2,y2,real = fname.split('.')[0].split('_')
# Parse them as ints
i,x1,y1,x2,y2,real = map(int, (i,x1,y1,x2,y2,real))
# Read the depth image
with open(os.path.join(dirname, '%d_depth.bin' % i),'rb') as f:
depth = np.fromfile(f, dtype='i2').reshape(480,640)
# Measure the distance, compare to known distance
dist = measure.estimate_distance(depth, (x1,y1), (x2,y2), method)
return dist, real/1000.0
