cv2.waitKey(1)
superMatcher.set_anchor(vehicle.front_camera[:, :, 0])
# new VO trajectory point is relative transformation from previous pose
robot_pose_start = all_poses[-1] @ current_pose
all_poses.append(robot_pose_start)
position_start = robot_pose_start @ np.array([0, 0, 0, 1])
trajectory_vo.append(position_start[:3])
###############################################################
# EKF UPDATE #TODO #########################
if (step % 1 == 0):
# vio_ekf.SuperGlueUpdate((copy.deepcopy(position_start[:3]).T @ vo_compensation).T)
vo_pose = (copy.deepcopy(position_start[:3]).T @ vo_compensation).T
vo_pose[2] = 0
vio_ekf.SuperGlueUpdate(vo_pose[:3])
vio_ekf.addToStateList()
########################################
# Add gt trajectory point (change to right handed coordinates,
# then transform to robot start frame)
gt_pos = vehicle.actor_list[0].get_location()
gt_pos = np.array([gt_pos.x, -gt_pos.y, gt_pos.z, 1])
trajectory_gt.append(H_global_to_start @ gt_pos)
# Apply control on vehicle
for q in range(len(vehicle.other_vehicles)):
vehicle.other_vehicles[q].apply_control(
vehicle.other_agents[q].run_step())
