def roomTemperature(): exp = Experiment() plotting = Plotter() plotting.initialize(filt = "median, movingAverage", skip = 500) dimension = 20 exp.run_experiments_multiple(RoomTemperature(), {},\ [VAR(),\ ECVARMAOGD(),\ OnlineWaveFilteringParameterFree(),\ OnlineWaveFilteringParameterFree(),\ OnlineWaveFilteringParameterFree() ],\ [{'p' : 8, 'dim': dimension},\ {'p' : 8, 'dim': dimension, 'lr': 1},\ {'max_k' : 10, 'action_dim': dimension, 'out_dim': dimension, 'opt': Hedge(), 'optForSubPredictors': FTRL()},\ {'max_k' : 30, 'action_dim': dimension, 'out_dim': dimension, 'opt': Hedge(), 'optForSubPredictors': FTRL()},\ {'max_k' : 50, 'action_dim': dimension, 'out_dim': dimension, 'opt': Hedge(), 'optForSubPredictors': FTRL()} ],\ ['VAR_8',\ 'ECVARMA_OGD16',\ 'OnlineWaveFilteringParameterFree10',\ 'OnlineWaveFilteringParameterFree30',\ 'OnlineWaveFilteringParameterFree50' ],\ n_runs = 20,\ plotter = plotting,\ verbose = True, action_generator = ProblemBasedAction(RoomTemperature()))
def sp500(): exp = Experiment() plotting = Plotter() plotting.initialize(filt = "movingAverage", skip = 100) exp.run_experiments_multiple(SP500(), {},\ [ARMA(),\ ARIMA(),\ OnlineWaveFilteringParameterFree(),\ OnlineWaveFilteringParameterFree(),\ OnlineWaveFilteringParameterFree() ],\ [{'p' : 64, 'optimizer': RealOGD(hyperparameters={'lr':10.0})},\ {'p' : 16, 'd' : 2, 'optimizer': RealOGD(hyperparameters={'lr':10.0})},\ {'max_k' : 20, 'action_dim': 1, 'out_dim': 1, 'opt': Hedge(), 'optForSubPredictors': FTRL()},\ {'max_k' : 50, 'action_dim': 1, 'out_dim': 1, 'opt': Hedge(), 'optForSubPredictors': FTRL()},\ {'max_k' : 100, 'action_dim': 1, 'out_dim': 1, 'opt': Hedge(), 'optForSubPredictors': FTRL()} ],\ ['ARMA_OGD',\ 'ARIMA_OGD',\ 'OnlineWaveFilteringParameterFree20',\ 'OnlineWaveFilteringParameterFree50',\ 'OnlineWaveFilteringParameterFree100' ],\ n_runs = 20,\ plotter = plotting,\ verbose = True, action_generator = ProblemBasedAction(SP500()))
def artif_exp_4(): exp = Experiment() plotting = Plotter() plotting.initialize(yscale='log', filt="median, movingAverage", skip=100, ylabel="Average Squared Error", col=['c', 'r', 'g', 'orange', 'k']) T = 1000 ag = RandomAction(mu=0, sigma=0.3) exp.run_experiments_multiple(Setting4(), {"timesteps" : T},\ [OnlineWaveFilteringParameterFree(),\ OnlineWaveFiltering(),\ EMKalmanFilter(),\ SSIDKalmanFilter(),\ Consistency()],\ [{'timesteps': T, 'max_k' : 30, 'action_dim': 1, 'out_dim': 1, 'opt': Hedge(), 'optForSubPredictors': FTRL()},\ {'timesteps': T, 'k' : 10, 'lr': 1e-2, 'action_dim': 1, 'out_dim': 1, 'R_m': 3.0},\ {'timesteps': T, 'order': 2, 'data': 100, 'iter': 500},\ {'timesteps': T, 'order': 2, 'data': 100},\ {'timesteps': T, 'out_dim': 1}],\ ['OnlineWaveFilteringParameterFree',\ 'OnlineWaveFiltering',\ 'EM',\ '4SID',\ 'Consistency'],\ n_runs = 20,\ plotter = plotting,\ action_generator = ag,\ verbose = True)
def artif_exp_2(): exp = Experiment() plotting = Plotter() plotting.initialize(yscale='log', filt="median, movingAverage", skip=100, ylabel="Average Squared Error", col=['c', 'r', 'g', 'orange', 'k']) A = np.array([[0.999, 0], [0, 0.5]]) B = np.array([[1.0], [1.0]]) C = np.array([[1.0, 1.0]]) D = np.array([[0.0]]) T = 1000 ag = RandomAction(mu=0, sigma=0.3) exp.run_experiments_multiple(LDS(), {"timesteps" : T, 'action_dim': 1, 'hidden_dim': 2, 'out_dim': 1, 'partially_observable': True,\ 'system_params': {'A': A, 'B' : B, 'C': C, 'D': D, 'noise_distribution': 'normal'}},\ [OnlineWaveFilteringParameterFree(),\ OnlineWaveFiltering(),\ EMKalmanFilter(),\ SSIDKalmanFilter(),\ Consistency()],\ [{'timesteps': T, 'max_k' : 30, 'action_dim': 1, 'out_dim': 1, 'opt': Hedge(), 'optForSubPredictors': FTRL()},\ {'timesteps': T, 'k' : 10, 'lr': 1e-3, 'action_dim': 1, 'out_dim': 1, 'R_m': 1.0},\ {'timesteps': T, 'order': 2, 'data': 100, 'iter': 500},\ {'timesteps': T, 'order': 2, 'data': 100},\ {'timesteps': T, 'out_dim': 1}],\ ['OnlineWaveFilteringParameterFree',\ 'OnlineWaveFiltering',\ 'EM',\ '4SID',\ 'Consistency'],\ n_runs = 20,\ plotter = plotting,\ action_generator = ag,\ verbose = True)
def artif_exp_5(): exp = Experiment() plotting = Plotter() plotting.initialize(yscale='log', filt="median, movingAverage", skip=100, ylabel="Average Squared Error", col=['c', 'r', 'g', 'k']) A = np.diag([0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9]) B = np.eye(10) C = np.random.normal(size=(10, 10)) * 0.3 D = np.zeros((10, 10)) T = 1000 ag = BlockAction(prob_repeat=0.8, sigma=0.3) exp.run_experiments_multiple(LDS(), {'action_dim': 10, 'hidden_dim': 10, 'out_dim': 10, 'partially_observable': True,\ 'system_params': {'A': A, 'B' : B, 'C': C, 'D': D}},\ [OnlineWaveFilteringParameterFree(),\ OnlineWaveFiltering(),\ EMKalmanFilter(),\ Consistency()],\ [{'timesteps': T, 'max_k' : 30, 'action_dim': 10, 'out_dim': 10, 'opt': Hedge(), 'optForSubPredictors': FTRL()},\ {'timesteps': T, 'k' : 30, 'lr': 1e-4, 'action_dim': 10, 'out_dim': 10, 'R_m': 5},\ {'timesteps': T, 'order': 10, 'data': 100, 'iter': 500},\ {'timesteps': T, 'out_dim': 10}],\ ['OnlineWaveFilteringParameterFree',\ 'OnlineWaveFiltering',\ 'EM',\ 'Consistency'],\ action_generator = ag,\ n_runs = 20,\ plotter = plotting,\ verbose = True)
def trackBoth(): #with open('vid2/position_file.csv', mode='w') as file: # writer = csv.writer(file, delimiter=',') # writer.writerow(["count", "detectedX", "detectedY", "myX", "myY", "kalX", "kalY"]) # Camera frame frame = None count = 0 # >>>> Kalman Filter stateSize = 4 measSize = 4 contrSize = 0 kf = OnlineWaveFilteringParameterFree() kf.initialize({ 'timesteps': 3000, 'max_k': 30, 'action_dim': 1 * measSize, 'out_dim': stateSize, 'opt': FTRL(), 'optForSubPredictors': FTRL() }) state = np.zeros(stateSize) # [x,y,v_x,v_y,w,h] meas = np.zeros(measSize) # [z_x,z_y,z_w,z_h] lastMeas = meas lastLastMeas = lastMeas kfOld = KalmanFilter() state2 = np.zeros(6) H = np.eye(6)[[0, 1, 4, 5]] # Process Noise Covariance Matrix Q # [ Ex 0 0 0 0 0 ] # [ 0 Ey 0 0 0 0 ] # [ 0 0 Ev_x 0 0 0 ] # [ 0 0 0 Ev_y 0 0 ] # [ 0 0 0 0 Ew 0 ] # [ 0 0 0 0 0 Eh ] #cv::setIdentity(kf.processNoiseCov, cv::Scalar(1e-2)) processNoiseCov = np.zeros((6, 6)) processNoiseCov[0, 0] = 1e-2 processNoiseCov[1, 1] = 1e-2 processNoiseCov[2, 2] = 5.0 processNoiseCov[3, 3] = 5.0 processNoiseCov[4, 4] = 1e-2 processNoiseCov[5, 5] = 1e-2 # Measures Noise Covariance Matrix R #cv2.setIdentity(kf.measurementNoiseCov, 1e-1) #TODO # <<<< Kalman Filter #kf.initialize({'h': state, 'A' : np.eye(6), 'B': np.zeros(6), 'C': np.eye(6), 'P': np.eye(6), 'Q': np.zeros(6), 'R': processNoiseCov}) kfOld.initialize({ 'h': state2, 'A': np.eye(6), 'B': np.zeros(6), 'C': np.eye(6), 'D': np.zeros(6), 'P': np.eye(6), 'Q': processNoiseCov, 'R': np.eye(6) }) # Camera Index idx = -1 # Camera Capture cap = cv2.VideoCapture(idx) #succ = cap.open(idx) cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1024) cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 768) print("\nHit 'q' to exit...\n") ch = 0 ticks = 0 found = False notFoundCount = 0 detectedX = 0 detectedY = 0 myX = 0 myY = 0 kalX = 0 kalY = 0 # >>>>> Main loop while (ch != 'q' and ch != 'Q'): precTick = ticks ticks = cv2.getTickCount() dT = (1.0 * (ticks - precTick)) / cv2.getTickFrequency() # Frame acquisition _, frame = cap.read() #cv2.imshow("Original", frame) res = frame.copy() if (found): #print(np.concatenate((meas, lastMeas)).shape) #state = kf.predict(np.concatenate((meas, lastMeas, lastLastMeas))) #state = kf.predict(np.concatenate((meas, lastMeas))) state = kf.predict(meas) print("State post: " + str(state)) rectWidth = int(state[2]) rectHeight = int(state[3]) rectX = int(state[0] - rectWidth / 2.0) rectY = int(state[1] - rectHeight / 2.0) centerX = int(state[0]) centerY = int(state[1]) cv2.circle(res, (centerX, centerY), 2, (0, 0, 255)) upper = (rectX, rectY) lower = (rectX + rectWidth, rectY + rectHeight) myX = centerX myY = centerY try: cv2.rectangle(res, upper, lower, (0, 0, 255), 2) cv2.putText(res, "(" + str(centerX) + ", " + str(centerY) + ")", (centerX, centerY), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255)) except TypeError as e: print(str(e)) #Kalman # >>>> Matrix A kfOld.A[0, 2] = dT kfOld.A[1, 3] = dT # <<<< Matrix A print("dT: " + str(dT)) state2 = kfOld.predict(0) print("State post: " + str(state2)) rectWidth = int(state2[4]) rectHeight = int(state2[5]) rectX = int(state2[0] - rectWidth // 2.0) rectY = int(state2[1] - rectHeight // 2.0) centerX = int(state2[0]) centerY = int(state2[1]) kalX = centerX kalY = centerY cv2.circle(res, (centerX, centerY), 2, (255, 0, 0)) cv2.rectangle(res, (rectX, rectY), (rectX + rectWidth, rectY + rectHeight), (255, 0, 0), 2) cv2.putText(res, "(" + str(centerX) + ", " + str(centerY) + ")", (centerX, centerY), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0)) else: #print(np.concatenate((meas, lastMeas)).shape) #state = kf.predict(np.concatenate((meas, lastMeas, lastLastMeas))) #state = kf.predict(np.concatenate((meas, lastMeas))) state = kf.predict(meas) lastLastMeas = lastMeas lastMeas = meas meas = state print("State post: " + str(state)) rectWidth = int(state[2]) rectHeight = int(state[3]) rectX = int(state[0] - rectWidth / 2.0) rectY = int(state[1] - rectHeight / 2.0) centerX = int(state[0]) centerY = int(state[1]) cv2.circle(res, (centerX, centerY), 2, (0, 0, 255)) myX = centerX myY = centerY upper = (rectX, rectY) lower = (rectX + rectWidth, rectY + rectHeight) try: cv2.rectangle(res, upper, lower, (0, 0, 255), 2) cv2.putText(res, "(" + str(centerX) + ", " + str(centerY) + ")", (centerX, centerY), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255)) except TypeError as e: print(str(e)) # >>>>> Noise smoothing blur = cv2.medianBlur(frame, 5) #cv2.GaussianBlur(frame, (5, 5), 3.0, 3.0) # <<<<< Noise smoothing # >>>>> HSV conversion frmHsv = cv2.cvtColor(blur, cv2.COLOR_BGR2HSV) # <<<<< HSV conversion # >>>>> Color Thresholding # Note: change parameters for different colors rangeRes = cv2.inRange(frmHsv, (MIN_H_BLUE / 2, 100, 80), (MAX_H_BLUE / 2, 255, 255)) # <<<<< Color Thresholding # >>>>> Improving the result #TODO I removed something here rangeRes = cv2.erode(rangeRes, np.eye(1), iterations=5) rangeRes = cv2.dilate(rangeRes, np.eye(1), iterations=5) # <<<<< Improving the result # Thresholding viewing cv2.imshow("Threshold", rangeRes) # >>>>> Contours detection contours, hierarchy = cv2.findContours(rangeRes, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) # <<<<< Contours detection # >>>>> Filtering balls = [] ballsBox = [] for i in range(len(contours)): bBox = cv2.boundingRect(contours[i]) ratio = (1.0 * bBox[2]) / bBox[3] if (ratio > 1.0): ratio = 1.0 / ratio # Searching for a bBox almost square if (ratio > 0.75 and bBox[2] * bBox[3] >= 400): balls.append(contours[i]) ballsBox.append(bBox) # <<<<< Filtering print("Balls found: " + str(len(ballsBox))) #assume: (x, y, width, height) # >>>>> Detection result for i in range(len(balls)): cv2.drawContours(res, balls, i, (20, 150, 20), 1) cv2.rectangle(res, ballsBox[i], (0, 255, 0), 2) centerX = int(ballsBox[i][0] + ballsBox[i][2] // 2) centerY = int(ballsBox[i][1] + ballsBox[i][3] // 2) cv2.circle(res, (centerX, centerY), 2, (20, 150, 20)) cv2.putText(res, "(" + str(centerX) + ", " + str(centerY) + ")", (centerX, centerY), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0)) detectedX = centerX detectedY = centerY #stringstream sstr #sstr << "(" << center.x << "," << center.y << ")" #cv::putText(res, sstr.str(), # cv::Point(center.x + 3, center.y - 3), # cv::FONT_HERSHEY_SIMPLEX, 0.5, (20,150,20), 2) # <<<<< Detection result # >>>>> Kalman Update if (len(balls) == 0): notFoundCount += 1 print("notFoundCount: " + str(notFoundCount)) if (notFoundCount >= 100): found = False else: notFoundCount = 0 lastLastMeas = lastMeas lastMeas = meas meas[0] = ballsBox[0][0] + ballsBox[0][2] / 2 meas[1] = ballsBox[0][1] + ballsBox[0][3] / 2 meas[2] = ballsBox[0][2] meas[3] = ballsBox[0][3] if (not found): # First detection! # >>>> Initialization state2[0] = meas[0] state2[1] = meas[1] state2[2] = 0 state2[3] = 0 state2[4] = meas[2] state2[5] = meas[3] # <<<< Initialization kfOld.h = state2 kfOld.P = np.eye(6) state = meas found = True else: up = np.array([meas[0], meas[1], 0.0, 0.0, meas[2], meas[3]]) #kf.update_parameters(meas) # Correct kfOld.update_parameters(up) kf.update_parameters(meas) # Correct print("Measure matrix: " + str(meas)) # Final result cv2.imshow("Tracking", res) #cv2.imwrite("vid2/frame%d.jpg" % count, res) #with open('vid2/position_file.csv', mode='a') as file: # writer = csv.writer(file, delimiter=',') # writer.writerow([count, detectedX, detectedY, myX, myY, kalX, kalY]) count += 1 # User key ch = cv2.waitKey(1)
def track(): # Camera frame frame = None # >>>> Kalman Filter stateSize = 4 measSize = 4 contrSize = 0 kf = OnlineWaveFilteringParameterFree() kf.initialize({ 'timesteps': 20000, 'max_k': 30, 'action_dim': 2 * measSize, 'out_dim': stateSize, 'opt': FTRL(), 'optForSubPredictors': FTRL() }) state = np.zeros(stateSize) # [x,y,v_x,v_y,w,h] meas = np.zeros(measSize) # [z_x,z_y,z_w,z_h] lastMeas = meas lastLastMeas = lastMeas # Camera Index idx = -1 # Camera Capture cap = cv2.VideoCapture(idx) #succ = cap.open(idx) cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1024) cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 768) print("\nHit 'q' to exit...\n") ch = 0 ticks = 0 found = False notFoundCount = 0 # >>>>> Main loop while (ch != 'q' and ch != 'Q'): precTick = ticks ticks = cv2.getTickCount() dT = (1.0 * (ticks - precTick)) / cv2.getTickFrequency() # Frame acquisition _, frame = cap.read() #cv2.imshow("Original", frame) res = frame.copy() if (found): #print(np.concatenate((meas, lastMeas)).shape) #state = kf.predict(np.concatenate((meas, lastMeas, lastLastMeas))) state = kf.predict(np.concatenate((meas, lastMeas))) #state = kf.predict(meas) print("State post: " + str(state)) rectWidth = int(state[2]) rectHeight = int(state[3]) rectX = int(state[0] - rectWidth / 2.0) rectY = int(state[1] - rectHeight / 2.0) centerX = int(state[0]) centerY = int(state[1]) cv2.circle(res, (centerX, centerY), 2, (0, 0, 255)) upper = (rectX, rectY) lower = (rectX + rectWidth, rectY + rectHeight) try: cv2.rectangle(res, upper, lower, (0, 0, 255), 2) cv2.putText(res, "(" + str(centerX) + ", " + str(centerY) + ")", (centerX, centerY), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255)) except TypeError as e: print(str(e)) else: #print(np.concatenate((meas, lastMeas)).shape) #state = kf.predict(np.concatenate((meas, lastMeas, lastLastMeas))) state = kf.predict(np.concatenate((meas, lastMeas))) #state = kf.predict(meas) lastLastMeas = lastMeas lastMeas = meas meas = state print("State post: " + str(state)) rectWidth = int(state[2]) rectHeight = int(state[3]) rectX = int(state[0] - rectWidth / 2.0) rectY = int(state[1] - rectHeight / 2.0) centerX = int(state[0]) centerY = int(state[1]) cv2.circle(res, (centerX, centerY), 2, (0, 0, 255)) upper = (rectX, rectY) lower = (rectX + rectWidth, rectY + rectHeight) try: cv2.rectangle(res, upper, lower, (0, 0, 255), 2) cv2.putText(res, "(" + str(centerX) + ", " + str(centerY) + ")", (centerX, centerY), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255)) except TypeError as e: print(str(e)) # >>>>> Noise smoothing blur = cv2.medianBlur(frame, 5) #cv2.GaussianBlur(frame, (5, 5), 3.0, 3.0) # <<<<< Noise smoothing # >>>>> HSV conversion frmHsv = cv2.cvtColor(blur, cv2.COLOR_BGR2HSV) # <<<<< HSV conversion # >>>>> Color Thresholding # Note: change parameters for different colors rangeRes = cv2.inRange(frmHsv, (MIN_H_BLUE / 2, 100, 80), (MAX_H_BLUE / 2, 255, 255)) # <<<<< Color Thresholding # >>>>> Improving the result #TODO I removed something here rangeRes = cv2.erode(rangeRes, np.eye(1), iterations=5) rangeRes = cv2.dilate(rangeRes, np.eye(1), iterations=5) # <<<<< Improving the result # Thresholding viewing cv2.imshow("Threshold", rangeRes) # >>>>> Contours detection contours, hierarchy = cv2.findContours(rangeRes, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) # <<<<< Contours detection # >>>>> Filtering balls = [] ballsBox = [] for i in range(len(contours)): bBox = cv2.boundingRect(contours[i]) ratio = (1.0 * bBox[2]) / bBox[3] if (ratio > 1.0): ratio = 1.0 / ratio # Searching for a bBox almost square if (ratio > 0.75 and bBox[2] * bBox[3] >= 400): balls.append(contours[i]) ballsBox.append(bBox) # <<<<< Filtering print("Balls found: " + str(len(ballsBox))) #assume: (x, y, width, height) # >>>>> Detection result for i in range(len(balls)): cv2.drawContours(res, balls, i, (20, 150, 20), 1) cv2.rectangle(res, ballsBox[i], (0, 255, 0), 2) centerX = int(ballsBox[i][0] + ballsBox[i][2] // 2) centerY = int(ballsBox[i][1] + ballsBox[i][3] // 2) cv2.circle(res, (centerX, centerY), 2, (20, 150, 20)) #stringstream sstr #sstr << "(" << center.x << "," << center.y << ")" #cv::putText(res, sstr.str(), # cv::Point(center.x + 3, center.y - 3), # cv::FONT_HERSHEY_SIMPLEX, 0.5, (20,150,20), 2) # <<<<< Detection result # >>>>> Kalman Update if (len(balls) == 0): notFoundCount += 1 print("notFoundCount: " + str(notFoundCount)) if (notFoundCount >= 100): found = False else: notFoundCount = 0 lastLastMeas = lastMeas lastMeas = meas meas[0] = ballsBox[0][0] + ballsBox[0][2] / 2 meas[1] = ballsBox[0][1] + ballsBox[0][3] / 2 meas[2] = ballsBox[0][2] meas[3] = ballsBox[0][3] if (not found): # First detection! state = meas found = True else: kf.update_parameters(meas) # Correct print("Measure matrix: " + str(meas)) # Final result cv2.imshow("Tracking", res) # User key ch = cv2.waitKey(1)