I made a little module to have a more controlled and programmatic testing of tracking algorithms and stuff.

I could use real world data, like a video recording, but I’d like to start here. I think this is smart. I also could have used a more complicated 3d imaging package. vpython makes sense, since it is easy, but getting programmatic access to the images in unsupported somehow as far as I can tell. Now, there is no way something that might work here will necessarily transfer over to real video even after I add noise and point mismatch, but it should simplify some things. I’ve been having more trouble than makes sense to me getting good rotations off of a KLT tracker that is clearly doing a pretty bang up job.

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 |
import cv2 import numpy as np class MyCam(): def __init__(self, frameSize=(480,640), focus =600, avgPointPos=np.array([0,0,3]), sigma = .5, pointNum=300): self.pointCloud = sigma * np.random.randn(pointNum, 3) self.pointCloud = map(lambda pnt: pnt + avgPointPos, self.pointCloud) self.t = np.zeros(3) self.R = np.identity(3) self.frameSize = frameSize self.focus = focus def read(self): pnts = np.array(self.projectPoints()) frame = np.zeros(self.frameSize + (3,)) for pnt in pnts.astype(int): if pnt[0] > 0 and pnt[1] > 1 and pnt[0] < self.frameSize[0] and pnt[1] < self.frameSize[1]: cv2.circle(frame,tuple(pnt),5,[0,0,255],-1) return frame def addVecToPoint(self,points,vec): return map(lambda pnt: pnt + vec, points) def transformPoints(self): rotated = np.dot(self.pointCloud, self.R.T) translated = self.addVecToPoint(rotated, self.t) return translated def projectPoints(self): transformed = self.transformPoints() inFrontofCameraPoints = filter(lambda pnt: pnt[2] > 0, transformed) return map(lambda pnt: self.focus * pnt[:2]/pnt[2] + np.array(self.frameSize)/2, inFrontofCameraPoints) cam = MyCam() ''' frame = cam.read() cv2.imshow('frame',frame) k = cv2.waitKey(0) ''' angle = .1 rotateZ = np.array([[np.cos(angle), np.sin(angle), 0], [-np.sin(angle), np.cos(angle), 0], [0,0,1]]) while(1): frame = cam.read() cv2.imshow('frame',frame) cam.R = np.dot(rotate, cam.R) k = cv2.waitKey(30) & 0xff if k == 27: break rotateX = np.array([[np.cos(angle), np.sin(angle), 0], [-np.sin(angle), np.cos(angle), 0], [0,0,1]]) cv2.destroyAllWindows() |