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))