-
Notifications
You must be signed in to change notification settings - Fork 0
/
feature_align.py
68 lines (52 loc) · 2 KB
/
feature_align.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
import numpy as np
import cv2
import sophus as sp
from src import Frame, PinholeCamera, SparseImgAlign, Map, Reprojector
import src
def getName(k):
name0 = "%06d" % k
name1 = "%06d" % (k+1)
name2 = "%06d" % (k+2)
sufix = '.png'
return (name0 + sufix, name1 + sufix, name2 + sufix)
path = '../slam/dataset/sequences/00/image_0/'
init = src.Initialization()
####### 1.0 read images #######
name0, name1, name2 = getName(0)
img0 = cv2.imread(path + name0)
img1 = cv2.imread(path + name1)
img2 = cv2.imread(path + name2)
gray0 = cv2.cvtColor(img0, cv2.COLOR_BGR2GRAY)
gray1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)
gray2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)
rows, cols = gray0.shape
####### 2.0 create camera, frames #######
cam = PinholeCamera(width=cols, height=rows,
fx=718.856, fy=718.856,
cx=607.1928, cy=185.2157)
first_frame = Frame(cam, gray0, 0.0)
second_frame = Frame(cam, gray1, 0.0)
third_frame = Frame(cam, gray2, 0.0)
init.add_first_frame(first_frame)
init.add_second_frame(second_frame)
img_align = src.SparseImgAlign(4, 1, 30, SparseImgAlign.GAUSS_NEWTON, False, False)
img_align_n_tracked = img_align.run(second_frame, third_frame)
points_map = Map()
first_frame.set_keyframe()
second_frame.set_keyframe()
points_map.add_keyframe(first_frame)
points_map.add_keyframe(second_frame)
# close_frames = points_map.get_close_keyframes(first_frame)
reprojector = Reprojector(cam, points_map)
overlap_kfs = reprojector.reproject_map(third_frame)
print(overlap_kfs)
# print(close_frames)
# for ft in first_frame.features:
# cv2.circle(img0, tuple(ft.uv), 2, (255, 0, 0), 1)
# for kp in first_frame.keypoints:
# cv2.circle(img0, tuple(kp.uv), 4, (0, 255, 0), 1)
# cv2.putText(img0, str(kp.uv), tuple(kp.uv), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255, 0, 255))
# cv2.circle(img0, (607, 185), 2, (0, 0, 255), 1)
# cv2.putText(img0, 'center' + str((607, 185)), (607, 185), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255, 0, 255))
# cv2.imshow('', img0)
# cv2.waitKey(0)