def depthmatch(x,y,leftimage,rightimage,roi=80,buf=50,baseline=2.7,focal_length=80):
"""depthmatch function
x,y : (int) pixel position of target in left image
leftimage, rightimage : (IplImage) stereo images
roi: (int) region of interest around x,y to use in matching
buf: (int) buffer outside of a straight horizontal search for a match
"""
#print "Match",x,y
info = cv.cvGetSize(leftimage)
width = info.width
height = info.height
centerx = width/2
centery = height/2
(y1,x1,y2,x2) = (y-roi,x-roi,y+roi,x+roi)
if y1<0: y1 = 0
if x1<0: x1 = 0
if y2>height: y2 = height
if x2>width: x2 = width
# copy subregion roi x roi
template_rect = cv.cvRect(x1,y1,(x2-x1),(y2-y1))
template = cv.cvGetSubRect(leftimage, template_rect)
#(y3,x3,y4,x4) = (y-roi-buf,x-roi-buf,y+roi+buf,width) # +/- 20 pixels in vertical direction, -20 to the right edge
(y3,x3,y4,x4) = (y-roi-buf,0,y+roi+buf,x+roi+buf) # +/- buf pixels in vertical direction, +buf to the left edge
if x3<0: x3 = 0
if y3<0: y3 = 0
if x4>=width: x4 = width-1
if y4>height: y4 = height
#cv.cvSetImageROI(rightimage, (y3,x3,y4,x4))
rightsub_rect = cv.cvRect(x3,y3,(x4-x3),(y4-y3))
rightsub = cv.cvGetSubRect(rightimage, rightsub_rect)
# result matrix should be (W - w + 1) x (H - h + 1) where WxH are template dimensions, wxh are rightsub dimensions
W = x4-x3
H = y4-y3
w = x2-x1
h = y2-y1
resy = (y4-y3)-(y2-y1)+1
resx = (x4-x3)-(x2-x1)+1
resultmat = cv.cvCreateImage((resx, resy), 32, 1)
cv.cvZero(resultmat)
# match template image in a subportion of rightimage
cv.cvMatchTemplate(rightsub, template, resultmat, cv.CV_TM_SQDIFF)
min_val, max_val, min_point, max_point = cv.cvMinMaxLoc(resultmat)
cv.cvNormalize(resultmat, resultmat, 1, 0, cv.CV_MINMAX)
depth = plane2point(x-centerx, y-centery, x3+min_point.x+roi-centerx, y3+min_point.y+roi-centery, baseline, focal_length)
#print "Found match at", min_point.x+x3, min_point.y+y3
return (depth, (x,y), (x3+min_point.x+roi, y3+min_point.y+roi))
def depthmatch(x,y,leftimage,rightimage,roi=20,buf=10,debug=False):
__doc__ = """depthmatch function
x,y : (int) pixel position of target in left image
leftimage, rightimage : (IplImage) stereo images
roi: (int) region of interest around x,y to use in matching
buf: (int) buffer outside of a straight horizontal search for a match
"""
info = cv.cvGetSize(leftimage)
width = info.width
height = info.height
(y1,x1,y2,x2) = (y-roi,x-roi,y+roi,x+roi)
#template = cv.cvCreateImage((roi*2,roi*2), 8, 3)
if y1<0: y1 = 0
if x1<0: x1 = 0
if y2>height: y2 = height
if x2>width: x2 = width
#cv.cvSetZero(template)
# copy subregion roi x roi
template_rect = cv.cvRect(x1,y1,(x2-x1),(y2-y1))
template = cv.cvGetSubRect(leftimage, template_rect)
(y3,x3,y4,x4) = (y-roi-buf,x-roi-buf,y+roi+buf,width) # +/- 20 pixels in vertical direction, -20 to the right edge
if x3<0: x3 = 0
if y3<0: y3 = 0
if x4>=width: x4 = width-1
if y4>height: y4 = height
#cv.cvSetImageROI(rightimage, (y3,x3,y4,x4))
rightsub_rect = cv.cvRect(x3,y3,(x4-x3),(y4-y3))
rightsub = cv.cvGetSubRect(rightimage, rightsub_rect)
# result matrix should be (W - w + 1) x (H - h + 1) where WxH are template dimensions, wxh are rightsub dimensions
W = x4-x3
H = y4-y3
w = x2-x1
h = y2-y1
resy = (y4-y3)-(y2-y1)+1
resx = (x4-x3)-(x2-x1)+1
resultmat = cv.cvCreateImage((resx, resy), 32, 1)
cv.cvZero(resultmat)
# match template image in a subportion of rightimage
cv.cvMatchTemplate(rightsub, template, resultmat, cv.CV_TM_SQDIFF)
min_val, max_val, min_point, max_point = cv.cvMinMaxLoc(resultmat)
cv.cvNormalize(resultmat, resultmat, 1, 0, cv.CV_MINMAX)
depth = stereo.depth(x, x3+min_point.x, max_pixels=width/2)
if debug:
print "Input image: %ix%i, target: (%i,%i)" % (width,height,x,y)
print "Template box: (%i,%i) to (%i,%i)" % (x1, y1, x2, y2)
print "Search area: (%i,%i) to (%i,%i)" % (x3, y3, x4, y4)
print "%ix%i, %ix%i" % (W,H,w,h)
print "Result matrix %ix%i" % (resx, resy)
print "stereo.depth(%i,%i,max_pixels=%i)" % (x, min_point.x+x3,width/2)
if depth[0]:
print "Depth: ", depth[0], "(cm)"
#cv.cvRectangle(rightimage, cv.cvPoint(x1,y1), cv.cvPoint(x2,y2), (255,0,0))
cv.cvRectangle(rightimage, cv.cvPoint(min_point.x+x3,min_point.y+y3), cv.cvPoint(min_point.x+x3+roi*2,min_point.y+y3+roi*2), (0,255,0))
cv.cvRectangle(rightimage, cv.cvPoint(x3,y3), cv.cvPoint(x4,y4), (0,0,255))
cv.cvRectangle(leftimage, cv.cvPoint(x1,y1), cv.cvPoint(x2,y2), (255,0,0))
#cv.cvRectangle(leftimage, cv.cvPoint(min_point.x+x3,min_point.y+y3), cv.cvPoint(min_point.x+x3+roi*2,min_point.y+y3+roi*2), (0,255,0))
cv.cvRectangle(leftimage, cv.cvPoint(x3,y3), cv.cvPoint(x4,y4), (0,0,255))
if depth[0]:
cv.cvPutText(leftimage, "%5f(cm)" % depth[0], (x1,y1), font, (255,255,255))
highgui.cvShowImage("depthmatch - template", template)
highgui.cvShowImage("depthmatch - match", resultmat)
highgui.cvShowImage("depthmatch - right", rightimage)
highgui.cvShowImage("depthmatch - left", leftimage)
