next_gyro = copy.deepcopy(vehicle.imu_sensor.gyroscope)
# convert to right-handed coordinates
# accel[1] *= -1
accel[2] *= -1
gyro[1] *= -1
# gyro[2] *= -1
gyro *= np.pi / 180 # radians
print(accel, "accel")
print(gyro, "gyro")
print()
# Perform prediction based on IMU signal
vio_ekf.IMUPrediction(accel, gyro, t - t_prev)
t_prev = copy.deepcopy(t)
accel = copy.deepcopy(next_accel)
gyro = copy.deepcopy(next_gyro)
###############################################
# Visual Odometry ###########################################
out_image_pair, pts1, pts2 = superMatcher.process(
vehicle.front_camera[:, :, 0])
R_, t = motionEstimator.estimate_R_t(
pts1,
pts2,
vehicle.front_camera_intrinsics,
vehicle.front_camera_depth_old,
vehicle.front_camera_semantic_old,
)
