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 _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'])