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.

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()

*Related*