plt3d = Mplot3d(title='3D trajectory')
is_draw_err = True
err_plt = Mplot2d(xlabel='img id', ylabel='m',title='error')
is_draw_matched_points = True
matched_points_plt = Mplot2d(xlabel='img id', ylabel='# matches',title='# matches')
img_id = 0
while dataset.isOk():
img = dataset.getImage(img_id)
if img is not None:
vo.track(img, img_id) # main VO function
if(img_id > 2): # start drawing from the third image (when everything is initialized and flows in a normal way)
x, y, z = vo.traj3d_est[-1]
x_true, y_true, z_true = vo.traj3d_gt[-1]
if is_draw_traj_img: # draw 2D trajectory (on the plane xz)
draw_x, draw_y = int(draw_scale*x) + half_traj_img_size, half_traj_img_size - int(draw_scale*z)
true_x, true_y = int(draw_scale*x_true) + half_traj_img_size, half_traj_img_size - int(draw_scale*z_true)
cv2.circle(traj_img, (draw_x, draw_y), 1,(img_id*255/4540, 255-img_id*255/4540, 0), 1) # estimated from green to blue
cv2.circle(traj_img, (true_x, true_y), 1,(0, 0, 255), 1) # groundtruth in red
# write text on traj_img
cv2.rectangle(traj_img, (10, 20), (600, 60), (0, 0, 0), -1)
text = "Coordinates: x=%2fm y=%2fm z=%2fm" % (x, y, z)
cv2.putText(traj_img, text, (20, 40), cv2.FONT_HERSHEY_PLAIN, 1, (255, 255, 255), 1, 8)
# seq 18
# im_init = 14
# im_end = 578
# im_init = 5644
# im_end = 6196
img_id = im_init
while dataset.isOk():
img = dataset.getImage(img_id)
if img is not None and None not in img:
vo.track(img, img_id, rotmat) # main VO function
if img_id == im_end:
break
#assert(False)
if(img_id-im_init > 2): # start drawing from the third image (when everything is initialized and flows in a normal way)
x, y, z = vo.traj3d_est[-1]
x_true, y_true, z_true = vo.traj3d_gt[-1]
#if is_draw_traj_img: # draw 2D trajectory (on the plane xz)
if is_draw_traj_img and not np.any(np.isnan([x_true,y_true,z_true])): # draw 2D trajectory (on the plane xz)
draw_x, draw_y = int(draw_scale*x) + half_traj_img_size, half_traj_img_size - int(draw_scale*z)
true_x, true_y = int(draw_scale*x_true) + half_traj_img_size, half_traj_img_size - int(draw_scale*z_true)
cv2.circle(traj_img, (draw_x, draw_y), 1,(img_id*255/4540, 255-img_id*255/4540, 0), 1) # estimated from green to blue
cv2.circle(traj_img, (true_x, true_y), 1,(0, 0, 255), 1) # groundtruth in red
# write text on traj_img