def main(): args = parse_args() image_path = os.path.join(args.path, 'image_02/', args.seq) oxts_path = os.path.join(args.path, 'oxts/', args.seq + '.txt') itms = [ os.path.join(image_path, it) for it in os.listdir(image_path) if it.endswith('.png') ] annotation = get_anno_tracking(oxts_path) img_shape = cv2.imread(itms[0], 0).shape cam = PinholeCamera(img_shape[1], img_shape[0], 718.8560, 718.8560, 607.1928, 185.2157) vo = VisualOdometry(cam, annotation, args.use_abs_scale, args.skip_frame) wb = Whiteboard() for img_id in range(0, len(itms), args.skip_frame): img = cv2.imread( args.path + 'image_02/' + args.seq + '/' + str(img_id).zfill(6) + '.png', 0) vo.update(img, img_id) if (img_id > 0): cur_t = vo.cur_t.squeeze() x, y, z = cur_t[0], cur_t[2], -cur_t[1] else: x, y, z = 0., 0., 0. t_loc = np.array([vo.trueX, vo.trueY, vo.trueZ]) t_color = (0, 0, 255) p_loc = np.array([x, y, z]) p_color = (img_id * 255.0 / len(itms), 255 - img_id * 255.0 / len(itms), 0) wb.draw(img_id, 'True', t_loc, t_color) wb.draw(img_id, 'Pred', p_loc, p_color) wb.show(img) if args.save_name: wb.save(args.save_name)
def getVO(data_path, rgb_txt): cam = PinholeCamera(640.0, 480.0, 517.3, 516.5, 318.6, 255.3, 0.2624, -0.9531, -0.0054, 0.0026, 1.1633) imgList = getImageLists() vo = VisualOdometry(cam, imgList) # xyz= np.array([[0],[0],[0]]) img = cv2.imread('./Data/' + imgList[0], 0) f = open("teat2.txt", "w") for img_id in range(len(imgList)): # img = cv2.imread('./Data/'+imgList[img_id], 0) vo.update(img, img_id) if img_id > 1: cur_t = vo.cur_t cur_r = vo.cur_R # xyz=cur_r.dot(xyz)+cur_t name = imgList[img_id].split('/')[1].split('.')[0] pose = transRo2Qu(cur_t, cur_r) f.write(name + ' ' + pose + '\n') print(img_id)
def main(): flag_train = False image_list_file = open(sys.argv[1]) image_list_file.readline() # skip the first line image_list = image_list_file.read().split('\n') scale_gt = np.loadtxt(sys.argv[2]) vo_params={'fx':716,'fy':716,'cx':607,'cy':189,'bucket_size':30,'dense':2,'feature_threshold':0.000007} vo = VisualOdometry(vo_params) model_params = {'image_shape':[1241,376],'grid_size':5} mc = ModelConstructor(model_params) if flag_train: environment_model = model_construction(image_list,scale_gt,vo,mc) mc.write('environment_model') else: mc.read('environment_model') environment_model = mc.model #mc.visualization() scale_params={'threshold':20} sc = ScalePredictor(mc,scale_params) vo.reset() path = vo_with_scale(image_list,vo,sc) np.savetxt('path.txt',path) draw.draw_path(path)
traj = np.zeros((traj_width,traj_width,3), dtype=np.uint8) # **************************** # Pin - Hole Camera Model # **************************** cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157) # ************************ # Visual Odometry # ************************ vo = VisualOdometry(cam, '/Volumes/phantom/dataset_tum/dataset/poses/00.txt') import numpy as np from pyquaternion import Quaternion for img_id in range(img_count): img = ai.imread(img_folder_path+str(img_id).zfill(6)+'.png', 0) vo.pointer(img, img_id) cur_t = vo.cur_t rotation = vo.cur_R print("\nRotational Matrix :") print(rotation)
import numpy as np import cv2 from visual_odometry import PinholeCamera, VisualOdometry cam = PinholeCamera(320.0, 180.0, 963.93956241, 966.19092668, 644.61135581, 356.78176629) vo = VisualOdometry(cam) traj = np.zeros((600, 600, 3), dtype=np.uint8) cap = cv2.VideoCapture(0) i = 0 #cv2.namedWindow("x") cv2.namedWindow("map") ret, rgb_img = cap.read() print(rgb_img.shape) while True: i = i + 1 ret, rgb_img = cap.read() img = cv2.cvtColor(cv2.resize(rgb_img, (320, 180)), cv2.COLOR_BGR2GRAY) vo.update(img) if (i > 2): x, y, z = vo.cur_t[0], vo.cur_t[1], vo.cur_t[2]
@author: Shelton """ import numpy as np import cv2 from visual_odometry import PinholeCamera, VisualOdometry cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157) #cam=PinholeCamera(640,480,538.8636705068433,538.8445314918096,318.83256039016896,242.66774610227353,k1=0.24478296725809076, k2=-0.5104803048920522, p1=-0.006448619715457021, p2=-0.002142112640096728, k3=0.0) index = input("input data number") ##input print("data number:", index) ##print index = str(index) #vo = VisualOdometry(cam, '/home/turtlebot/Downloads/dataset/sequences/00/02.txt') vo = VisualOdometry( cam, '/home/turtlebot/Downloads/dataset/sequences/00/0' + index + '.txt') traj = np.zeros((1000, 1000, 3), dtype=np.uint8) for img_id in xrange(4541): #img = cv2.imread('/home/turtlebot/Downloads/dataset/sequences/02/image_0/'+str(img_id).zfill(6)+'.png', 0) img = cv2.imread( '/home/turtlebot/Downloads/dataset/sequences/0' + index + '/image_0/' + str(img_id).zfill(6) + '.png', 0) #img=cv2.imread('/home/turtlebot/data/'+str(img_id)+'.png') #img=img[240:480,:] vo.update(img, img_id) cur_t = vo.cur_t if (img_id > 2): x, y, z = cur_t[0], cur_t[1], cur_t[2]
import numpy as np import cv2 from visual_odometry import PinholeCamera, VisualOdometry cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157) vo = VisualOdometry(cam, '/home/surya/monoVO-python/dataset/sequences/00/00.txt') traj = np.zeros((600, 600, 3), dtype=np.uint8) for img_id in xrange(4541): img = cv2.imread( '/home/surya/monoVO-python/dataset/sequences/00/image_0/' + str(img_id).zfill(6) + '.png', 0) vo.update(img, img_id) cur_t = vo.cur_t if (img_id > 2): 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) + 290, int(z) + 90 true_x, true_y = int(vo.trueX) + 290, int(vo.trueZ) + 90 cv2.circle(traj, (draw_x, draw_y), 1, (img_id * 255 / 4540, 255 - img_id * 255 / 4540, 0), 1) cv2.circle(traj, (true_x, true_y), 1, (0, 0, 255), 2) cv2.rectangle(traj, (10, 20), (600, 60), (0, 0, 0), -1) text = "Coordinates: x=%2fm y=%2fm z=%2fm" % (x, y, z)
from visual_odometry import VisualOdometry from camera import PinholeCamera import cv2 import glob import numpy as np camera = PinholeCamera(1241.0, 376.0, 718.8560, 607.1928, 185.2157) vo = VisualOdometry(camera, "00.txt") img_paths = sorted(glob.glob("data/*")) traj = np.zeros((600,600,3), dtype=np.uint8) frame_id = 0 for path in img_paths: img = cv2.imread(path, 0) vo.process_image(img, frame_id) t = vo.t if frame_id > 1: x, y, z = t[0], t[1], t[2] else: x, y, z = 0, 0, 0 draw_x, draw_y = int(x)+290, int(z)+90 true_x, true_y = int(vo.trueX)+290, int(vo.trueZ)+90 cv2.circle(traj, (true_x,true_y), 1, (255,0,0), 2) cv2.circle(traj, (draw_x, draw_y), 1, (0,0,255), 2) cv2.imshow('Road facing camera', img) cv2.imshow('Trajectory', traj) cv2.waitKey(1) frame_id += 1 cv2.imwrite('map2.png', traj)
config.cam_settings['Camera.cx'], config.cam_settings['Camera.cy'], config.DistCoef, config.cam_settings['Camera.fps']) num_features=2000 # how many features do you want to detect and track? # select your tracker configuration (see the file feature_tracker_configs.py) # LK_SHI_TOMASI, LK_FAST # SHI_TOMASI_ORB, FAST_ORB, ORB, BRISK, AKAZE, FAST_FREAK, SIFT, ROOT_SIFT, SURF, SUPERPOINT, FAST_TFEAT tracker_config = FeatureTrackerConfigs.LK_SHI_TOMASI tracker_config['num_features'] = num_features feature_tracker = feature_tracker_factory(**tracker_config) # create visual odometry object vo = VisualOdometry(cam, groundtruth, feature_tracker) is_draw_traj_img = True traj_img_size = 800 traj_img = np.zeros((traj_img_size, traj_img_size, 3), dtype=np.uint8) half_traj_img_size = int(0.5*traj_img_size) draw_scale = 1 is_draw_3d = True if kUsePangolin: viewer3D = Viewer3D() else: plt3d = Mplot3d(title='3D trajectory') is_draw_err = True err_plt = Mplot2d(xlabel='img id', ylabel='m',title='error')
def main(): # Parse commands arguments argument = argparse.ArgumentParser() argument.add_argument("-pose_path", help="specify the path of ground truth poses") argument.add_argument("-image_dir", help="specify the directory of input images (undistorted)") argument.add_argument("-image_end", help="specify the filename extension of input images (e.g. png)") argument.add_argument("--v", help="set verbose as true", action="store_true") argument.add_argument("--a", help="use gps scale info", action="store_true") args = argument.parse_args() verbose = args.v absolute = args.a # Check validation of arguments image_dir = os.path.realpath(IMAGE_DIR) if args.image_dir is not None: if os.path.isdir(args.image_dir): image_dir = os.path.realpath(args.image_dir) pose_path = os.path.realpath(POSE_PATH) if args.pose_path is not None: if os.path.isfile(args.pose_path): pose_path = os.path.realpath(args.pose_path) with open(pose_path) as f: poses_context = f.readlines() image_end = IMAGE_END if args.pose_path is not None: image_end = args.image_end if absolute: if not os.path.isfile(pose_path): print(PRIFIX + '[Error] The ground truth poses not found, required by absolute scale info!') sys.exit() # Find all images under image_dir (with image_end) images_files_list = [] for file in os.listdir(image_dir): if file.endswith(image_end): images_files_list.append(image_dir + '/' + file) if len(images_files_list) == 0: print(PRIFIX + '[Error] There is no image ends with ' + image_end + ' under image_dir!') sys.exit() images_files_list.sort() # Show basic information print(PRIFIX + "===== HyphaROS Mono-VO Example =====") print(PRIFIX + "the pose_path: " + pose_path) print(PRIFIX + "The image_dir: " + image_dir) print(PRIFIX + "The image_end: " + image_end) print(PRIFIX + "Use GPS scale: " + str(absolute)) print(PRIFIX + "Total images found: " + str(len(images_files_list))) print('\n') # Initial VisualOdometry Object camera_model = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157) vo = VisualOdometry(camera_model) trajectory_plot = np.zeros((1000,1000,3), dtype=np.uint8) # ======================= # ==== Bootstrapping ==== # ======================= print(PRIFIX + "===== Start Bootstrapping =====") first_index = 0 second_index = first_index + 3 first_frame = cv2.imread(images_files_list[first_index], 0) second_frame = cv2.imread(images_files_list[second_index], 0) second_keypoints, first_keypoints = vo.bootstrapping(first_frame, second_frame) print(PRIFIX + "First frame id: ", str(first_index) ) print(PRIFIX + "Second frame id: ", str(second_index) ) print(PRIFIX + "Initial matches: ", second_keypoints.shape[0] ) # Draw features tracking show_image = drawTrackedFeatures(second_frame, first_frame, second_keypoints, first_keypoints) print(PRIFIX + "Wait any input to continue ...") cv2.imshow('Front Camera', show_image) cv2.waitKey() # ======================= # ==== Frame Process ==== # ======================= print('\n') print(PRIFIX + "===== Start Frame Processing =====") print(PRIFIX + "Press 'ESC' to stop the loop.") prev_frame = second_frame prev_keypoints = second_keypoints # Main loop for frame processing for index in range(second_index+1, len(images_files_list)): curr_frame = cv2.imread(images_files_list[index], 0) # Get GPS ground truth trajectory true_pose, true_scale = getGroundTruthAndScale(poses_context, index) true_x, true_y = int(true_pose[0])+290, int(true_pose[2])+90 # Process frame w/o true scale info if not absolute: curr_keypoints, prev_keypoints, reinitial = vo.processFrame(curr_frame, prev_frame, prev_keypoints) # Process frame w/ true scale info else: curr_keypoints, prev_keypoints, reinitial = vo.processFrame(curr_frame, prev_frame, prev_keypoints, absolute_scale = true_scale) # Get VO translation curr_t = vo.curr_t if(index > 2): x, y, z = curr_t[0], curr_t[1], curr_t[2] else: x, y, z = 0., 0., 0. odom_x, odom_y = int(x)+290, int(z)+90 if verbose: print('\n') print(PRIFIX + "Image index: ", str(index)) print(PRIFIX + "Odometry T:\n", vo.curr_t) print(PRIFIX + "Odometry R:\n", vo.curr_R) # Draw trajectory cv2.circle(trajectory_plot, (odom_x,odom_y), 1, (index*255/4540,255-index*255/4540,0), 1) cv2.circle(trajectory_plot, (true_x,true_y), 1, (0,0,255), 2) cv2.rectangle(trajectory_plot, (10, 20), (600, 60), (0,0,0), -1) text = "Coordinates: x=%2fm y=%2fm z=%2fm"%(x,y,z) cv2.putText(trajectory_plot, text, (20,40), cv2.FONT_HERSHEY_PLAIN, 1, (255,255,255), 1, 8) cv2.imshow('Trajectory', trajectory_plot) # Draw features tracking show_image = drawTrackedFeatures(curr_frame, prev_frame, curr_keypoints, prev_keypoints, reinitial=reinitial) cv2.imshow('Front Camera', show_image) if verbose and reinitial: print(PRIFIX + "Re-initial, tracked features: ", str(prev_keypoints.shape[0])) # Handle the exit key (ESC) k = cv2.waitKey(10) & 0xff if k == 27: break # Update the old datas prev_frame = curr_frame prev_keypoints = curr_keypoints #ipdb.set_trace() # for debug print('\n') input(PRIFIX + "Press any key to exit.") cv2.destroyAllWindows() print(PRIFIX + "===== Finished ! =====")
import numpy as np import sys sys.path.remove('/opt/ros/kinetic/lib/python2.7/dist-packages') import cv2 from visual_odometry import PinholeCamera, VisualOdometry cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157) vo = VisualOdometry(cam, 'dataset/dataset/poses/00.txt') traj = np.zeros((600, 600, 3), dtype=np.uint8) for img_id in range(4541): img = cv2.imread( 'dataset/sequences/00/image_0/' + str(img_id).zfill(6) + '.png', 0) vo.update(img, img_id) cur_t = vo.cur_t if (img_id > 2): 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) + 290, int(z) + 90 true_x, true_y = int(vo.trueX) + 290, int(vo.trueZ) + 90 cv2.circle(traj, (draw_x, draw_y), 1, (img_id * 255 / 4540, 255 - img_id * 255 / 4540, 0), 1) cv2.circle(traj, (true_x, true_y), 1, (0, 0, 255), 2) cv2.rectangle(traj, (10, 20), (600, 60), (0, 0, 0), -1) text = "Coordinates: x=%2fm y=%2fm z=%2fm" % (x, y, z)
noise_std = 0 for noise_std in [5, 15, 25, 35, 45, 55]: for dynamic in [0, 1]: name = "dynamic" #print("upper limit of noise std:", upper) print("noise std:", noise_std) if (dynamic == 0): print('fixed threshold') name = "fixed" else: print("dynamic_threshold") mse_lst = [] for k in range(num_trials): cam = PinholeCamera(1226.0, 370.0, 718.8560, 718.8560, 607.1928, 185.2157) vo = VisualOdometry(cam, '/Users/farhantoddywala/desktop/dataset/sequences/06/calib2.txt') traj = np.zeros((600,600,3), dtype=np.uint8) scale = 1.0 predicted = [] true = [] #noise_std = 0 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
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) + 290, int(z) + 90 true_x, true_y = int(vo.trueX) + 290, int(vo.trueZ) + 90 cv2.circle(traj, (draw_x, draw_y), 1, (img_id * 255 / 4540, 255 - img_id * 255 / 4540, 0), 1) cv2.circle(traj, (true_x, true_y), 1, (0, 0, 255), 2) cv2.rectangle(traj, (10, 20), (600, 60), (0, 0, 0), -1) text = "Coordinates: x=%2fm y=%2fm z=%2fm" % (x, y, z) cv2.putText(traj, text, (20, 40), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 255), 1, 8) cv2.imshow('Road facing camera', img) # cv2.moveWindow('Trajectory', 800, 800) cv2.imshow('Trajectory', traj) cv2.waitKey(1) cv2.imwrite('map.png', traj) if __name__ == "__main__": args = get_parser().parse_args() print("Command Line Args:", args) cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157) vo = VisualOdometry(cam, args.pose_file) run(vo, args.image_dir)
fy = P[1, 1] # focal length Y cx = P[0, 2] # principal point X cy = P[1, 2] # principal point Y # get img width, height img_path = img_dir + "000000.png" img = cv2.imread(img_path, 0) height = img.shape[0] width = img.shape[1] # create Camera and VisualOdometry objects #cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157) cam = PinholeCamera(width, height, fx, fy, cx, cy) vo_learning = VisualOdometry(cam, poses_path, learning=True) vo_fast = VisualOdometry(cam, poses_path, learning=False) f2 = open(poses_path, 'r') lines = f2.readlines() max_x = max_y = max_z = -np.inf min_x = min_y = min_z = np.inf tx = ty = tz = 0 for line in lines: T = np.array([float(x) for x in line.replace('\n', '').split(' ')]).reshape( (3, 4)) # 3x3 rotation matrix + 3x1 translation tx = T[0, 3] ty = T[1, 3]
import numpy as np import cv2 from visual_odometry import PinholeCamera, VisualOdometry cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157) vo = VisualOdometry(cam, "D:\\file\\data_odometry_gray\\dataset\\poses\\00.txt") traj = np.zeros((600, 600, 3), dtype=np.uint8) for img_id in range(4541): img = cv2.imread( "D:\\file\\data_odometry_gray\\dataset\\sequences\\00\\image_0\\" + str(img_id).zfill(6) + ".png", 0) vo.update(img, img_id) cur_t = vo.cur_t if (img_id > 2): 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) + 290, int(z) + 90 true_x, true_y = int(vo.trueX) + 290, int(vo.trueZ) + 90 cv2.circle(traj, (draw_x, draw_y), 1, (img_id * 255 / 4540, 255 - img_id * 255 / 4540, 0), 1) cv2.circle(traj, (true_x, true_y), 1, (0, 0, 255), 2) cv2.rectangle(traj, (10, 20), (600, 60), (0, 0, 0), -1)
def main(): parser = argparse.ArgumentParser( prog='test_video.py', description='visual odometry script in python') parser.add_argument('--type', type=str, default='KITTI', choices=['KITTI', 'image', 'video'], metavar='<type of input>') parser.add_argument('--source', type=str, metavar='<path to source>') parser.add_argument('--camera_width', type=float, metavar='resolution in x direction') parser.add_argument('--camera_height', type=float, metavar='resolution in y direction') parser.add_argument('--focal', type=float, metavar='focal length in pixels') parser.add_argument('--pp_x', type=float, metavar='x coordinate of pricipal point') parser.add_argument('--pp_y', type=float, metavar='y coordinate of pricipal point') parser.add_argument('--skip', type=int, default=1) parser.add_argument('--verbose', type=bool, default=True, metavar='information of computing E') parser.add_argument('--show', type=bool, default=False, metavar='show the odometry result in real-time') args = parser.parse_args() print(args) if args.type == 'KITTI': cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157) vo = VisualOdometry( cam, 'KITTI', '/home/r05525060/sharedfolder/indoorirPano/Dataset/dataset/poses/00.txt' ) for img_id in xrange(4541): img = cv2.imread( '/home/r05525060/sharedfolder/indoorirPano/Dataset/dataset/sequences/00/image_0/{:06d}.png' .format(img_id), 0) vo.update(img, img_id) cur_t = vo.cur_t if (img_id > 2): 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) + 290, int(z) + 90 true_x, true_y = int(vo.trueX) + 290, int(vo.trueZ) + 90 cv2.circle(traj, (draw_x, draw_y), 1, (img_id * 255 / 4540, 255 - img_id * 255 / 4540, 0), 1) #TODO: 4540 to be changed cv2.circle(traj, (true_x, true_y), 1, (0, 0, 255), 2) cv2.rectangle(traj, (10, 20), (600, 60), (0, 0, 0), -1) text = "Coordinates: x=%2fm y=%2fm z=%2fm" % (x, y, z) cv2.putText(traj, text, (20, 40), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 255), 1, 8) cv2.imshow('Road facing camera', img) cv2.imshow('Trajectory', traj) cv2.waitKey(1) if args.type == 'video': cap = cv2.VideoCapture(args.source) cam = PinholeCamera(args.camera_width, args.camera_height, args.focal, args.focal, args.pp_x, args.pp_y) vo = VisualOdometry(cam, type='video', annotations=None) frame_idx = 0 frame_count = 0 while (True): ret, frame = cap.read() frame_count += 1 if not ret: print 'Video finished!' break if not frame_count % args.skip == 0: continue print 'processing... frame_count:', frame_count print 'processing... frame_index:', frame_idx gray_img = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # print("shape of gray img:", gray_img.shape) vo.update(gray_img, frame_idx) cur_t = vo.cur_t if (frame_idx > 2): 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) + 590, int(z) + 290 true_x, true_y = int(vo.trueX) + 590, int(vo.trueZ) + 290 # cv2.circle(traj, (draw_x,draw_y), 1, (frame_idx*255/4540,255-frame_idx*255/4540,0), 1) cv2.circle(traj, (draw_x, draw_y), 1, (0, 255, 0), 2) cv2.circle(traj, (true_x, true_y), 1, (0, 0, 255), 2) cv2.rectangle(traj, (10, 20), (1200, 60), (0, 0, 0), -1) # black backgroud for text text = "Coordinates: x=%2fm y=%2fm z=%2fm" % (x, y, z) cv2.putText(traj, text, (20, 40), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 255), 1, 8) if args.show: cv2.imshow('Road facing camera', gray_img) cv2.imshow('Trajectory', traj) cv2.waitKey(1) frame_idx += 1 cv2.putText(traj, "fps: {}.".format(30 / args.skip), (20, 100), cv2.FONT_HERSHEY_PLAIN, 2, (255, 255, 255), 2, 8) cv2.putText(traj, "focal length: {} pixels".format(args.focal), (20, 140), cv2.FONT_HERSHEY_PLAIN, 2, (255, 255, 255), 2, 8) if args.type == "KITTI": cv2.imwrite('KITTI_map.jpg', traj) elif args.type == "video": cv2.imwrite( 'output_skip{}/{}_map_f{}_pp{}-{}.jpg'.format( args.skip, os.path.basename(args.source), args.focal, args.pp_x, args.pp_y), traj)
import numpy as np import cv2 from visual_odometry import PinholeCamera, VisualOdometry cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157) vo = VisualOdometry(cam, 'E:/Dataset/KITTI/odometry/dataset/poses/00.txt') traj = np.zeros((600,600,3), dtype=np.uint8) for img_id in range(4541): img = cv2.imread('E:/Dataset/KITTI/odometry/dataset/sequences/00/image_2/%06d.png'%(img_id), 0) vo.update(img, img_id) if vo.state == 2: x, y, z = vo.get_T() draw_x, draw_y = int(x)+290, int(z)+90 true_x, true_y = int(vo.trueX)+290, int(vo.trueZ)+90 cv2.circle(traj, (draw_x,draw_y), 1, (img_id*255/4540,255-img_id*255/4540,0), 1) cv2.circle(traj, (true_x,true_y), 1, (0,0,255), 2) cv2.rectangle(traj, (10, 20), (600, 60), (0,0,0), -1) text = "Coordinates: x=%.2fm y=%.2fm z=%.2fm"%(x,y,z) cv2.putText(traj, text, (20,40), cv2.FONT_HERSHEY_PLAIN, 1, (255,255,255), 1, 8) cv2.imshow('Road facing camera', img) cv2.imshow('Trajectory', traj) if 27 == cv2.waitKey(1): break
import numpy as np import cv2 from visual_odometry import PinholeCamera, VisualOdometry cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157) vo = VisualOdometry( cam, '/home/r05525060/sharedfolder/indoorirPano/Dataset/dataset/poses/00.txt') traj = np.zeros((600, 600, 3), dtype=np.uint8) for img_id in xrange(4541): img = cv2.imread( '/home/r05525060/sharedfolder/indoorirPano/Dataset/dataset/sequences/00/image_0/{:06d}.png' .format(img_id), 0) vo.update(img, img_id) cur_t = vo.cur_t if (img_id > 2): 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) + 290, int(z) + 90 true_x, true_y = int(vo.trueX) + 290, int(vo.trueZ) + 90 cv2.circle(traj, (draw_x, draw_y), 1, (img_id * 255 / 4540, 255 - img_id * 255 / 4540, 0), 1) cv2.circle(traj, (true_x, true_y), 1, (0, 0, 255), 2) cv2.rectangle(traj, (10, 20), (600, 60), (0, 0, 0), -1)
import numpy as np import cv2 from visual_odometry import PinholeCamera, VisualOdometry cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157) #vo = VisualOdometry(cam, '/home/xxx/datasets/KITTI_odometry_poses/00.txt') vo = VisualOdometry(cam, 'C:/Users/KJoshi/Downloads/Kitti/00.txt') traj = np.zeros((600,600,3), dtype=np.uint8) for img_id in range(4000): img = cv2.imread('C:/Users/KJoshi/Downloads/Kitti/image_0/'+str(img_id).zfill(6)+'.png', 0) vo.update(img, img_id) cur_t = vo.cur_t if(img_id > 2): 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)+290, int(z)+90 true_x, true_y = int(vo.trueX)+290, int(vo.trueZ)+90 cv2.circle(traj, (draw_x,draw_y), 1, (img_id*255/4540,255-img_id*255/4540,0), 1) cv2.circle(traj, (true_x,true_y), 1, (0,0,255), 2) cv2.rectangle(traj, (10, 20), (600, 60), (0,0,0), -1) text = "Coordinates: x=%2fm y=%2fm z=%2fm"%(x,y,z) cv2.putText(traj, text, (20,40), cv2.FONT_HERSHEY_PLAIN, 1, (255,255,255), 1, 8)
import numpy as np import cv2 from visual_odometry import PinholeCamera, VisualOdometry cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157) vo = VisualOdometry(cam, '/home/hillcrest/project/data/kittk/poses/00.txt') traj = np.zeros((600, 600, 3), dtype=np.uint8) for img_id in xrange(4541): imgPath = '/home/hillcrest/project/data/kittk/sequences/00/image_0/' + str( img_id).zfill(6) + '.png' img = cv2.imread(imgPath, 0) vo.update(img, img_id) cur_t = vo.cur_t if (img_id > 2): 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) + 290, int(z) + 90 true_x, true_y = int(vo.trueX) + 290, int(vo.trueZ) + 90 cv2.circle(traj, (draw_x, draw_y), 1, (img_id * 255 / 4540, 255 - img_id * 255 / 4540, 0), 1) cv2.circle(traj, (true_x, true_y), 1, (0, 0, 255), 2) cv2.rectangle(traj, (10, 20), (600, 60), (0, 0, 0), -1) text = "Coordinates: x=%2fm y=%2fm z=%2fm" % (x, y, z)
import numpy as np import cv2 import os from visual_odometry import PinholeCamera, VisualOdometry cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157) vo = VisualOdometry(cam, "dataset/poses/01.txt") traj = np.zeros((600, 600, 3), dtype=np.uint8) images_path = 'data_odometry_gray/dataset/sequences/00/image_0' files = [os.path.join(images_path, p) for p in sorted(os.listdir(images_path))] for img_id in range(len(files)): if img_id != 173: img = cv2.imread(files[img_id], 0) vo.update(img, img_id) # cv2.circle(img, (st1,st2), 1, (0,0,255), 1) cur_t = vo.cur_t if (img_id > 0): x, y, z = cur_t[0], cur_t[1], cur_t[2] else: x, y, z = 0., 0., 0. #print('xframe_id',img_id,':',x) #print('yframe_id',img_id,':',z) #print('trueX frame_id',img_id,':',vo.trueX) #print('trueY frame_id',img_id,':',vo.trueZ) draw_x, draw_y = int(x) + 290, int(z) + 90 true_x, true_y = int(vo.trueX) + 290, int(vo.trueZ) + 90
import numpy as np import cv2 import matplotlib.pyplot as plt from visual_odometry import PinholeCamera, VisualOdometry import time import pdb cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157) vo = VisualOdometry(cam, './dataset/00.txt') traj = np.zeros((600, 600, 3), dtype=np.uint8) fig, ax = plt.subplots(nrows=2) plt.show(block=False) for img_id in range(4541): img = cv2.imread('./dataset/00/image_0/' + str(img_id).zfill(6) + '.png', 0) vo.update(img, img_id) cur_t = vo.cur_t if (img_id > 2): 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) + 290, int(z) + 90 true_x, true_y = int(vo.trueX) + 290, int(vo.trueZ) + 90 cv2.circle(traj, (draw_x, draw_y), 1, (img_id * 255 / 4540, 255 - img_id * 255 / 4540, 0), 1) cv2.circle(traj, (true_x, true_y), 1, (0, 0, 255), 2)
height = 480 fx = 554.254691191187 fy = 554.254691191187 cx = 320.5 cy = 240.5 D = [0.0, 0.0, 0.0, 0.0, 0.0] cam = KinectCamera(width, height, fx, fy, cx, cy, D) feature_tracker = FeatureTracker(num_features=MinNumFeatureDefault, num_levels=8, scale_factor=1.2, match_ratio_test=RatioTest, tracker_type=FeatureTrackerTypes.DES_BF) vo = VisualOdometry(cam, feature_tracker, True, True) """ Live plot of the Data using Matplotlib animation """ plt.style.use('seaborn') x = [] y = [] x_t = [] y_t = [] errors = [] frames = [] fig, axs = plt.subplots(ncols=2,
import numpy as np import cv2 from visual_odometry import PinholeCamera, VisualOdometry # width, height, fx, fy, cx, cy, cam = PinholeCamera(640, 480, 45, 45, 22.3, 14.9) vo = VisualOdometry(cam, '/home/chris/Desktop/VO/dataset/poses/00.txt') traj = np.zeros((600, 600, 3), dtype=np.uint8) cap = cv2.VideoCapture(0) for img_id in xrange(4541): _, img = cap.read() img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) vo.update(img, img_id) cur_t = vo.cur_t if (img_id > 2): 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) + 290, int(z) + 90 true_x, true_y = int(vo.trueX) + 290, int(vo.trueZ) + 90 cv2.circle(traj, (draw_x, draw_y), 1, (img_id * 255 / 4540, 255 - img_id * 255 / 4540, 0), 1) cv2.circle(traj, (true_x, true_y), 1, (0, 0, 255), 2) cv2.rectangle(traj, (10, 20), (600, 60), (0, 0, 0), -1) text = "Coordinates: x=%2fm y=%2fm z=%2fm" % (x, y, z) cv2.putText(traj, text, (20, 40), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 255), 1, 8)
meta_data_path = './interpolated.csv' base_path = "C:/Users/asus/Desktop/Self Driving/Comma Scratch/data" skip_factor = 1 start = 8800 df = pd.read_csv(meta_data_path) df = df[df['frame_id'] == 'center_camera'][::skip_factor][start:] df['timestamp'] = df['timestamp'] / 1000000000 ins = pd.read_csv('imu.csv') ins['timestamp'] = ins['timestamp'] / 1000000000 cam = PinholeCamera(640, 480, 2152.445406, 2166.161453, 268.010970, 302.594211) vo = VisualOdometry(cam, meta_data_path) model = Bicycle() traj = np.zeros((600, 600, 3), dtype=np.uint8) #rel_x, rel_y = df[['lat', 'long']].values[0] prev_lat, prev_lon = df[['lat', 'long']].values[0] xg, yg = 0, 0 R_y = 0.4 Q_y = 0.025 INIT_y = 0 INIT_VAR_y = 0.5**2 kalman_y = KalmanFilter(INIT_y, INIT_VAR_y, R_y**2, Q_y**2) R_x = 0.6 Q_x = 0.02
import numpy as np import cv2 from matplotlib import pyplot as plt from visual_odometry import PinholeCamera, VisualOdometry sequence = 6 height, width = np.shape( cv2.imread( '../dataset/sequences/' + str(sequence).zfill(2) + '/image_0/' + str(0).zfill(6) + '.png', 0)) cam = PinholeCamera(width, height, 718.8560, 718.8560, 607.1928, 185.2157) vo = VisualOdometry(cam, '../dataset/poses/' + str(sequence).zfill(2) + '.txt') window_width, window_height = 1000, 1000 traj = np.zeros((window_width, window_height, 3), dtype=np.uint8) x, y, z = 0., 0., 0. x_1, y_1, z_1 = 0., 0., 0. true_x1, true_y1, true_z1 = 0., 0., 0. trajectory = np.zeros((len(vo.annotations), 3)) gt = np.zeros((len(vo.annotations), 3)) for img_id in range(len( vo.annotations)): # Terminates on end of image sequence img = cv2.imread( '../dataset/sequences/' + str(sequence).zfill(2) + '/image_0/' + str(img_id).zfill(6) + '.png', 0) vo.update(img, img_id) cur_t = vo.cur_t cur_t_u = vo.cur_t_unscaled if (img_id > 2):
def visual_odometry(): # ---------------- Initial definitions ----------------------- drone_states = DroneStates() ic = image_converter() #pose = Reference() rospy.Subscriber('sensor_depth/depth', Float32, drone_states.set_depth) rospy.Subscriber('sensor_imu_1/data', ImuData, drone_states.imu_data) rospy.Subscriber('camera/image_raw', Image, ic.callback) rospy.Subscriber('observer/state_estimate', StateEstimate, drone_states.set_estimate) rospy.init_node('visual_odometry', anonymous=True) #pub = rospy.Publisher('reference_generator/reference', Reference, queue_size=1) # Camera matrix is for image size 1920 x 1080 mtx = np.array([[1.35445761E+03, 0.00000000E+00, 8.91069717E+02], [0.00000000E+00, 1.37997405E+03, 7.56192877E+02], [0.00000000E+00, 0.00000000E+00, 1.00000000E+00]]) dist = np.array( [-0.2708139, 0.20052465, 2.08302E-02, 0.0002806, -0.10134601]) cam = PinholeCamera(960, 540, mtx[0, 0] / 2, mtx[1, 1] / 2, mtx[0, 2] / 2, mtx[1, 2] / 2) # -------------------------------------------------------------- vo = VisualOdometry(cam) kf = VOKalmanFilter() traj = np.zeros((600, 650, 3), np.uint8) init = True row = [ 'p_x', 'p_y', 'p_z', 'time', 'x_hat', 'y_hat', 'z_hat', 'a_x', 'a_y', 'a_z' ] f = open('VOestimates.csv', 'w+') writer = csv.writer(f) writer.writerow(row) x_hat = np.zeros((12, 1)) drone_t = np.zeros((3, 1)) reference = Reference() # controller setup: surge_controller = controller(0.1, 0., 0.) sway_controller = controller(0.08, 0., 0.) yaw_controller = controller(0.009, 0., 0.005) depth_controller = controller(3.5, 0., 0.0) depth_controller.init_ref = 0.5 rate = rospy.Rate(7.5) while not rospy.is_shutdown(): # while i < len(depth_data.depth): t = time.time() #ret, frame = cap.read() #if not ret: # continue frame = ic.cv_image #print(frame) 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: dt = time.time() - t cur_t = drone_states.R_cd.dot(vo.cur_t) x, y, z = cur_t[0, 0], cur_t[1, 0], cur_t[2, 0] roll, pitch, yaw = rotationMatrixToEulerAngles(vo.cur_R) 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) # Kalman filtering of the estimates #if isinstance(vo.scale, np.float64): # dt = time.time() - t # drone_t = drone_states.R_cd.dot(vo.t) # u = np.array([[x], [y], [drone_states.z], [drone_states.p], [drone_states.q], [drone_states.r]]) # kf.update(u, dt) # x_hat = kf.x_hat * dt # write estimates to file for plotting row = [x, y, z, roll, pitch, yaw] writer.writerow(row) draw_x, draw_y = int(y) * (1) + 290, int(x) * (-1) + 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_filtered, y_filtered, z_filtered, 1 / dt) cv2.putText(traj, text, (20, 40), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 255), 1, 8) cv2.imshow('Trajectory', traj) cv2.imshow('Video', frame) ch = cv2.waitKey(1) if ch & 0xFF == ord('q'): f.close() break # -------------CONTROLLERS------------------- if vo.scale == 1.0: reference.heave = depth_controller.pid(drone_states.z, dt) depth_scale(depth_controller, drone_states.z, z, vo) reference.surge = 0. reference.yaw = -0.05 * reference.heave reference.sway = -0.3 * reference.heave if isinstance(vo.scale, np.float64): reference.surge = surge_controller.pid(x_filtered, dt) reference.sway = sway_controller.pid(y_filtered, dt) if -0.1 < reference.sway < 0: reference.sway = -0.1 reference.yaw = yaw_controller.pid(drone_states.psi, dt) reference.heave = depth_controller.pid(drone_states.z, dt) reference.depth = 0. reference.depth_rate = 0. reference.heading = 0. reference.heading_rate = 0. rate.sleep() reference.surge = 0. reference.sway = 0. reference.yaw = 0. reference.depth = 0. reference.depth_rate = 0. reference.heading = 0. reference.heading_rate = 0. pub.publish(reference)
import numpy as np import cv2 import sys from matplotlib import pyplot as plt gt_path = sys.argv[1] image_path = sys.argv[2] from visual_odometry import PinholeCamera, VisualOdometry cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157) vo = VisualOdometry(cam, gt_path) traj = np.zeros((600,600,3), dtype=np.uint8) for img_id in range(4541): print(img_id) img = cv2.imread(sys.argv[2]+str(img_id).zfill(6)+'.png', 0) #print(img) vo.update(img, img_id) img_vis = img.copy() if img_id>0: print(vo.feature3d.shape) feature2d = vo.feature3d[:,0:2] feature2d[:,0] = feature2d[:,0]*cam.fx/vo.feature3d[:,2]+cam.cx feature2d[:,1] = feature2d[:,1]*cam.fx/vo.feature3d[:,2]+cam.cy print(feature2d) for point2d in feature2d: cv2.circle(img_vis,(int(point2d[0]),int(point2d[1])),3,(0,255,0),-1)
import numpy as np import cv2 from visual_odometry import FisheyeCamera, VisualOdometry cam = FisheyeCamera(640.0, 480.0, 320, 240, -179.47183, 0, 0.002316744, -3.6359684 * 10**(-6), 2.0546507 * 10**(-8), 256) vo = VisualOdometry( cam, '/home/dongk/다운로드/rpg_urban_fisheye_info/info/groundtruth.txt' ) # CHANGE THIS DIRECTORY PATH traj = np.zeros((600, 600, 3), dtype=np.uint8) for img_id in range(2500): img = cv2.imread('/home/dongk/다운로드/rpg_urban_fisheye_data/data/img/' + 'img' + str(img_id + 390 + 1).zfill(4) + '_0.png', 0) # CHANGE THIS DIRECTORY PATH vo.update(img, img_id) cur_t = vo.cur_t if (img_id > 2): x, y, z = cur_t[0, 0], cur_t[1, 0], cur_t[2, 0] else: x, y, z = 0., 0., 0. draw_x, draw_y = int(x) + 290, int(z) + 90 true_x, true_y = int(-vo.trueX) + 290, int(vo.trueY) + 90 cv2.circle(traj, (draw_x, draw_y), 1, (255.0, 0), 2) cv2.circle(traj, (true_x, true_y), 1, (0, 0, 255), 1) cv2.rectangle(traj, (10, 20), (600, 60), (0, 0, 0), -1)
# Maybe we should try to use older functions, or upgrade, I am not sure.. ######################## from visual_odometry import PinholeCamera, VisualOdometry # Measure time ms_start = int(round(time.time() * 1000)) # Sleep time for debugging sleeptime = 0#0.3 plotting = True # Cam: width, height, focal length x, focal length y, cam center x, cam center y cam = PinholeCamera(640.0, 480.0, 1192, 1192, 320.0, 240.0) # VO: cam, speed vo = VisualOdometry(cam, 0.1) # Some trajectory or so traj = np.zeros((600,600,3), dtype=np.uint8) # Loop through every image (gray scale) in our DB for img_id in range(1,105): # Load image img = cv2.imread('DB_duckiebot/test_g_'+str(img_id).zfill(6)+'.png', 0) # Update odometry vo.update(img, img_id) # Obtain positions cur_t = vo.cur_t if(img_id > 2): x, y, z = cur_t[0], cur_t[1], cur_t[2]