#============================================
# create SLAM object
#============================================
slam = Slam(cam, feature_tracker) #, grountruth)
timer = Timer()
#============================================
# N.B.: keep this coherent with the above forced camera settings
img_ref = cv2.imread('../data/kitti06-12.png', cv2.IMREAD_COLOR)
#img_cur = cv2.imread('../data/kitti06-12-01.png',cv2.IMREAD_COLOR)
img_cur = cv2.imread('../data/kitti06-13.png', cv2.IMREAD_COLOR)
slam.track(img_ref, frame_id=0)
slam.track(img_cur, frame_id=1)
f_ref = slam.map.get_frame(-2)
f_cur = slam.map.get_frame(-1)
print('search for triangulation...')
timer.start()
idxs_ref, idxs_cur, num_found_matches, img_cur_epi = search_frame_for_triangulation_test(
f_ref, f_cur, img_cur, img1=img_ref) # test
#idxs_ref, idxs_cur, num_found_matches, img_cur_epi = search_frame_for_triangulation(f_ref, f_cur)
elapsed = timer.elapsed()
print('time:', elapsed)
print('# found matches:', num_found_matches)
while dataset.isOk():
if not is_paused:
print('..................................')
print('image: ', img_id)
img = dataset.getImageColor(img_id)
if img is None:
print('image is empty')
getchar()
timestamp = dataset.getTimestamp() # get current timestamp
next_timestamp = dataset.getNextTimestamp() # get next timestamp
frame_duration = next_timestamp - timestamp
if img is not None:
time_start = time.time()
slam.track(img, img_id, timestamp) # main SLAM function
# 3D display (map display)
if viewer3D is not None:
viewer3D.draw_map(slam)
img_draw = slam.map.draw_feature_trails(img)
# 2D display (image display)
if display2d is not None:
display2d.draw(img_draw)
else:
cv2.imshow('Camera', img_draw)
if matched_points_plt is not None:
if slam.tracking.num_matched_kps is not None: