Anonymous - 1 year ago
846 0

No description

Python

``````import os, errno
import sys
import cv2
import time
import rospy
import numpy as np
import scipy.io as spio

class Tracking(object):
def __init__(self):
i = 1

def ComputeFOE(self, matchedPoints1, I1, I2, params):
## Calculate optical flow
p1, st, err = cv2.calcOpticalFlowPyrLK(I1, I2, matchedPoints1, None, **params)

## Extract only valid points
matchedPoints1 = matchedPoints1[st==1]
matchedPoints2 = p1[st==1]

Ax = np.array(matchedPoints2[:,0])
ones = np.array(-1*np.ones(len(Ax)))
Ax = np.column_stack((Ax, ones))
Axinv = np.linalg.pinv(Ax)

Bx = np.matrix.transpose(matchedPoints2[:,0] - matchedPoints1[:,0])

Solx = np.dot(Axinv, Bx)
Sol = np.row_stack((Solx, Solx))
# print Sol

x0 = Solx[1]/Solx[0]
y0 = np.mean(matchedPoints2[:,1] - ((matchedPoints2[:,1] - matchedPoints1[:,1])/Solx[0]))
return x0, y0, matchedPoints1, matchedPoints2, Sol

# Sol has [Alphax, Alphay; Betax, Betay]
# findContours gives [x,y]
# ImageCenter is not used as it cancels out
# print BoundaryList
Shifted = []
Valid = []
print Sol[0,1]
for i in range(len(BoundaryList)):
Shifted.append(np.round([Sol[0,0]*(BoundaryList[i][:,0][:,1] - FOE[0]) + BoundaryList[i][:,0][:,1],
Sol[0,0]*(BoundaryList[i][:,0][:,0] - FOE[1]) + BoundaryList[i][:,0][:,0]]))

# print Shifted

# Remove pixels outside Image
ShiftedX = Shifted[0][0]
ShiftedY = Shifted[0][1]
# print ValidX
# print ValidY

ValidX = []
ValidY = []
for j in range(len(ShiftedX)):
if ShiftedX[j] <= ImageSize[0] and ShiftedX[j] >= 1 and ShiftedY[j] <= ImageSize[1] and ShiftedY[j] >= 1:
ValidX.append(np.uint8(ShiftedX[j]))
ValidY.append(np.uint8(ShiftedY[j]))

# print ValidX
# print ValidY