collab/igvc2018

git clone https://git.julia.blue/collab/igvc2018

Vivek Malneediuntested odometryd809afd

camera
3.8 KiB124 linesraw
1import cv2
2import numpy as np
3
4MAX_FRAME = 1000
5MIN_NUM_FEAT = 2000
6
7
8def featureTracking(img1, img2, points1, points2, status):
9    error = []
10    winSize = (21, 21)
11    termcrit = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 30, 0.01)
12    cv2.calcOpticalFlowPyrLK(img1, img2, points1, points2, status, error,
13                             winSize, 3, termcrit, 0, 0.001)
14
15    indexCorrection = 0
16    for i in range(0, status.size()):
17        pt = points2[i - indexCorrection]
18        if (status[i] == 0) or (pt.x < 0) or (pt.y < 0):
19            if (pt.x < 0 or pt.y < 0):
20                status[i] = 0
21                del points1[i - indexCorrection]
22                del points2[i - indexCorrection]
23                indexCorrection += 1
24
25
26def featureDetection(img1):
27    fast = cv2.FastFeatureDetector_create()
28    fast.setBool('nonmaxSuppression', True)
29    fast.setInt('threshold', 20)
30    keypoints1 = fast.detect(img1, cv2.NONE)
31    return 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
38    return 1.0
39
40
41scale = 1.0
42
43vid = cv2.VideoCapture("vid")
44while not vid.isOpened():
45    vid = cv2.VideoCapture("vid")
46    cv2.waitKey(1000)
47    print("Wait for the header")
48
49f1, img1c = vid.read()
50f2, img2c = vid.read()
51if (not f1 or not f2):
52    print("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:
73    flag, frame = vid.read()
74    if flag:
75        # The frame is ready and already captured
76        pos_frame = vid.get(cv2.cv.CV_CAP_PROP_POS_FRAMES)
77        print(str(pos_frame) + " frames")
78        currImage = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
79        status = []
80        currFeatures = []
81        featureTracking(prevImage, currImage, prevFeatures, currFeatures,
82                        status)
83
84        E, mask = cv2.findEssentialMat(currFeatures, prevFeatures, 1.0)
85        pointsa, R, t, mask = cv2.recoverPose(E, currFeatures, prevFeatures)
86        prevImage = img2
87        prevFeatures = points2
88        R_f = np.copy(R)
89        t_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
94        if (scale > 0.1 and t[2] > t[0] and t[2] > t[1]):
95            t_f = t_f + scale * (R_f * t)
96            R_f = R * R_f
97
98        prevImage = np.copy(currImage)
99        prevFeatures = currFeatures
100        x = t_f[0] + 300
101        y = t_f[2] + 100
102        cv2.circle(traj, (x, y), 1, cv2.CV_RGB(255, 0, 0), 2)
103        cv2.rectangle(traj, (10, 30), (550, 50), cv2.CV_RGB(0, 0, 0),
104                      cv2.CV_FILLED)
105        print('Coordinates: x = {:f}m y = {:f}m z = {:f}m'.format(
106            t_f[0], t_f[1], t_f[2]))
107
108        cv2.imshow("Road facing camera", frame)
109        cv2.imshow("Trajectory", traj)
110        cv2.waitKey(1)
111    else:
112        # The next frame is not ready, so we try to read it again
113        vid.set(cv2.cv.CV_CAP_PROP_POS_FRAMES, pos_frame - 1)
114        print("frame is not ready")
115        # It is better to wait for a while for the next frame to be ready
116        cv2.waitKey(1000)
117
118    if cv2.waitKey(10) == 27:
119        break
120    if vid.get(cv2.cv.CV_CAP_PROP_POS_FRAMES) == vid.get(
121            cv2.cv.CV_CAP_PROP_FRAME_COUNT):
122        # If the number of captured frames is equal to the total number of frames,
123        # we stop
124        break