3/15/22, 12:39 AM /home/y3rsn/Dev/cpp/PySLAM/camera-path-generator.

py

1 import numpy as np
2 import cv2
3  
4 from visual_odometry import PinholeCamera, VisualOdometry
5  
6 print("1. KITTI Dataset")
7 print("2. College Library Indoors")
8 print("3. Indoor Hallway")
9  
10 selection = 1
11 selection = input("Enter your choice number: ")
12  
13 cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157)
14  
15 if selection == "1":
16 cam = PinholeCamera(1241.0, 376.0, 718.8560, 718.8560, 607.1928, 185.2157)
17  
18 elif selection == "2" or selection == "3":
19 cam = PinholeCamera(1080.0, 1920.0, 718.8560, 718.8560, 607.1928, 185.2157)
#phone camera
20  
21 vo = VisualOdometry(cam, 'ground_truth.txt')
22  
23 traj = np.zeros((600,600,3), dtype=np.uint8)
24  
25 range1 = 4541
26 range2 = 1363
27 range3 = 1115
28  
29 for img_id in range(range1):
30  
31 if selection == "1":
32 img =
cv2.imread('/home/y3rsn/kitty/sequences/00/image_0/'+str(img_id).zfill(6)+'.png', 0)
33  
34 elif selection == "2":
35 img = cv2.imread('data/college-library/'+str(img_id).zfill(6)+'.jpg', 0)
36  
37 else:
38 img = cv2.imread('data/indoorsample/'+str(img_id).zfill(6)+'.jpg', 0)
39  
40 vo.update(img, img_id)
41  
42 cur_t = vo.cur_t
43 if(img_id > 2):
44 x, y, z = cur_t[0], cur_t[1], cur_t[2]
45 else:
46 x, y, z = 0., 0., 0.
47 draw_x, draw_y = int(x)+290, int(z)+90
48 true_x, true_y = int(vo.trueX)+290, int(vo.trueZ)+90
49  
50 cv2.circle(traj, (draw_x,draw_y), 1, (img_id*255/4540,255-img_id*255/4540,0), 1)
51 cv2.rectangle(traj, (10, 20), (600, 60), (0,0,0), -1)
52 text = "Coordinates: x=%2fm y=%2fm z=%2fm"%(x,y,z)
53 cv2.putText(traj, text, (20,40), cv2.FONT_HERSHEY_PLAIN, 1, (255,255,255), 1, 8)
54  
55 cv2.imshow('Camera View', img)
56 cv2.imshow('Trajectory', traj)
57 cv2.waitKey(1)
58  
59 cv2.imwrite('map.png', traj)

localhost:35577 1/1