def _poses_from_oxts(self, oxts_packets):
"""Helper method to compute SE(3) pose matrices from OXTS packets."""
er = 6378137. # earth radius (approx.) in meters
# compute scale from first lat value
scale = np.cos(oxts_packets[0].lat * np.pi / 180.)
t_0 = [] # initial position
poses = [] # list of poses computed from oxts
for packet in oxts_packets:
# Use a Mercator projection to get the translation vector
tx = scale * packet.lon * np.pi * er / 180.
ty = scale * er * \
np.log(np.tan((90. + packet.lat) * np.pi / 360.))
tz = packet.alt
t = np.array([tx, ty, tz])
# We want the initial position to be the origin, but keep the ENU
# coordinate system
if len(t_0) == 0:
t_0 = t
# Use the Euler angles to get the rotation matrix
Rx = utils.rotx(packet.roll)
Ry = utils.roty(packet.pitch)
Rz = utils.rotz(packet.yaw)
R = Rz.dot(Ry.dot(Rx))
# Combine the translation and rotation into a homogeneous transform
poses.append(utils.transform_from_rot_trans(R, t - t_0))
return poses
def _poses_from_oxts(self, oxts_packets):
"""Helper method to compute SE(3) pose matrices from OXTS packets."""
er = 6378137. # earth radius (approx.) in meters
# compute scale from first lat value
scale = np.cos(oxts_packets[0].lat * np.pi / 180.)
t_0 = [] # initial position
poses = [] # list of poses computed from oxts
for packet in oxts_packets:
# Use a Mercator projection to get the translation vector
tx = scale * packet.lon * np.pi * er / 180.
ty = scale * er * \
np.log(np.tan((90. + packet.lat) * np.pi / 360.))
tz = packet.alt
t = np.array([tx, ty, tz])
# We want the initial position to be the origin, but keep the ENU
# coordinate system
if len(t_0) == 0:
t_0 = t
# Use the Euler angles to get the rotation matrix
Rx = utils.rotx(packet.roll)
Ry = utils.roty(packet.pitch)
Rz = utils.rotz(packet.yaw)
R = Rz.dot(Ry.dot(Rx))
# Combine the translation and rotation into a homogeneous transform
poses.append(utils.transform_from_rot_trans(R, t - t_0))
return poses
def fix_imu(self):
new_oxts = []
for oxts in self.data_kitti.oxts:
T = oxts.T_w_imu
reverse_yaw = rotz(oxts.packet.yaw).T
reverse_yaw = transform_from_rot_trans(reverse_yaw,
np.array([0, 0, 0]))
T = reverse_yaw @ T
new_oxts.append(OxtsData(oxts.packet, T))
self.data_kitti.oxts = new_oxts
def _load_calib_rigid(self, filename):
"""Read a rigid transform calibration file as a numpy.array."""
filepath = os.path.join(self.calib_path, filename)
data = utils.read_calib_file(filepath)
return utils.transform_from_rot_trans(data['R'], data['T'])
def _load_calib_rigid(self, filename):
"""Read a rigid transform calibration file as a numpy.array."""
filepath = os.path.join(self.calib_path, filename)
data = utils.read_calib_file(filepath)
return utils.transform_from_rot_trans(data['R'], data['T'])
