Example #1
0
 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)
Example #2
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
Example #3
0
File: base.py Project: Ized06/cmr
    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
Example #4
0
    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)
Example #5
0
File: base.py Project: Ized06/cmr
 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
Example #6
0
    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)
Example #8
0
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]])
Example #9
0
def mat2quat(mat33):
    mat44 = np.eye(4)
    mat44[:3,:3] = mat33
    return transformations.quaternion_from_matrix(mat44)
Example #10
0
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()
Example #13
0
def quaternion_from_matrix(matrix, isprecise=False):
    trans_q = trans.quaternion_from_matrix(matrix)
    return to_pyquat(trans_q)