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