mf-tracker

bb.py

from math import sqrt

def calculateBBCenter(bb):
    center = (0.5*(bb[0] + bb[2]),0.5*(bb[1]+bb[3]))
    return center
    
def getFilledBBPoints(bb, numM, numN, margin):
    pointDim = 2
    bb_local = (bb[0] + margin, bb[1] + margin, bb[2] - margin, bb[3] - margin)
    if numM == 1 and numN == 1 :
        pts = calculateBBCenter(bb_local)
        return pts
    
    elif numM > 1 and numN == 1:
        divM = numM - 1
        divN = 2
        spaceM = (bb_local[3]-bb_local[1])/divM
        center = calculateBBCenter(bb_local)
        pt = [0.0]*((numN-1)*numM*pointDim+numM*pointDim)
        for i in range(numN):
            for j in range(numM):
                pt[i * numM * pointDim + j * pointDim + 0] = center[0]
                pt[i * numM * pointDim + j * pointDim + 1] = bb_local[1] + j * spaceM
                
        return pt
        
    elif numM == 1 and numN > 1:
        divM = 2
        divN = numN - 1
        spaceN = (bb_local[2] - bb_local[0]) / divN
        center = calculateBBCenter(bb_local)
        pt = [0.0]*((numN-1)*numM*pointDim+numN*pointDim)
        for i in range(numN):
            for j in range(numN):
                pt[i * numM * pointDim + j * pointDim + 0] = bb_local[0] + i * spaceN
                pt[i * numM * pointDim + j * pointDim + 1] = center[1]
        return pt
        
    elif numM > 1 and numN > 1:
        divM = numM - 1
        divN = numN - 1
    
    spaceN = (bb_local[2] - bb_local[0]) / divN
    spaceM = (bb_local[3] - bb_local[1]) / divM
    print spaceN, spaceM
    pt = [0.0]*((numN-1)*numM*pointDim+numM*pointDim)
    
    print numN*numM*pointDim+numM*pointDim+1
    
    for i in range(numN):
        for j in range(numM):
            pt[i * numM * pointDim + j * pointDim + 0] = bb_local[0] + i * spaceN
            pt[i * numM * pointDim + j * pointDim + 1] = bb_local[1] + j * spaceM
    return pt

def getBBwidth(bb):
    return bb[2]-bb[0]+1
    
def getBBwidth(bb):
    return bb[3]-bb[1]+1
    
def predictBB(bb0, pt0, pt1, nPts):
    ofx = []
    ofy = []
    for i in range(nPts):
        ofx.append(pt1[i][0]-pt0[i][0])
        ofy.append(pt1[i][1]-pt0[i][1])
    
    # Need to write function for getMedianUnmanaged from Median.cpp
    dx = getMedianUnmanaged(ofx, nPts)
    dy = getMedianUnmanaged(ofy, nPts)
    ofx=ofy=0
    
    lenPdist = nPts * (nPts - 1) / 2
    dist0=[]
    
    for i in range(nPts):
        for j in range(nPts):
            temp0 = sqrt(pow(pt0[i].x - pt0[j].x, 2) + pow(pt0[i].y - pt0[j].y, 2))
            temp1 = sqrt(pow(pt1[i].x - pt1[j].x, 2) + pow(pt1[i].y - pt1[j].y, 2))
            dist0.append(temp1/temp0)
    shift = getMedianUnmanaged(dist0, lenPdist)
    
    s0 = 0.5 * (shift - 1) * getBBWidth(bb0)
    s1 = 0.5 * (shift - 1) * getBBHeight(bb0)
    
    bb1 = (bb0[0] - s0 + dx, bb0[1] - s1 + dy,
           bb0[2] + s0 + dx, bb0[3] + s1 + dy)
              
    return bb1
    
def getMedianUnmanaged(a, n):
    low = 0
    high = n - 1
    median = (low + high) / 2
    while True:
        if high <= low:
            return a[median]
        if high = low + 1:
            if a[low] > a[high]:
                a[low],a[high] = a[high],a[low]
            return a[median]
        middle = (low+high)/2
        if a[middle] > a[high]:
            a[middle],a[high] = a[high],a[middle]
        if a[low] > a[high]:
            a[low],a[high] = a[high],a[low]
        if a[middle] > a[low]:
            a[middle],a[low] = a[low],a[middle]
        a[middle],a[low+1] = a[low+1],a[middle]
        
        ll = low + 1
        hh = high
        
        while True:
            while True:
                ll +=1
                if a[low] > a[ll]:
                    break                    
            while True:
                hh -= 1
                if a[hh] > a[low]:
                    break
            if hh < ll:
                break
            a[ll],a[hh] = a[hh],a[ll]
        a[low],a[hh] = a[hh],a[low]
        if hh <= median:
            low = ll
        if hh >= median:
            high = hh - 1

lk.py

from SimpleCV import *
from cv2.cv import *
from cv2 import *
import time

def ecludianDistance(point1,point2,nPts):
    match=[]
    for i in range(nPts):
        match.append(sqrt((point1[i][0] - point2[i][0]) * (point1[i][0] - point2[i][0]) 
                    + (point1[i][1] - point2[i][1]) * (point1[i][1] - point2[i][1])))
    return match

def normCrossCorrelation(img1, img2, pt0, pt1, nPts, status, winsize, method):
    for i in range(nPts):
        if status[i] == 1:
            patch1 = getRectSubPix(img1,(winsize,winsize),pt0[i])
            patch2 = getRectSubPix(img2,(winsize,winsize),pt1[i])
            result.append(matchTemplate(patch1,patch2,method))
        else:
            result.append(0.0)
    return result
            

def lktrack(img1, img2, ptsI, nPtsI, ptsJ, nPtsJ):
    eig = []
    winsize_ncc = 10
    template_pt = []
    target_pt = []
    fb_pt = []
    status=""
    status_bt=""
    for i in range(nPtsI):
        template_pt.append((ptsI[2*i],ptsI[2*i+1]))
        target_pt.append((ptsJ[2*i],ptsJ[2*i+1]))
        fb_pt.append((ptsI[2*i],ptsI[2*i+1]))
        
    #lucas kanade track
    curr_feature, status, track_error = calcOpticalFlowPyrLK(img1, img2,
        template_pt, target_pt, status, err, 
        (win_size_lk, win_size_lk), 0, TERM_CRITERIA_EPS, 
        (TERM_CRITERIA_EPS | TERM_CRITERIA_COUNT, 10, 0.03), eig)
        
    curr_feature_bt, status_bt, track_error_bt = calcOpticalFlowPyrLK(img2,img1,
                            target_pt,fb_pt, status_bt, err,
                            (win_size_lk,win_size_lk),0, TERM_CRITERIA_EPS, 
                            (TERM_CRITERIA_EPS | TERM_CRITERIA_COUNT, 10, 0.03), eig)
    
    for i in range(nPtsI):
        if status[i] == 1 and status_bt[i] == 1:
            status[i]=1
        else:
            status[i]=0
            
    result = normCrossCorrelation(img1, img2, template_pt, target_pt, status, winsize_ncc, CV_TM_CCOEFF_NORMED)
    fb = ecludianDistance(template_pt, target_pt, nPtsI)
    
    for i in range(nPtsI):
        if status[i] == 1:
            ptsJ[2 * i] = target_pt[i][0]
            ptsJ[2 * i + 1] = target_pt[i][1]
        else:
            ptsJ[2 * i] = None
            ptsJ[2 * i + 1] = None
            fb[i] = None
            ncc[i] = None
            
    return fb, result, status

tld.py

from SimpleCV import *
import time

def getBB():
    cam = Camera()
    p1 = None
    p2 = None
    d = Display()
    while d.isNotDone():
        try:
            img = cam.getImage()
            a=img.save(d)
            dwn = d.leftButtonDownPosition()
            up = d.leftButtonUpPosition()
            
            if dwn:
                p1 = dwn
            if up:
                p2 = up
                break

            time.sleep(0.1)
        except KeyboardInterrupt:
            break
    if not p1 or not p2:
        return None
        
    img.drawBB(p1,p2,width=5)
    i = img.copy()
    img.save(d)

getBB()