def camera_to_world_pose(self, radius, elev, az, roll, x, y):
        """Convert spherical coords to a camera pose in the world."""
        # generate camera center from spherical coords
        delta_t = np.array([x, y, 0])
        camera_z = np.array([sph2cart(radius, az, elev)]).squeeze()
        camera_center = camera_z + delta_t
        camera_z = -camera_z / np.linalg.norm(camera_z)

        # find the canonical camera x and y axes
        camera_x = np.array([camera_z[1], -camera_z[0], 0])
        x_norm = np.linalg.norm(camera_x)
        if x_norm == 0:
            camera_x = np.array([1, 0, 0])
        else:
            camera_x = camera_x / x_norm
        camera_y = np.cross(camera_z, camera_x)
        camera_y = camera_y / np.linalg.norm(camera_y)

        # Reverse the x direction if needed so that y points down
        if camera_y[2] > 0:
            camera_x = -camera_x
            camera_y = np.cross(camera_z, camera_x)
            camera_y = camera_y / np.linalg.norm(camera_y)

        # rotate by the roll
        R = np.vstack((camera_x, camera_y, camera_z)).T
        roll_rot_mat = transformations.rotation_matrix(roll, camera_z,
                                                       np.zeros(3))[:3, :3]
        R = roll_rot_mat.dot(R)
        T_camera_world = RigidTransform(R,
                                        camera_center,
                                        from_frame=self.frame,
                                        to_frame="world")

        return T_camera_world
    def object_to_camera_pose(self, radius, elev, az, roll):
        """ Convert spherical coords to an object-camera pose. """
        # generate camera center from spherical coords
        camera_center_obj = np.array([sph2cart(radius, az, elev)]).squeeze()
        camera_z_obj = -camera_center_obj / np.linalg.norm(camera_center_obj)

        # find the canonical camera x and y axes
        camera_x_par_obj = np.array([camera_z_obj[1], -camera_z_obj[0], 0])
        if np.linalg.norm(camera_x_par_obj) == 0:
            camera_x_par_obj = np.array([1, 0, 0])
        camera_x_par_obj = camera_x_par_obj / np.linalg.norm(camera_x_par_obj)
        camera_y_par_obj = np.cross(camera_z_obj, camera_x_par_obj)
        camera_y_par_obj = camera_y_par_obj / np.linalg.norm(camera_y_par_obj)
        if camera_y_par_obj[2] > 0:
            camera_x_par_obj = -camera_x_par_obj
            camera_y_par_obj = np.cross(camera_z_obj, camera_x_par_obj)
            camera_y_par_obj = camera_y_par_obj / np.linalg.norm(
                camera_y_par_obj)

        # rotate by the roll
        R_obj_camera_par = np.c_[camera_x_par_obj, camera_y_par_obj,
                                 camera_z_obj]
        R_camera_par_camera = np.array([[np.cos(roll), -np.sin(roll), 0],
                                        [np.sin(roll),
                                         np.cos(roll), 0], [0, 0, 1]])
        R_obj_camera = R_obj_camera_par.dot(R_camera_par_camera)
        t_obj_camera = camera_center_obj

        # create final transform
        T_obj_camera = RigidTransform(R_obj_camera,
                                      t_obj_camera,
                                      from_frame=self.frame,
                                      to_frame='obj')
        return T_obj_camera.inverse()
Exemple #3
0
    def object_to_camera_poses(self, camera_intr):
        """Turn the params into a set of object to camera transformations.

        Returns
        -------
        :obj:`list` of :obj:`RigidTransform`
            A list of rigid transformations that transform from object space
            to camera space.
        :obj:`list` of :obj:`RigidTransform`
            A list of rigid transformations that transform from object space
            to camera space without the translation in the plane
        :obj:`list` of :obj:`CameraIntrinsics`
            A list of camera intrinsics that project the translated object
            into the center pixel of the camera, simulating cropping
        """
        # compute increments in radial coordinates
        if self.max_radius == self.min_radius:
            radius_inc = 1
        elif self.num_radii == 1:
            radius_inc = self.max_radius - self.min_radius + 1
        else:
            radius_inc = (self.max_radius - self.min_radius) / (self.num_radii - 1)
        az_inc = (self.max_az - self.min_az) / self.num_az
        if self.max_elev == self.min_elev:
            elev_inc = 1
        elif self.num_elev == 1:
            elev_inc = self.max_elev - self.min_elev + 1
        else:
            elev_inc = (self.max_elev - self.min_elev) / (self.num_elev - 1)
        roll_inc = (self.max_roll - self.min_roll) / self.num_roll

        if self.max_x == self.min_x:
            x_inc = 1
        elif self.num_x == 1:
            x_inc = self.max_x - self.min_x + 1
        else:
            x_inc = (self.max_x - self.min_x) / (self.num_x - 1)


        if self.max_y == self.min_y:
            y_inc = 1
        elif self.num_y == 1:
            y_inc = self.max_y - self.min_y + 1
        else:
            y_inc = (self.max_y - self.min_y) / (self.num_y - 1)

        # create a pose for each set of spherical coords
        object_to_camera_poses = []
        object_to_camera_normalized_poses = []
        camera_shifted_intrinsics = []
        radius = self.min_radius
        while radius <= self.max_radius:
            elev = self.min_elev
            while elev <= self.max_elev:
                az = self.min_az
                while az < self.max_az: #not inclusive due to topology (simplifies things)
                    roll = self.min_roll
                    while roll < self.max_roll:
                        x = self.min_x
                        while x <= self.max_x:
                            y = self.min_y
                            while y <= self.max_y:
                                num_poses = len(object_to_camera_poses)

                                # generate camera center from spherical coords
                                delta_t = np.array([x, y, 0])
                                camera_center_obj = np.array([sph2cart(radius, az, elev)]).squeeze() + delta_t
                                camera_z_obj = -np.array([sph2cart(radius, az, elev)]).squeeze()
                                camera_z_obj = camera_z_obj / np.linalg.norm(camera_z_obj)

                                # find the canonical camera x and y axes
                                camera_x_par_obj = np.array([camera_z_obj[1], -camera_z_obj[0], 0])
                                if np.linalg.norm(camera_x_par_obj) == 0:
                                    camera_x_par_obj = np.array([1, 0, 0])
                                camera_x_par_obj = camera_x_par_obj / np.linalg.norm(camera_x_par_obj)
                                camera_y_par_obj = np.cross(camera_z_obj, camera_x_par_obj)
                                camera_y_par_obj = camera_y_par_obj / np.linalg.norm(camera_y_par_obj)
                                if camera_y_par_obj[2] > 0:
                                    print 'Flipping', num_poses
                                    camera_x_par_obj = -camera_x_par_obj
                                    camera_y_par_obj = np.cross(camera_z_obj, camera_x_par_obj)
                                    camera_y_par_obj = camera_y_par_obj / np.linalg.norm(camera_y_par_obj)

                                # rotate by the roll
                                R_obj_camera_par = np.c_[camera_x_par_obj, camera_y_par_obj, camera_z_obj]
                                R_camera_par_camera = np.array([[np.cos(roll), -np.sin(roll), 0],
                                                                [np.sin(roll), np.cos(roll), 0],
                                                                [0, 0, 1]])
                                R_obj_camera = R_obj_camera_par.dot(R_camera_par_camera)
                                t_obj_camera = camera_center_obj

                                # create final transform
                                T_obj_camera = RigidTransform(R_obj_camera, t_obj_camera,
                                                              from_frame='camera',
                                                              to_frame='obj')
                            
                                object_to_camera_poses.append(T_obj_camera.inverse())

                                # compute pose without the center offset because we can easily add in the object offset later
                                t_obj_camera_normalized = camera_center_obj - delta_t
                                T_obj_camera_normalized = RigidTransform(R_obj_camera, t_obj_camera_normalized,
                                                                         from_frame='camera',
                                                                         to_frame='obj')
                                object_to_camera_normalized_poses.append(T_obj_camera_normalized.inverse())

                                # compute new camera center by projecting object 0,0,0 into the camera
                                center_obj_obj = Point(np.zeros(3), frame='obj')
                                center_obj_camera = T_obj_camera.inverse() * center_obj_obj
                                u_center_obj = camera_intr.project(center_obj_camera)
                                camera_shifted_intr = copy.deepcopy(camera_intr)
                                camera_shifted_intr.cx = 2 * camera_intr.cx - float(u_center_obj.x)
                                camera_shifted_intr.cy = 2 * camera_intr.cy - float(u_center_obj.y)
                                camera_shifted_intrinsics.append(camera_shifted_intr)

                                y += y_inc
                            x += x_inc
                        roll += roll_inc
                    az += az_inc
                elev += elev_inc
            radius += radius_inc
        return object_to_camera_poses, object_to_camera_normalized_poses, camera_shifted_intrinsics
Exemple #4
0
    def object_to_camera_poses(self):
        """Turn the params into a set of object to camera transformations.

        Returns
        -------
        :obj:`list` of :obj:`RigidTransform`
            A list of rigid transformations that transform from object space
            to camera space.
        """
        # compute increments in radial coordinates
        if self.max_radius == self.min_radius:
            radius_inc = 1
        elif self.num_radii == 1:
            radius_inc = self.max_radius - self.min_radius + 1
        else:
            radius_inc = (self.max_radius - self.min_radius) / (self.num_radii - 1)
        az_inc = (self.max_az - self.min_az) / self.num_az
        if self.max_elev == self.min_elev:
            elev_inc = 1
        elif self.num_elev == 1:
            elev_inc = self.max_elev - self.min_elev + 1
        else:
            elev_inc = (self.max_elev - self.min_elev) / (self.num_elev - 1)
        roll_inc = (self.max_roll - self.min_roll) / self.num_roll

        # create a pose for each set of spherical coords
        object_to_camera_poses = []
        radius = self.min_radius
        while radius <= self.max_radius:
            elev = self.min_elev
            while elev <= self.max_elev:
                az = self.min_az
                while az < self.max_az: #not inclusive due to topology (simplifies things)
                    roll = self.min_roll
                    while roll < self.max_roll:

                        # generate camera center from spherical coords
                        camera_center_obj = np.array([sph2cart(radius, az, elev)]).squeeze()
                        camera_z_obj = -camera_center_obj / np.linalg.norm(camera_center_obj)

                        # find the canonical camera x and y axes
                        camera_x_par_obj = np.array([camera_z_obj[1], -camera_z_obj[0], 0])
                        if np.linalg.norm(camera_x_par_obj) == 0:
                            camera_x_par_obj = np.array([1, 0, 0])
                        camera_x_par_obj = camera_x_par_obj / np.linalg.norm(camera_x_par_obj)
                        camera_y_par_obj = np.cross(camera_z_obj, camera_x_par_obj)
                        camera_y_par_obj = camera_y_par_obj / np.linalg.norm(camera_y_par_obj)
                        if camera_y_par_obj[2] > 0:
                            camera_x_par_obj = -camera_x_par_obj
                            camera_y_par_obj = np.cross(camera_z_obj, camera_x_par_obj)
                            camera_y_par_obj = camera_y_par_obj / np.linalg.norm(camera_y_par_obj)

                        # rotate by the roll
                        R_obj_camera_par = np.c_[camera_x_par_obj, camera_y_par_obj, camera_z_obj]
                        R_camera_par_camera = np.array([[np.cos(roll), -np.sin(roll), 0],
                                                        [np.sin(roll), np.cos(roll), 0],
                                                        [0, 0, 1]])
                        R_obj_camera = R_obj_camera_par.dot(R_camera_par_camera)
                        t_obj_camera = camera_center_obj

                        # create final transform
                        T_obj_camera = RigidTransform(R_obj_camera, t_obj_camera,
                                                      from_frame='camera', to_frame='obj')
                        object_to_camera_poses.append(T_obj_camera.inverse())
                        roll += roll_inc
                    az += az_inc
                elev += elev_inc
            radius += radius_inc
        return object_to_camera_poses
    def get_camera_poses(config, frame='world'):
        """Get a list of camera-to-frame poses broken up uniformly about a viewsphere.

        Parameters
        ----------
        config : autolab_core.YamlConfig
            A configuration containing parameters of the random variable.
        frame : str
            The name of the target world frame.

        Notes
        -----
        Required parameters of config are specified in Other Parameters.

        Other Parameters
        ----------------
        radius:               Distance from camera to world origin.
            min : float
            max : float
            n   : int
        azimuth:              Azimuth (angle from x-axis) of camera in degrees.
            min : float
            max : float
            n   : int
        elevation:            Elevation (angle from z-axis) of camera in degrees.
            min : float
            max : float
            n   : int
        roll:                 Roll (angle about view direction) of camera in degrees.
            min : float
            max : float
            n   : int

        Returns
        -------
        list of autolab_core.RigidTransform
            A list of camera-to-frame transforms.
        """
        min_radius = config['radius']['min']
        max_radius = config['radius']['max']
        num_radius = config['radius']['n']
        radii = np.linspace(min_radius, max_radius, num_radius)

        min_azimuth = config['azimuth']['min']
        max_azimuth = config['azimuth']['max']
        num_azimuth = config['azimuth']['n']
        azimuths = np.linspace(min_azimuth, max_azimuth, num_azimuth)

        min_elev = config['elev']['min']
        max_elev = config['elev']['max']
        num_elev = config['elev']['n']
        elevs = np.linspace(min_elev, max_elev, num_elev)

        min_roll = config['roll']['min']
        max_roll = config['roll']['max']
        num_roll = config['roll']['n']
        rolls = np.linspace(min_roll, max_roll, num_roll)

        camera_to_frame_tfs = []
        for r in radii:
            for a in azimuths:
                for e in elevs:
                    for roll in rolls:
                        cam_center = np.array([sph2cart(r, a, e)]).squeeze()
                        cz = -cam_center / np.linalg.norm(cam_center)
                        cx = np.array([cz[1], -cz[0], 0])
                        if np.linalg.norm(cx) == 0:
                            cx = np.array([1.0, 0.0, 0.0])
                        cx = cx / np.linalg.norm(cx)
                        cy = np.cross(cz, cx)
                        cy = cy / np.linalg.norm(cy)
                        if cy[2] > 0:
                            cx = -cx
                            cy = np.cross(cz, cx)
                            cy = cy / np.linalg.norm(cy)
                        R_cam_frame = np.array([cx, cy, cz]).T
                        R_roll = RigidTransform.z_axis_rotation(roll)
                        R_cam_frame = R_cam_frame.dot(R_roll)

                        T_camera_frame = RigidTransform(R_cam_frame, cam_center,
                                                        from_frame='camera', to_frame=frame)
                        camera_to_frame_tfs.append(T_camera_frame)
        return camera_to_frame_tfs