00001 import time
00002 import itertools
00003
00004 import cv
00005 import numpy
00006
00007 def l2(x0, y0, x1, y1):
00008 return (x1 - x0) ** 2 + (y1 - y0) ** 2
00009
00010 def strongest(img):
00011 eig_image = cv.CreateMat(img.rows, img.cols, cv.CV_32FC1)
00012 temp_image = cv.CreateMat(img.rows, img.cols, cv.CV_32FC1)
00013 return cv.GoodFeaturesToTrack(img, eig_image, temp_image, 100, 0.03, minDistance = siz / 28, useHarris = True)
00014
00015 siz = 256
00016 sizcorners = [(0,siz-1), (0,0), (siz-1,0), (siz-1,siz-1)]
00017
00018 def backf(hypH, im, found):
00019 (code,corners,pattern) = found
00020 persp = cv.CreateMat(3, 3, cv.CV_32FC1)
00021 fc = [corners[i,0] for i in range(4)]
00022 cv.GetPerspectiveTransform(fc, sizcorners, persp)
00023 cc = cv.Reshape(cv.fromarray(numpy.array(sizcorners).astype(numpy.float32)), 2)
00024 t1 = cv.CreateMat(4, 1, cv.CV_32FC2)
00025 t2 = cv.CreateMat(4, 1, cv.CV_32FC2)
00026 _persp = cv.CreateMat(3, 3, cv.CV_32FC1)
00027 cv.Invert(persp, _persp)
00028 _hypH = cv.CreateMat(3, 3, cv.CV_32FC1)
00029 cv.Invert(hypH, _hypH)
00030
00031 cv.PerspectiveTransform(cc, t1, _hypH)
00032 cv.PerspectiveTransform(t1, t2, _persp)
00033 return [t2[i,0] for i in range(4)]
00034
00035 def refinecorners(im, found):
00036 """ For a found marker, return the refined corner positions """
00037 t0 = time.time()
00038 (code,corners,pattern) = found
00039 persp = cv.CreateMat(3, 3, cv.CV_32FC1)
00040 fc = [corners[i,0] for i in range(4)]
00041 cv.GetPerspectiveTransform(fc, sizcorners, persp)
00042 cim = cv.CreateMat(siz, siz, cv.CV_8UC1)
00043 cv.WarpPerspective(im, cim, persp, flags = cv.CV_INTER_LINEAR|cv.CV_WARP_FILL_OUTLIERS, fillval = 255)
00044
00045 unit = siz / 14.
00046 hunit = unit / 2
00047 def nearest1(x, y):
00048 ix = int((x + hunit) / unit)
00049 iy = int((y + hunit) / unit)
00050 if (2 <= ix < 13) and (2 <= iy < 13):
00051 nx = int(unit * ix)
00052 ny = int(unit * iy)
00053 return (nx, ny)
00054 else:
00055 return (0,0)
00056
00057 def nearest(x, y):
00058 """ Return all grid points within sqrt(2) units of (x,y), closest first """
00059 close = []
00060 for ix in range(2, 14):
00061 for iy in range(2, 14):
00062 (nx, ny) = (unit * ix, unit * iy)
00063 d = l2(x, y, nx, ny)
00064 close.append((d, (nx, ny)))
00065 return [p for (d,p) in sorted(close) if d < 2*unit*unit]
00066
00067 corners = strongest(cim)
00068 pool = [((x,y), nearest(x, y)) for (x, y) in corners]
00069
00070 ga = dict([(x+y,((x,y),P)) for ((x,y),P) in pool])
00071 gb = dict([(x-y,((x,y),P)) for ((x,y),P) in pool])
00072 hyp = [ga[min(ga)], ga[max(ga)],
00073 gb[min(gb)], gb[max(gb)]]
00074
00075 aL = [a for (a,bs) in hyp]
00076 oldcorners = cv.fromarray(numpy.array(corners).astype(numpy.float32))
00077 oldcorners = cv.Reshape(oldcorners, 2)
00078 newcorners = cv.CreateMat(len(corners), 1, cv.CV_32FC2)
00079 best = (9999999, None)
00080 for bL in itertools.product(*[bs for (a,bs) in hyp]):
00081 hypH = cv.CreateMat(3, 3, cv.CV_32FC1)
00082 cv.GetPerspectiveTransform(aL, bL, hypH)
00083 cv.PerspectiveTransform(oldcorners, newcorners, hypH)
00084 error = 0
00085 for i in range(newcorners.rows):
00086 (x,y) = newcorners[i,0]
00087 (nx, ny) = nearest1(x, y)
00088 error += l2(x, y, nx, ny)
00089 best = min(best, (error, hypH))
00090 if error < 1000:
00091 break
00092
00093 if best[0] < 2500:
00094 pose = best[1]
00095 return backf(pose, im, found)
00096 else:
00097 return None
00098
