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)