/
slam.py
92 lines (73 loc) · 2.37 KB
/
slam.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
import cv2
from visualize import Mapping, Visualize
from frame import Frame
from point import Point, Map
import numpy as np
from process_frame import process_frame
import os
# Intrinsic parameters:
F = 984
# assuming no scaling factor
# assuming principal point at center of the frame
# reading video sequence:
cap = cv2.VideoCapture("test_kitti984.mp4")
path = os.getcwd()+"/" + "kitti/"
# if the test is image sequence (e.g. KITTI database):
flag = 2
# using flag to run video or image sequence:\
if flag == 1:
# live video test
# cap = cv2.VideoCapture(-1)
i = 0
while cap.isOpened():
ret, frame = cap.read()
if i == 0:
print("Loading successful")
# determine intrinsic parameters:
W = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
H = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
# since the mapping used width 1024:
if W > 1024:
downscale = 1024.0/W
# doing this to keep the aspect ratio of the original image
F *= downscale
H = int(H * downscale)
W = 1024
K = np.array([[F, 0, W // 2], [0, F, H // 2], [0, 0, 1]])
Kinv = np.linalg.inv(K)
# initialize:
mapp = Map()
plot2d = Visualize(W, H)
plot3d = Mapping()
i += 1
continue
if ret is True:
img = cv2.resize(frame, (W, H))
process_frame(mapp, img, K, Kinv, plot2d, plot3d)
else:
break
i += 1
else:
files = os.listdir(path)
files.sort()
for i in range(len(files)):
pathfull = path + files[i]
frame = cv2.imread(pathfull)
if i == 0:
print("Loading successful")
H, W, C = frame.shape
if W > 1024:
downscale = 1024.0/W
# doing this to keep the aspect ratio of the original image
F *= downscale
H = int(H * downscale)
W = 1024
K = np.array([[F, 0, W // 2], [0, F, H // 2], [0, 0, 1]])
Kinv = np.linalg.inv(K)
# initialize:
mapp = Map()
plot2d = Visualize(W, H)
plot3d = Mapping()
continue
img = cv2.resize(frame, (W, H))
process_frame(mapp, img, K, Kinv, plot2d, plot3d)