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