def __init__(self, img_size, faces, t_size=3):
self.renderer = NeuralRenderer(img_size)
self.faces = Variable(torch.IntTensor(faces).cuda(),
requires_grad=False)
if self.faces.dim() == 2:
self.faces = torch.unsqueeze(self.faces, 0)
default_tex = np.ones(
(1, self.faces.shape[1], t_size, t_size, t_size, 3))
blue = np.array([156, 199, 234.]) / 255.
default_tex = default_tex * blue
# Could make each triangle different color
self.default_tex = Variable(torch.FloatTensor(default_tex).cuda(),
requires_grad=False)
# rot = transformations.quaternion_about_axis(np.pi/8, [1, 0, 0])
# This is median quaternion from sfm_pose
# rot = np.array([ 0.66553962, 0.31033762, -0.02249813, 0.01267084])
# This is the side view:
import cv2
R0 = cv2.Rodrigues(np.array([np.pi / 3, 0, 0]))[0]
R1 = cv2.Rodrigues(np.array([0, np.pi / 2, 0]))[0]
R = R1.dot(R0)
R = np.vstack((np.hstack((R, np.zeros((3, 1)))), np.array([0, 0, 0,
1])))
rot = transformations.quaternion_from_matrix(R, isprecise=True)
cam = np.hstack([0.75, 0, 0, rot])
self.default_cam = Variable(torch.FloatTensor(cam).cuda(),
requires_grad=False)
self.default_cam = torch.unsqueeze(self.default_cam, 0)
def get_next_pos(trans_params1, dof, cam_cali_mat):
"""
Given the first frame's Aurora position line and relative 6dof, return second frame's position line
:param trans_params1: Aurora position line of the first frame
:param dof: 6 degrees of freedom based on the first frame, rotations should be degrees
:param cam_cali_mat: Camera calibration matrix of this case
:return: Aurora position line of the second frame
"""
trans_mat1 = params_to_mat44(trans_params1, cam_cali_mat=cam_cali_mat)
""" Transfer degrees to euler """
dof[3:] = dof[3:] * (2 * math.pi) / 360
rot_mat = tfms.euler_matrix(dof[5], dof[4], dof[3], 'rzyx')[:3, :3]
relative_mat = np.identity(4)
relative_mat[:3, :3] = rot_mat
relative_mat[:3, 3] = dof[:3]
next_mat = np.dot(inv(cam_cali_mat), inv(np.dot(relative_mat, trans_mat1)))
quaternions = tfms.quaternion_from_matrix(next_mat) # wxyz
next_params = np.zeros(7)
next_params[:3] = next_mat[:3, 3]
next_params[3:6] = quaternions[1:]
next_params[6] = quaternions[0]
return next_params
def forward_img(self, index):
data = self.anno[index]
data_sfm = self.anno_sfm[index]
# sfm_pose = (sfm_c, sfm_t, sfm_r)
sfm_pose = [np.copy(data_sfm.scale), np.copy(data_sfm.trans), np.copy(data_sfm.rot)]
sfm_rot = np.pad(sfm_pose[2], (0,1), 'constant')
sfm_rot[3, 3] = 1
sfm_pose[2] = transformations.quaternion_from_matrix(sfm_rot, isprecise=True)
img_path = osp.join(self.img_dir, str(data.rel_path))
img = imread(img_path) / 255.0
# Some are grayscale:
if len(img.shape) == 2:
img = np.repeat(np.expand_dims(img, 2), 3, axis=2)
mask = np.expand_dims(data.mask, 2)
# Adjust to 0 indexing
bbox = np.array(
[data.bbox.x1, data.bbox.y1, data.bbox.x2, data.bbox.y2],
float) - 1
parts = data.parts.T.astype(float)
kp = np.copy(parts)
vis = kp[:, 2] > 0
kp[vis, :2] -= 1
# Peturb bbox
if self.opts.split == 'train':
bbox = image_utils.peturb_bbox(
bbox, pf=self.padding_frac, jf=self.jitter_frac)
else:
bbox = image_utils.peturb_bbox(
bbox, pf=self.padding_frac, jf=0)
bbox = image_utils.square_bbox(bbox)
# crop image around bbox, translate kps
img, mask, kp, sfm_pose = self.crop_image(img, mask, bbox, kp, vis, sfm_pose)
# scale image, and mask. And scale kps.
img, mask, kp, sfm_pose = self.scale_image(img, mask, kp, vis, sfm_pose)
# Mirror image on random.
if self.opts.split == 'train':
img, mask, kp, sfm_pose = self.mirror_image(img, mask, kp, sfm_pose)
# Normalize kp to be [-1, 1]
img_h, img_w = img.shape[:2]
kp_norm, sfm_pose = self.normalize_kp(kp, sfm_pose, img_h, img_w)
# Finally transpose the image to 3xHxW
img = np.transpose(img, (2, 0, 1))
return img, kp_norm, mask, sfm_pose
def diff_vp(self,
verts,
cam=None,
angle=90,
axis=[1, 0, 0],
texture=None,
kp_verts=None,
new_ext=None,
extra_elev=False):
if cam is None:
cam = self.default_cam[0]
if new_ext is None:
new_ext = [0.6, 0, 0]
# Cam is 7D: [s, tx, ty, rot]
import cv2
cam = asVariable(cam)
quat = cam[-4:].view(1, 1, -1)
R = transformations.quaternion_matrix(
quat.squeeze().data.cpu().numpy())[:3, :3]
rad_angle = np.deg2rad(angle)
rotate_by = cv2.Rodrigues(rad_angle * np.array(axis))[0]
# new_R = R.dot(rotate_by)
new_R = rotate_by.dot(R)
if extra_elev:
# Left multiply the camera by 30deg on X.
R_elev = cv2.Rodrigues(np.array([np.pi / 9, 0, 0]))[0]
new_R = R_elev.dot(new_R)
# Make homogeneous
new_R = np.vstack(
[np.hstack((new_R, np.zeros((3, 1)))),
np.array([0, 0, 0, 1])])
new_quat = transformations.quaternion_from_matrix(new_R,
isprecise=True)
new_quat = Variable(torch.Tensor(new_quat).cuda(), requires_grad=False)
# new_cam = torch.cat([cam[:-4], new_quat], 0)
new_ext = Variable(torch.Tensor(new_ext).cuda(), requires_grad=False)
new_cam = torch.cat([new_ext, new_quat], 0)
rend_img = self.__call__(verts, cams=new_cam, texture=texture)
if kp_verts is None:
return rend_img
else:
kps = self.renderer.project_points(kp_verts.unsqueeze(0),
new_cam.unsqueeze(0))
kps = kps[0].data.cpu().numpy()
return kp2im(kps, rend_img, radius=1)
def mirror_image(self, img, mask, kp, sfm_pose):
kp_perm = self.kp_perm
if np.random.rand(1) > 0.5:
# Need copy bc torch collate doesnt like neg strides
img_flip = img[:, ::-1, :].copy()
mask_flip = mask[:, ::-1].copy()
# Flip kps.
new_x = img.shape[1] - kp[:, 0] - 1
kp_flip = np.hstack((new_x[:, None], kp[:, 1:]))
kp_flip = kp_flip[kp_perm, :]
# Flip sfm_pose Rot.
R = transformations.quaternion_matrix(sfm_pose[2])
flip_R = np.diag([-1, 1, 1, 1]).dot(R.dot(np.diag([-1, 1, 1, 1])))
sfm_pose[2] = transformations.quaternion_from_matrix(flip_R, isprecise=True)
# Flip tx
tx = img.shape[1] - sfm_pose[1][0] - 1
sfm_pose[1][0] = tx
return img_flip, mask_flip, kp_flip, sfm_pose
else:
return img, mask, kp, sfm_pose
def pub_ee_frame_velocity(self,
direction='z',
vel_scale=1.0,
duration_sec=0.1):
target_pos, target_quat = self.get_target_pose()
T = transformations.quaternion_matrix(target_quat)
T[:3, 3] = target_pos
dT = np.eye(4)
if direction == 'x':
dT[0, 3] = vel_scale * 0.005
if direction == 'y':
dT[1, 3] = vel_scale * 0.005
if direction == 'z':
dT[2, 3] = vel_scale * 0.005
T_post_vel = T.dot(dT)
new_target_pos = T_post_vel[:3, 3]
new_target_quat = transformations.quaternion_from_matrix(T_post_vel)
new_target_pose = np.concatenate([new_target_pos, new_target_quat])
self.set_target_pose(new_target_pose)
def quatswap(quat):
mat = transformations.quaternion_matrix(quat)
mat_new = np.c_[mat[:,2],mat[:,1],-mat[:,0],mat[:,3]]
return transformations.quaternion_from_matrix(mat_new)
Tw1 = tf.quaternion_matrix(q1)
Tw2 = tf.quaternion_matrix(q2)
# replace fourth column in T1 by the position
Tw1[:, 3] = p1
Tw2[:, 3] = p2
# compute relative transformation between Tw1 and Tw2
T12 = inv(Tw1) * Tw2
# get relative traslation
t12 = T12[:3, 3]
# get relative quaternion
R12 = T12[:3, :3]
q12 = tf.quaternion_from_matrix(T12)
#print ("T12",T12)
print("t12", t12)
print("q12", q12)
#############################################
## Compute Rotation between IMU and base_link
#############################################
# Sequence 01
# Rotation
R1_baselink_cam = np.array(
[[4.25132721e-03, -3.28406911e-01, 9.44526774e-01],
[-9.99990705e-01, -7.17997435e-04, 4.25132721e-03],
[-7.17997435e-04, -9.44536069e-01, -3.28406911e-01]])
def mat2quat(mat33):
mat44 = np.eye(4)
mat44[:3,:3] = mat33
return transformations.quaternion_from_matrix(mat44)
def hmat_to_pose(hmat):
quat = transformations.quaternion_from_matrix(hmat)
trans = hmat[:3,3]
return trans_rot_to_pose(trans, quat)
def main():
"""Main."""
parser = argparse.ArgumentParser()
parser.description = "Convert S-PTAM log file to TUM format"
parser.add_argument(
'-s',
'--sptam-log',
required=True,
help=
("S-PTAM log file (format: TRACKED_FRAME_POSE: timestamp frame_number r00 r01 r02 tx r10 r11 r12 ty r20 r21 r22 tz Cov00 .. Covxx)"
))
parser.add_argument('-o',
'--output',
default="sptam_tum.txt",
required=False,
help=("Output file"))
# parse cmdline args
parser_args = vars(parser.parse_args())
in_sptam_log_file = parser_args["sptam_log"]
out_new_format = parser_args["output"]
# Convert S-PTAM log file to TUM format
if in_sptam_log_file is None:
logger.warn("S-PTAM log file not provided")
return
else:
logger.info("Converting S-PTAM log file to TUM format...")
with open(in_sptam_log_file) as inputFile:
with open(out_new_format, 'w') as outputFile:
writer = csv.writer(outputFile, delimiter=' ')
reader = csv.reader(inputFile, delimiter=' ')
timestamp = 0
for line in reader:
if "FRAME_TIMESTAMP" == line[0]:
sec = np.uint64(line[2])
nsec = np.uint64(line[3])
time = Time(sec, nsec)
timestamp = time.to_sec()
if "TRACKED_FRAME_POSE" == line[0]:
# get sptam pose
sptam_pose = np.array(line[2:14])
transformation = np.reshape(sptam_pose.astype(float),
(3, 4))
traslation = transformation[:, 3]
rotation = transformation[:3, :3]
# quaternion has the form [w, x, y, z]
quaternion = tf.quaternion_from_matrix(rotation)
# TUM quaternion format is [x, y, z, w]
quaternion = np.array([
quaternion[1], quaternion[2], quaternion[3],
quaternion[0]
])
# create TUM format
newRow = [timestamp
] + list(traslation) + list(quaternion)
writer.writerow(newRow)
# close files
inputFile.close()
outputFile.close()
def main():
"""Main."""
parser = argparse.ArgumentParser()
parser.description = "Convert a KITTI ground-truth file to TUM format. The timestamps are extracted from the kitti rosbag used to run the sequence"
parser.add_argument(
'-k',
'--kitti-gt',
required=True,
help=
("KITTI ground-truth file (format: r00 r01 r02 tx r10 r11 r12 ty r20 r21 r22 tz)"
))
parser.add_argument(
'-r',
'--rosbag-file',
required=True,
help=("Rosbag file to extract the timestamp of frames."))
parser.add_argument('-o',
'--output',
default="gt_tum_kittiXX.txt",
required=False,
help=("Output file"))
# parse cmdline args
parser_args = vars(parser.parse_args())
in_kitti_gt_file = parser_args["kitti_gt"]
in_rosbag_file = parser_args["rosbag_file"]
out_new_format = parser_args["output"]
# Convert KITTI ground-truth with times file to TUM format
if in_kitti_gt_file is None:
logger.warn("KITTI ground-truth file not provided")
return
else:
logger.info("Converting KITTI ground-truth file to TUM format...")
# get timestamps from rosbag
timestamps = []
for topic, msg, t in rosbag.Bag(in_rosbag_file, 'r').read_messages():
# set seq for left camera_info message
if topic == "kitti_stereo/left/image_rect":
timestamp = msg.header.stamp.to_sec()
timestamps.append(timestamp)
with open(in_kitti_gt_file) as kitti_gt_file:
with open(out_new_format, 'w') as outputFile:
writer = csv.writer(outputFile, delimiter=' ')
kitti_gt_reader = csv.reader(kitti_gt_file, delimiter=' ')
# each line in the kitti is a pose
for kitti_gt_line, timestamp in zip(kitti_gt_reader,
timestamps):
# get pose
kitti_pose = np.array(kitti_gt_line)
transformation = np.reshape(kitti_pose.astype(float),
(3, 4))
traslation = transformation[:, 3]
rotation = transformation[:3, :3]
# quaternion has the form [w, x, y, z]
quaternion = tf.quaternion_from_matrix(rotation)
# TUM quaternion format is [x, y, z, w]
quaternion = np.array([
quaternion[1], quaternion[2], quaternion[3],
quaternion[0]
])
# create TUM format
newRow = [timestamp] + list(traslation) + list(quaternion)
writer.writerow(newRow)
# check that ground-truth and rosbag timestamps have the same length
assert kitti_gt_reader.line_num == len(timestamps)
# close files
kitti_gt_file.close()
outputFile.close()
def quaternion_from_matrix(matrix, isprecise=False):
trans_q = trans.quaternion_from_matrix(matrix)
return to_pyquat(trans_q)
