def depthmatrix(leftimage, rightimage, precision=4, mask=0):
"""Returns a 3-channel 32bit floating-point distance matrix. Channels 1,2,3 = x,y,z coordinates of that point.
Precision is the number of times to downsample mask. Downsample is the number of loops to
go through with successively smaller match areas. If mask is set, only pixels in the mask are set."""
info = cv.cvGetSize(leftimage)
width = info.width
height = info.height
precision_pixels = (2**precision)
downsampled_size = cv.cvSize(width/precision_pixels, height/precision_pixels)
print "Precision of", downsampled_size.width, downsampled_size.height, "px"
if mask:
downsampled_mask = cv.cvCreateImage(downsampled_size, 8, 1)
cv.cvResize(mask, downsampled_mask)
matx = cv.cvCreateImage(downsampled_size, 8, 1)
maty = cv.cvCreateImage(downsampled_size, 8, 1)
matz = cv.cvCreateImage(downsampled_size, 8, 1)
for i in xrange(width/precision_pixels):
for j in xrange(height/precision_pixels):
if mask:
if (not cv.cvGetReal2D(downsampled_mask, j, i)):
continue
x = i*precision
y = j*precision
depth = depthmatch(x+precision_pixels/2, y+precision_pixels/2, leftimage, rightimage, roi=precision_pixels, buf=precision_pixels*2)
#print i, j
# fill in result matrix if mask wasn't 0 at this point (X,Y,Z)
cv.cvSetReal2D(matx, j, i, int(depth[0][0]))
cv.cvSetReal2D(maty, j, i, int(depth[0][1]))
cv.cvSetReal2D(matz, j, i, int(depth[0][2]))
return matz
(0, scaleheight * val_cutoff / sample_pixels),
(v_bins * scalewidth, scaleheight * val_cutoff / sample_pixels),
(255, 0, 0),
1,
)
highgui.cvShowImage("Histogram - Value", hist_val_img)
# classify objects
cv.cvZero(output_mask)
for x in xrange(size.width):
for y in xrange(size.height):
hue = cv.cvGetReal2D(img_h, y, x)
hue_bin = math.ceil(hue * h_bins / h_limit) - 1
if hue_bin < 0:
hue_bin = 0
# print hue_bin
if cv.cvGetReal1D(h_hue.bins, int(hue_bin)) < hue_cutoff:
cv.cvSetReal2D(output_mask, y, x, 255)
continue
val = cv.cvGetReal2D(img_v, y, x)
val_bin = math.ceil(val * v_bins / v_limit) - 1
if val_bin < 0:
val_bin = 0
if cv.cvGetReal1D(h_val.bins, int(val_bin)) < val_cutoff:
cv.cvSetReal2D(output_mask, y, x, 255)
continue
# highgui.cvWaitKey(1)
highgui.cvShowImage("Obstacles", output_mask)
highgui.cvWaitKey(10)