```
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
def PropagateMaskBoundary(self, Mask, FOE, Sol, ImageSize):
# Sol has [Alphax, Alphay; Betax, Betay]
# findContours gives [x,y]
# ImageCenter is not used as it cancels out
_, BoundaryList, _ = cv2.findContours(Mask,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_NONE)
# print BoundaryList
Shifted = []
Masks = []
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
Masks.append(np.zeros(ImageSize))
# 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
# Masks[i][BoundaryList[i][:,0][:,1], BoundaryList[i][:,0][:,0]] = 255
Masks[i][ValidX, ValidY] = 255
cv2.imshow("Masks", Masks[0])
```