frame = cv2.undistort(frame, mtx, dist) #frame = cv2.resize(frame, (0, 0), fx=0.5, fy=0.5) frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # run visual odometry vo.update(frame) if init: x, y, z, = 0., 0., 0. dt = time.time() - t init = False else: vo.scale = 0.088152 #Median from scale tests dt = time.time() - t cur_t = drone_states.R_cd.dot(vo.cur_t) roll, pitch, yaw = rotationMatrixToEulerAngles(vo.cur_R) x, y, z = cur_t[0, 0], cur_t[1, 0], cur_t[2, 0] keypoints = [] for m in vo.good: keypoints.append(vo.kp_cur[m.trainIdx]) frame = cv2.drawKeypoints(frame, keypoints, np.array([]), color=(0, 255, 0), flags=0) draw_x, draw_y = int(x) + 290, int(y) + 290 cv2.circle(traj, (draw_x, draw_y), 1, (0, 255, 0), 1) cv2.rectangle(traj, (10, 20), (650, 60), (0, 0, 0), -1) text = "Coordinates: x=%2fm y=%2fm z=%2fm fps: %f" % (x, y, z, 1 / dt) cv2.putText(traj, text, (20, 40), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 255), 1, 8)
for img_id in range(0, len(X)):
img = cv2.imread('/Users/farhantoddywala/desktop/dataset/sequences/06/image_0/'+str(img_id).zfill(6)+'.png', 0)
#img = cv2.resize(img, (620,188))
#print(img[0][0])
#print(img.shape)
#rand = np.random.randint(low = -1, high = 2)
#noise_std = min(max(noise_std + rand, 0), upper)
img = img + np.random.normal(loc = 0, scale = noise_std, size = img.shape)
img[img > 255] = 255
img[img < 0] = 0
img = img.astype("uint8")
#print(img.shape)
#img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
if (img_id > 0):
scale = np.linalg.norm(X[img_id][:,3] - X[img_id - 1][:,3])
vo.scale = scale
vo.update(img, img_id)
cur_t = vo.cur_t
kp = len(vo.detector.detect(img))
#print(kp)
if (dynamic == 1):
if (kp > high):
vo.detector = cv2.FastFeatureDetector_create((int)(vo.detector.getThreshold() * increase_rate), nonmaxSuppression=True)
if (kp < low):
vo.detector = cv2.FastFeatureDetector_create((int)(vo.detector.getThreshold() * decrease_rate), nonmaxSuppression=True)
if(img_id > 0):
x, y, z = cur_t[0], cur_t[1], cur_t[2]
else:
x, y, z = 0., 0., 0.
draw_x, draw_y = int(x) // 5 +180, int(z)//5 +270
true_x, true_y = (int)(X[img_id][:,3][0]) // 5 + 180, (int)(X[img_id][:,3][2]) // 5 + 270