collab/igvc2018
git clone https://git.julia.blue/collab/igvc2018
d809afd
camera
1import cv2 2import numpy as np 3 4MAX_FRAME = 1000 5MIN_NUM_FEAT = 2000 6 7 8def featureTracking (img1 ,img2 ,points1 ,points2 ,status ): 9error = [] 10winSize = (21 ,21 ) 11termcrit = (cv2 .TERM_CRITERIA_EPS | cv2 .TERM_CRITERIA_COUNT ,30 ,0.01 ) 12cv2 .calcOpticalFlowPyrLK (img1 ,img2 ,points1 ,points2 ,status ,error , 13winSize ,3 ,termcrit ,0 ,0.001 ) 14 15indexCorrection = 0 16for i in range (0 ,status .size ()): 17pt = points2 [i - indexCorrection ] 18if (status [i ]== 0 )or (pt .x < 0 )or (pt .y < 0 ): 19if (pt .x < 0 or pt .y < 0 ): 20status [i ]= 0 21del points1 [i - indexCorrection ] 22del points2 [i - indexCorrection ] 23indexCorrection += 1 24 25 26def featureDetection (img1 ): 27fast = cv2 .FastFeatureDetector_create () 28fast .setBool ('nonmaxSuppression' ,True ) 29fast .setInt ('threshold' ,20 ) 30keypoints1 = fast .detect (img1 ,cv2 .NONE ) 31return np .float ([keypoints1 [idx ].pt for idx in len (keypoints1 )]).reshape ( 32- 1 ,1 ,2 ) 33#convert keypoints to points 34 35 36def getScale (): 37#TODO: return from encoder speed 38return 1.0 39 40 41scale = 1.0 42 43vid = cv2 .VideoCapture ("vid" ) 44while not vid .isOpened (): 45vid = cv2 .VideoCapture ("vid" ) 46cv2 .waitKey (1000 ) 47"Wait for the header" ) 48 49f1 ,img1c = vid .read () 50f2 ,img2c = vid .read () 51if (not f1 or not f2 ): 52"error reading video" ) 53 54img1 = cv2 .cvtColor (img1c ,cv2 .COLOR_BGR2GRAY ) 55img2 = cv2 .cvtColor (img2c ,cv2 .COLOR_BGR2GRAY ) 56points2 = [] 57points1 = featureDetection (img1 ) 58status = [] 59featureTracking (img1 ,img2 ,points1 ,points2 ,status ) 60 61E ,mask = cv2 .findEssentialMat (points2 ,points1 ,1.0 ) 62cv2 .recoverPose (E ,points2 ,points1 ) 63 64prevImage = img2 65prevFeatures = points2 66 67cv2 .namedWindow ("camera" ) 68cv2 .namedWindow ("trajectory" ) 69traj = np .zeroes ((600 ,600 ),cv2 .CV_8UC3 ) 70 71pos_frame = vid .get (cv2 .cv .CV_CAP_PROP_POS_FRAMES ) 72while True : 73flag ,frame = vid .read () 74if flag : 75# The frame is ready and already captured 76pos_frame = vid .get (cv2 .cv .CV_CAP_PROP_POS_FRAMES ) 77str (pos_frame )+ " frames" ) 78currImage = cv2 .cvtColor (frame ,cv2 .COLOR_BGR2GRAY ) 79status = [] 80currFeatures = [] 81featureTracking (prevImage ,currImage ,prevFeatures ,currFeatures , 82status ) 83 84E ,mask = cv2 .findEssentialMat (currFeatures ,prevFeatures ,1.0 ) 85pointsa ,R ,t ,mask = cv2 .recoverPose (E ,currFeatures ,prevFeatures ) 86prevImage = img2 87prevFeatures = points2 88R_f = np .copy (R ) 89t_f = np .copy (t ) 90 91# prevPts = np.zeroes((len(prevFeatures), 2), cv2.CV_64F) 92# currPts = np.zeroes((len(currFeatures), 2), cv2.CV_64F) 93 94if (scale > 0.1 and t [2 ]> t [0 ]and t [2 ]> t [1 ]): 95t_f = t_f + scale * (R_f * t ) 96R_f = R * R_f 97 98prevImage = np .copy (currImage ) 99prevFeatures = currFeatures 100x = t_f [0 ]+ 300 101y = t_f [2 ]+ 100 102cv2 .circle (traj , (x ,y ),1 ,cv2 .CV_RGB (255 ,0 ,0 ),2 ) 103cv2 .rectangle (traj , (10 ,30 ), (550 ,50 ),cv2 .CV_RGB (0 ,0 ,0 ), 104cv2 .CV_FILLED ) 105'Coordinates: x = {:f}m y = {:f}m z = {:f}m' .format ( 106t_f [0 ],t_f [1 ],t_f [2 ])) 107 108cv2 .imshow ("Road facing camera" ,frame ) 109cv2 .imshow ("Trajectory" ,traj ) 110cv2 .waitKey (1 ) 111else : 112# The next frame is not ready, so we try to read it again 113vid .set (cv2 .cv .CV_CAP_PROP_POS_FRAMES ,pos_frame - 1 ) 114"frame is not ready" ) 115# It is better to wait for a while for the next frame to be ready 116cv2 .waitKey (1000 ) 117 118if cv2 .waitKey (10 )== 27 : 119break 120if vid .get (cv2 .cv .CV_CAP_PROP_POS_FRAMES )== vid .get ( 121cv2 .cv .CV_CAP_PROP_FRAME_COUNT ): 122# If the number of captured frames is equal to the total number of frames, 123# we stop 124break