# transform from global frame to robot start frame initial_pos = H_global_to_start @ np.append(t_right_handed, 1) trajectory_gt = [initial_pos] # EKF Initialization #TODO ######################################################### # Convert the right-handed rotation to a quaternion, roll it to get the form w,x,y,z from x,y,z,w initial_quat_wxyz = np.roll(r_right_handed.as_quat(), 1) # initial_quat_wxyz = np.roll(R.from_matrix(car_start_r).as_quat(), 1) ## Initialize the EKF system #TODO check initial values # vio_ekf = EKF(np.array([0,0,0]), np.array([0, 0, 0]), initial_quat_wxyz, debug=False) vio_ekf = EKF(np.array([0, 0, 0]), np.array([0, 0, 0]), np.array([1, 0, 0, 0]), debug=False) vio_ekf.use_new_data = True vio_ekf.setSigmaAccel(1.) vio_ekf.setSigmaGyro(0.5) vio_ekf.setSigmaVO(100.) accel_list = [] gyro_list = [] # Main Loop ############################################################# max_length = 500 first = True step = 0 while True: step += 1 accel_list.append(accel) gyro_list.append(gyro)
## Convert rotation and location to right-handed r_right_handed = R.from_matrix( carla_rotation_to_numpy_rotation_matrix(initial_rotation)) t_right_handed = np.array( [initial_location.x, initial_location.y * -1, initial_location.z]).T ## Convert the right-handed rotation to a quaternion, roll it to get the form w,x,y,z from x,y,z,w initial_quat = np.roll(r_right_handed.as_quat(), 1) ## Initialize the EKF system #TODO check initial values vio_ekf = EKF(np.array([0, 0, 0]), np.array([0, 0, 0]), initial_quat, debug=False) vio_ekf.use_new_data = False # --- Set instrument noise parameters vio_ekf.setSigmaAccel(0.0) vio_ekf.setSigmaGyro(0.0) ### Location transform ############################################### H_local_to_global = np.eye(4) H_local_to_global[:3, :3] = r_right_handed.as_matrix() H_local_to_global[:3, 3] = t_right_handed H_global_to_local = np.linalg.inv(H_local_to_global) r_right_handed = R.from_matrix(H_global_to_local[:3, :3]) t_right_handed = H_global_to_local[:3, 3] r_right_handed = r_right_handed.as_rotvec()