def __init__(self, R=np.eye(3), t=np.zeros(3)): """ Pose is defined as p_cw (pose of the world wrt camera) """ p = RigidTransform.from_Rt(R, t) RigidTransform.__init__(self, xyzw=p.quat.to_xyzw(), tvec=p.tvec) self.__cached_inverse = None
def read_pose(gt): cam_dir, cam_up = gt.cam_dir, gt.cam_up z = cam_dir / np.linalg.norm(cam_dir) x = np.cross(cam_up, z) y = np.cross(z, x) R = np.vstack([x, y, z]).T t = gt.cam_pos / 1000.0 return RigidTransform.from_Rt(R, t)
def oxts_process_cb(self, fn): X = np.fromfile(fn, dtype=np.float64, sep=' ') packet = AttrDict({fmt: x for (fmt, x) in zip(self.oxts_formats, X)}) # compute scale from first lat value if self.scale_ is None: self.scale_ = np.cos(packet.lat * np.pi / 180.) # Use a Mercator projection to get the translation vector tx = self.scale_ * packet.lon * np.pi * EARTH_RADIUS / 180. ty = self.scale_ * EARTH_RADIUS * \ 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 rx, ry, rz = packet.roll, packet.pitch, packet.yaw Rx = np.float32([1,0,0, 0, np.cos(rx), -np.sin(rx), 0, np.sin(rx), np.cos(rx)]).reshape(3,3) Ry = np.float32([np.cos(ry),0,np.sin(ry), 0, 1, 0, -np.sin(ry), 0, np.cos(ry)]).reshape(3,3) Rz = np.float32([np.cos(rz), -np.sin(rz), 0, np.sin(rz), np.cos(rz), 0, 0, 0, 1]).reshape(3,3) R = np.dot(Rz, Ry.dot(Rx)) pose = RigidTransform.from_Rt(R, t) if self.p_init_ is None: self.p_init_ = pose.inverse() # Use the Euler angles to get the rotation matrix return AttrDict(packet=packet, pose=self.p_init_ * pose)
def kitti_load_poses(fn): X = (np.fromfile(fn, dtype=np.float64, sep=' ')).reshape(-1,12) return [RigidTransform.from_Rt(p[:3,:3], p[:3,3]) for p in [x.reshape(3,4) for x in X]]
def kitti_load_poses(fn): X = (np.fromfile(fn, dtype=np.float64, sep=' ')).reshape(-1,12) return map(lambda p: RigidTransform.from_Rt(p[:3,:3], p[:3,3]), map(lambda x: x.reshape(3,4), X))