Anonymous Anonymous - 8 months ago
658 0

No description

Python

PropagateMaskBoundary

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])
Recommended from our users: Dynamic Network Monitoring from WhatsUp Gold from IPSwitch. Free Download