if scale is None:
scale = np.cos(lat * np.pi / 180.)
R, t = pose_from_oxts_packet(metadata[:6], scale, imu2cam)
if origin is None:
origin = t
if orientation is None:
After Change
origin = None
imu2velo = read_calib_file(drive.parent/"calib_imu_to_velo.txt")
velo2cam = read_calib_file(drive.parent/"calib_velo_to_cam.txt")
cam2cam = read_calib_file(drive.parent/"calib_cam_to_cam.txt")
velo2cam_mat = transform_from_rot_trans(velo2cam["R"], velo2cam["T"])
imu2velo_mat = transform_from_rot_trans(imu2velo["R"], imu2velo["T"])
cam_2rect_mat = transform_from_rot_trans(cam2cam["R_rect_00"], np.zeros(3))
imu2cam = cam_2rect_mat @ velo2cam_mat @ imu2velo_mat
for n, f in enumerate(oxts):
metadata = np.genfromtxt(f)
speed = metadata[8:11]
scene_data["speed"].append(speed)
scene_data["frame_id"].append("{:010d}".format(n))
lat = metadata[0]
if scale is None:
scale = np.cos(lat * np.pi / 180.)
pose_matrix = pose_from_oxts_packet(metadata[:6], scale)
if origin is None:
origin = pose_matrix
odo_pose = imu2cam @ np.linalg.inv(origin) @ pose_matrix @ np.linalg.inv(imu2cam)
scene_data["pose"].append(odo_pose[:3])
sample = self.load_image(scene_data, 0)
if sample is None:
