def projection_matrix_pseudo_inverse(proj_mat, batch_shape=None):
"""
Given projection matrix :math:`\mathbf{P}\in\mathbb{R}^{3×4}`, compute it's pseudo-inverse
:math:`\mathbf{P}^+\in\mathbb{R}^{4×3}`.\n
`[reference] <localhost:63342/ivy/docs/source/references/mvg_textbook.pdf?_ijt=25ihpil89dmfo4da975v402ogc#page=179>`_
bottom of page 161, section 6.2.2
:param proj_mat: Projection matrix *[batch_shape,3,4]*
:type proj_mat: array
:param batch_shape: Shape of batch. Inferred from inputs if None.
:type batch_shape: sequence of ints, optional
:return: Projection matrix pseudo-inverse *[batch_shape,4,3]*
"""
if batch_shape is None:
batch_shape = proj_mat.shape[:-2]
# shapes as list
batch_shape = list(batch_shape)
# transpose idxs
num_batch_dims = len(batch_shape)
transpose_idxs = list(
range(num_batch_dims)) + [num_batch_dims + 1, num_batch_dims]
# BS x 4 x 3
matrix_transposed = _ivy.transpose(proj_mat, transpose_idxs)
# BS x 4 x 3
return _ivy.matmul(matrix_transposed,
_ivy.inv(_ivy.matmul(proj_mat, matrix_transposed)))
def projection_matrix_inverse(proj_mat):
"""
Given projection matrix :math:`\mathbf{P}\in\mathbb{R}^{3×4}`, compute it's inverse
:math:`\mathbf{P}^{-1}\in\mathbb{R}^{3×4}`.\n
`[reference] <https://github.com/pranjals16/cs676/blob/master/Hartley%2C%20Zisserman%20-%20Multiple%20View%20Geometry%20in%20Computer%20Vision.pdf#page=174>`_
middle of page 156, section 6.1, eq 6.6
:param proj_mat: Projection matrix *[batch_shape,3,4]*
:type proj_mat: array
:return: Projection matrix inverse *[batch_shape,3,4]*
"""
# BS x 3 x 3
rotation_matrix = proj_mat[..., 0:3]
# BS x 3 x 3
rotation_matrix_inverses = _ivy.inv(rotation_matrix)
# BS x 3 x 1
translations = proj_mat[..., 3:4]
# BS x 3 x 1
translation_inverses = -_ivy.matmul(rotation_matrix_inverses, translations)
# BS x 3 x 4
return _ivy.concatenate((rotation_matrix_inverses, translation_inverses),
-1)
def calib_mat_to_intrinsics_object(calib_mat, image_dims, batch_shape=None):
"""
Create camera intrinsics object from calibration matrix.
:param calib_mat: Calibration matrices *[batch_shape,3,3]*
:type calib_mat: array
:param image_dims: Image dimensions. Inferred from inputs in None.
:type image_dims: sequence of ints
:param batch_shape: Shape of batch. Inferred from inputs if None.
:type batch_shape: sequence of ints, optional
:return: Camera intrinsics object
"""
if batch_shape is None:
batch_shape = calib_mat.shape[:-2]
# shapes as list
batch_shape = list(batch_shape)
image_dims = list(image_dims)
# BS x 2
focal_lengths = _ivy.concatenate((calib_mat[..., 0, 0:1], calib_mat[..., 1, 1:2]), -1)
# BS x 2
persp_angles = focal_lengths_to_persp_angles(focal_lengths, image_dims)
# BS x 2
pp_offsets = _ivy.concatenate((calib_mat[..., 0, -1:], calib_mat[..., 1, -1:]), -1)
# BS x 3 x 3
inv_calib_mat = _ivy.inv(calib_mat)
# intrinsics object
intrinsics = _Intrinsics(focal_lengths, persp_angles, pp_offsets, calib_mat, inv_calib_mat)
return intrinsics
def focal_lengths_and_pp_offsets_to_intrinsics_object(focal_lengths, pp_offsets, image_dims, batch_shape=None):
"""
Create camera intrinsics object from focal lengths :math:`f_x, f_y`, principal-point offsets :math:`p_x, p_y`, and
image dimensions [height, width].
:param focal_lengths: Focal lengths *[batch_shape,2]*
:type focal_lengths: array
:param pp_offsets: Principal-point offsets *[batch_shape,2]*
:type pp_offsets: array
:param image_dims: Image dimensions. Inferred from inputs in None.
:type image_dims: sequence of ints
:param batch_shape: Shape of batch. Inferred from inputs if None.
:type batch_shape: sequence of ints, optional
:return: Camera intrinsics object
"""
if batch_shape is None:
batch_shape = focal_lengths.shape[:-1]
# shapes as list
batch_shape = list(batch_shape)
image_dims = list(image_dims)
# BS x 2
persp_angles = focal_lengths_to_persp_angles(focal_lengths, image_dims)
# BS x 3 x 3
calib_mat = focal_lengths_and_pp_offsets_to_calib_mat(focal_lengths, pp_offsets, batch_shape)
# BS x 3 x 3
inv_calib_mat = _ivy.inv(calib_mat)
# intrinsics object
intrinsics = _Intrinsics(focal_lengths, persp_angles, pp_offsets, calib_mat, inv_calib_mat)
return intrinsics
def setup_primitive_scene(self):
# shape matrices
shape_matrices = ivy.concatenate([ivy.reshape(ivy.array(obj.get_matrix(), 'float32'), (1, 3, 4))
for obj in self._objects], 0)
# shape dims
x_dims = ivy.concatenate([ivy.reshape(ivy.array(
obj.get_bounding_box()[1] - obj.get_bounding_box()[0], 'float32'), (1, 1)) for obj in self._objects], 0)
y_dims = ivy.concatenate([ivy.reshape(ivy.array(
obj.get_bounding_box()[3] - obj.get_bounding_box()[2], 'float32'), (1, 1)) for obj in self._objects], 0)
z_dims = ivy.concatenate([ivy.reshape(ivy.array(
obj.get_bounding_box()[5] - obj.get_bounding_box()[4], 'float32'), (1, 1)) for obj in self._objects], 0)
shape_dims = ivy.concatenate((x_dims, y_dims, z_dims), -1)
# primitve scene visualization
if self._with_primitive_scene_vis:
scene_vis = [Shape.create(PrimitiveShape.CUBOID, ivy.to_numpy(shape_dim).tolist())
for shape_dim in shape_dims]
[obj.set_matrix(ivy.to_numpy(shape_mat).reshape(-1).tolist())
for shape_mat, obj in zip(shape_matrices, scene_vis)]
[obj.set_transparency(0.5) for obj in scene_vis]
# sdf
primitive_scene = PrimitiveScene(cuboid_ext_mats=ivy.inv(ivy_mech.make_transformation_homogeneous(
shape_matrices))[..., 0:3, :], cuboid_dims=shape_dims)
self.sdf = primitive_scene.sdf
def measure_incremental_mat(self):
inv_ext_mat = ivy.reshape(ivy.array(self._handle.get_matrix()), (3, 4))
inv_ext_mat_homo = ivy_mech.make_transformation_homogeneous(inv_ext_mat)
ext_mat_homo = ivy.inv(inv_ext_mat_homo)
ext_mat = ext_mat_homo[0:3, :]
rel_mat = ivy.matmul(ext_mat, self._inv_ext_mat_homo)
self._inv_ext_mat_homo = inv_ext_mat_homo
return rel_mat
def get_fundamental_matrix(full_mat1,
full_mat2,
camera_center1=None,
pinv_full_mat1=None,
batch_shape=None,
dev_str=None):
"""
Compute fundamental matrix :math:`\mathbf{F}\in\mathbb{R}^{3×3}` between two cameras, given their extrinsic
matrices :math:`\mathbf{E}_1\in\mathbb{R}^{3×4}` and :math:`\mathbf{E}_2\in\mathbb{R}^{3×4}`.\n
`[reference] <localhost:63342/ivy/docs/source/references/mvg_textbook.pdf#page=262>`_
bottom of page 244, section 9.2.2, equation 9.1
:param full_mat1: Frame 1 full projection matrix *[batch_shape,3,4]*
:type full_mat1: array
:param full_mat2: Frame 2 full projection matrix *[batch_shape,3,4]*
:type full_mat2: array
:param camera_center1: Frame 1 camera center, inferred from full_mat1 if None *[batch_shape,3,1]*
:type camera_center1: array, optional
:param pinv_full_mat1: Frame 1 full projection matrix pseudo-inverse, inferred from full_mat1 if None *[batch_shape,4,3]*
:type pinv_full_mat1: array, optional
:param batch_shape: Shape of batch. Inferred from inputs if None.
:type batch_shape: sequence of ints, optional
:param dev_str: device on which to create the array 'cuda:0', 'cuda:1', 'cpu' etc. Same as x if None.
:type dev_str: str, optional
:return: Fundamental matrix connecting frames 1 and 2 *[batch_shape,3,3]*
"""
if batch_shape is None:
batch_shape = full_mat1.shape[:-2]
if dev_str is None:
dev_str = _ivy.dev_str(full_mat1)
# shapes as list
batch_shape = list(batch_shape)
if camera_center1 is None:
inv_full_mat1 = _ivy.inv(
_ivy_mech.make_transformation_homogeneous(full_mat1, batch_shape,
dev_str))[..., 0:3, :]
camera_center1 = _ivy_svg.inv_ext_mat_to_camera_center(inv_full_mat1)
if pinv_full_mat1 is None:
pinv_full_mat1 = _ivy.pinv(full_mat1)
# BS x 4 x 1
camera_center1_homo = _ivy.concatenate(
(camera_center1, _ivy.ones(batch_shape + [1, 1], dev_str=dev_str)), -2)
# BS x 3
e2 = _ivy.matmul(full_mat2, camera_center1_homo)[..., -1]
# BS x 3 x 3
e2_skew_symmetric = _ivy.linalg.vector_to_skew_symmetric_matrix(e2)
# BS x 3 x 3
return _ivy.matmul(e2_skew_symmetric, _ivy.matmul(full_mat2,
pinv_full_mat1))
def triangulate_depth(ds_pixel_coords,
full_mats,
inv_full_mats=None,
camera_centers=None,
method='cmp',
batch_shape=None,
image_dims=None):
"""
Triangulate depth in frame 1, returning depth scaled homogeneous pixel co-ordinate image
:math:`\mathbf{X}\in\mathbb{R}^{h×w×3}` in frame 1.\n
:param ds_pixel_coords: Homogeneous pixel co-ordinate images: *[batch_shape,h,w,3]*
:type ds_pixel_coords: array
:param full_mats: Full projection matrices *[batch_shape,2,3,4]*
:type full_mats: array
:param inv_full_mats: Inverse full projection matrices, required for closest_mutual_points method *[batch_shape,2,3,4]*
:type inv_full_mats: array, optional
:param camera_centers: Camera centers, required for closest_mutual_points method *[batch_shape,2,3,1]*
:type camera_centers: array, optional
:param method: Triangulation method, one of [cmp|dlt], for closest mutual points or homogeneous dlt approach, closest_mutual_points by default
:type method: str, optional
:param batch_shape: Shape of batch. Inferred from inputs if None.
:type batch_shape: sequence of ints, optional
:param image_dims: Image dimensions. Inferred from inputs in None.
:type image_dims: sequence of ints, optional
:return: Depth scaled homogeneous pixel co-ordinates image in frame 1 *[batch_shape,h,w,3]*
"""
if batch_shape is None:
batch_shape = ds_pixel_coords.shape[:-4]
if image_dims is None:
image_dims = ds_pixel_coords.shape[-3:-1]
# shapes as list
batch_shape = list(batch_shape)
image_dims = list(image_dims)
if method == 'cmt':
if inv_full_mats is None:
inv_full_mats = _ivy.inv(
_ivy_mech.make_transformation_homogeneous(
full_mats, batch_shape + [2]))[..., 0:3, :]
if camera_centers is None:
camera_centers = _ivy_svg.inv_ext_mat_to_camera_center(
inv_full_mats)
try:
return TRI_METHODS[method](ds_pixel_coords, full_mats, inv_full_mats,
camera_centers, batch_shape, image_dims)
except KeyError:
raise Exception(
'Triangulation method must be one of [cmp|dlt], but found {}'.
format(method))
def __init__(self, interactive, try_use_sim):
super().__init__(interactive, try_use_sim)
# initialize scene
if self.with_pyrep:
for i in range(6):
self._vision_sensors[i].remove()
self._vision_sensor_bodies[i].remove()
[item.remove() for item in self._vision_sensor_rays[i]]
self._default_camera.set_position(np.array([-2.3518, 4.3953, 2.8949]))
self._default_camera.set_orientation(np.array([i*np.pi/180 for i in [112.90, 27.329, -10.978]]))
inv_ext_mat = ivy.reshape(ivy.array(self._default_vision_sensor.get_matrix(), 'float32'), (3, 4))
self.default_camera_ext_mat_homo = ivy.inv(ivy_mech.make_transformation_homogeneous(inv_ext_mat))
# public objects
self.omcam = SimCam(self._spherical_vision_sensor)
# wait for user input
self._user_prompt('\nInitialized scene with an omni-directional camera in the centre.\n\n'
'You can click on the omni directional camera, which appears as a small floating black sphere, '
'then select the box icon with four arrows in the top panel of the simulator, '
'and then drag the camera around dynamically.\n'
'Starting to drag and then holding ctrl allows you to also drag the camera up and down. \n\n'
'This demo enables you to capture 10 different omni-directional images from the camera, '
'and render the associated 10 point clouds in an open3D visualizer.\n\n'
'Both visualizers can be translated and rotated by clicking either the left mouse button or the wheel, '
'and then dragging the mouse.\n'
'Scrolling the mouse wheel zooms the view in and out.\n\n'
'Both visualizers can be rotated and zoomed by clicking either the left mouse button or the wheel, '
'and then dragging with the mouse.\n\n'
'Press enter in the terminal to use method ivy_mech.polar_coords_to_cartesian_coords and '
'show the first cartesian point cloud reconstruction of the scene, '
'converted from the polar co-ordinates captured from the omni-directional camera.\n\n')
else:
# public objects
self.omcam = DummyOmCam()
self.default_camera_ext_mat_homo = ivy.array(
[[-0.872, -0.489, 0., 0.099],
[-0.169, 0.301, -0.938, 0.994],
[0.459, -0.818, -0.346, 5.677],
[0., 0., 0., 1.]])
# message
print('\nInitialized dummy scene with an omni-directional camera in the centre.'
'\nClose the visualization window to use method ivy_mech.polar_coords_to_cartesian_coords and show'
'a cartesian point cloud reconstruction of the scene, '
'converted from the omni-directional camera polar co-ordinates\n')
# plot scene before rotation
if interactive:
plt.imshow(mpimg.imread(os.path.join(os.path.dirname(os.path.realpath(__file__)),
'ptcc_no_sim', 'before_capture.png')))
plt.show()
def test_inv(x, dtype_str, tensor_fn, dev_str, call):
if call in [helpers.tf_call, helpers.tf_graph_call] and 'cpu' in dev_str:
# tf.linalg.inv segfaults when CUDA is installed, but array is on CPU
pytest.skip()
# smoke test
x = tensor_fn(x, dtype_str, dev_str)
ret = ivy.inv(x)
# type test
assert ivy.is_array(ret)
# cardinality test
assert ret.shape == x.shape
# value test
assert np.allclose(call(ivy.inv, x), ivy.numpy.inv(ivy.to_numpy(x)))
# compilation test
helpers.assert_compilable(ivy.inv)
def setup_primitive_scene_no_sim(self, box_pos=None):
# lists
shape_matrices_list = list()
shape_dims_list = list()
this_dir = os.path.dirname(os.path.realpath(__file__))
for i in range(11):
shape_mat = np.load(os.path.join(this_dir, 'no_sim/obj_inv_ext_mat_{}.npy'.format(i)))
if i == 10 and box_pos is not None:
shape_mat[..., -1:] = box_pos.reshape((1, 3, 1))
shape_matrices_list.append(ivy.array(shape_mat, 'float32'))
shape_dims_list.append(
ivy.array(np.load(os.path.join(this_dir, 'no_sim/obj_bbx_{}.npy'.format(i))), 'float32')
)
# matices
shape_matrices = ivy.concatenate(shape_matrices_list, 0)
shape_dims = ivy.concatenate(shape_dims_list, 0)
# sdf
primitive_scene = PrimitiveScene(cuboid_ext_mats=ivy.inv(ivy_mech.make_transformation_homogeneous(
shape_matrices))[..., 0:3, :], cuboid_dims=shape_dims)
self.sdf = primitive_scene.sdf
def main(f=None):
# Framework Setup #
# ----------------#
# choose random framework
f = choose_random_framework() if f is None else f
set_framework(f)
# Orientation #
# ------------#
# rotation representations
# 3
rot_vec = ivy.array([0., 1., 0.])
# 3 x 3
rot_mat = ivy_mech.rot_vec_to_rot_mat(rot_vec)
# 3
euler_angles = ivy_mech.rot_mat_to_euler(rot_mat, 'zyx')
# 4
quat = ivy_mech.euler_to_quaternion(euler_angles)
# 4
axis_and_angle = ivy_mech.quaternion_to_axis_angle(quat)
# 3
rot_vec_again = axis_and_angle[..., :-1] * axis_and_angle[..., -1:]
# Pose #
# -----#
# pose representations
# 3
position = ivy.ones_like(rot_vec)
# 6
rot_vec_pose = ivy.concatenate((position, rot_vec), 0)
# 3 x 4
mat_pose = ivy_mech.rot_vec_pose_to_mat_pose(rot_vec_pose)
# 6
euler_pose = ivy_mech.mat_pose_to_euler_pose(mat_pose)
# 7
quat_pose = ivy_mech.euler_pose_to_quaternion_pose(euler_pose)
# 6
rot_vec_pose_again = ivy_mech.quaternion_pose_to_rot_vec_pose(quat_pose)
# Position #
# ---------#
# conversions of positional representation
# 3
cartesian_coord = ivy.random_uniform(0., 1., (3, ))
# 3
polar_coord = ivy_mech.cartesian_to_polar_coords(cartesian_coord)
# 3
cartesian_coord_again = ivy_mech.polar_to_cartesian_coords(polar_coord)
# cartesian co-ordinate frame-of-reference transformations
# 3 x 4
trans_mat = ivy.random_uniform(0., 1., (3, 4))
# 4
cartesian_coord_homo = ivy_mech.make_coordinates_homogeneous(
cartesian_coord)
# 3
trans_cartesian_coord = ivy.matmul(
trans_mat, ivy.expand_dims(cartesian_coord_homo, -1))[:, 0]
# 4
trans_cartesian_coord_homo = ivy_mech.make_coordinates_homogeneous(
trans_cartesian_coord)
# 4 x 4
trans_mat_homo = ivy_mech.make_transformation_homogeneous(trans_mat)
# 3 x 4
inv_trans_mat = ivy.inv(trans_mat_homo)[0:3]
# 3
cartesian_coord_again = ivy.matmul(
inv_trans_mat, ivy.expand_dims(trans_cartesian_coord_homo, -1))[:, 0]
# message
print('End of Run Through Demo!')
def depth_from_flow_and_cam_mats(flow,
full_mats,
inv_full_mats=None,
camera_centers=None,
uniform_pixel_coords=None,
triangulation_method='cmp',
batch_shape=None,
image_dims=None,
dev_str=None):
"""
Compute depth map :math:`\mathbf{X}\in\mathbb{R}^{h×w×1}` in frame 1 using optical flow
:math:`\mathbf{U}_{1→2}\in\mathbb{R}^{h×w×2}` from frame 1 to 2, and the camera geometry.\n
:param flow: Optical flow from frame 1 to 2 *[batch_shape,h,w,2]*
:type flow: array
:param full_mats: Full projection matrices *[batch_shape,2,3,4]*
:type full_mats: array
:param inv_full_mats: Inverse full projection matrices, inferred from full_mats if None and 'cmp' triangulation method *[batch_shape,2,3,4]*
:type inv_full_mats: array, optional
:param camera_centers: Camera centers, inferred from inv_full_mats if None and 'cmp' triangulation method *[batch_shape,2,3,1]*
:type camera_centers: array, optional
:param uniform_pixel_coords: Homogeneous uniform (integer) pixel co-ordinate images, inferred from image_dims if None *[batch_shape,h,w,3]*
:type uniform_pixel_coords: array, optional
:param triangulation_method: Triangulation method, one of [cmp|dlt], for closest mutual points or homogeneous dlt approach, closest_mutual_points by default
:type triangulation_method: str, optional
:param batch_shape: Shape of batch. Inferred from inputs if None.
:type batch_shape: sequence of ints, optional
:param image_dims: Image dimensions. Inferred from inputs in None.
:type image_dims: sequence of ints, optional
:param dev_str: device on which to create the array 'cuda:0', 'cuda:1', 'cpu' etc. Same as x if None.
:type dev_str: str, optional
:return: Depth map in frame 1 *[batch_shape,h,w,1]*
"""
if batch_shape is None:
batch_shape = flow.shape[:-3]
if image_dims is None:
image_dims = flow.shape[-3:-1]
# shapes as list
batch_shape = list(batch_shape)
image_dims = list(image_dims)
if dev_str is None:
dev_str = _ivy.dev_str(flow)
if inv_full_mats is None:
inv_full_mats = _ivy.inv(
_ivy_mech.make_transformation_homogeneous(full_mats,
batch_shape + [2],
dev_str))[..., 0:3, :]
if camera_centers is None:
camera_centers = _ivy_svg.inv_ext_mat_to_camera_center(inv_full_mats)
if uniform_pixel_coords is None:
uniform_pixel_coords = _ivy_svg.create_uniform_pixel_coords_image(
image_dims, batch_shape, dev_str=dev_str)
# BS x H x W x 3
flow_homo = _ivy.concatenate(
(flow, _ivy.zeros(batch_shape + image_dims + [1], dev_str=dev_str)),
-1)
# BS x H x W x 3
transformed_pixel_coords = uniform_pixel_coords + flow_homo
# BS x 2 x H x W x 3
pixel_coords = _ivy.concatenate(
(_ivy.expand_dims(uniform_pixel_coords, -4),
_ivy.expand_dims(transformed_pixel_coords, -4)), -4)
# BS x H x W x 1
return _ivy_tvg.triangulate_depth(pixel_coords, full_mats, inv_full_mats,
camera_centers, triangulation_method,
batch_shape, image_dims)[..., -1:]
def __init__(self, interactive, try_use_sim):
super().__init__(interactive, try_use_sim)
# initialize scene
if self.with_pyrep:
self._spherical_vision_sensor.remove()
for i in range(0, 3):
[item.set_color([0.2, 0.2, 0.8]) for item in self._vision_sensor_rays[i]]
for i in range(4, 6):
self._vision_sensors[i].remove()
self._vision_sensor_bodies[i].remove()
[item.remove() for item in self._vision_sensor_rays[i]]
self._plant.remove()
self._dining_chair_0.remove()
self._dining_chair_1.remove()
self._dining_table.set_position(np.array([0.3, 0, 0.55]))
self._dining_table.set_orientation(np.array([np.pi / 2, 0., 0.]))
self._swivel_chair.set_position(np.array([0.33, 0.98, 0.46]))
self._swivel_chair.set_orientation(np.array([0., 0., np.pi]))
self._vision_sensor_0.set_perspective_angle(60)
self._vision_sensor_0.set_resolution([1024, 1024])
self._vision_sensor_body_0.set_position(np.array([1.35, -0.05, 1.95]))
self._vision_sensor_body_0.set_orientation(np.array([-145.81*np.pi/180, -27.763*np.pi/180, 136.5*np.pi/180]))
self._vision_sensor_1.set_perspective_angle(60)
self._vision_sensor_1.set_resolution([1024, 1024])
self._vision_sensor_body_1.set_position(np.array([1.65, -2.075, 0.875]))
self._vision_sensor_body_1.set_orientation(np.array([-91.181*np.pi/180, -30.478*np.pi/180, -171.39*np.pi/180]))
self._vision_sensor_2.set_perspective_angle(60)
self._vision_sensor_2.set_resolution([1024, 1024])
self._vision_sensor_body_2.set_position(np.array([-0.94, -1.71, 0.994]))
self._vision_sensor_body_2.set_orientation(np.array([-116.22*np.pi/180, 39.028*np.pi/180, -138.88*np.pi/180]))
self._vision_sensor_3.set_perspective_angle(60)
self._vision_sensor_3.set_resolution([512, 512])
self._vision_sensor_body_3.set_position(np.array([0.35, -2.05, 0.68]))
self._vision_sensor_body_3.set_orientation(np.array([-90.248*np.pi/180, -1.2555*np.pi/180, -179.88*np.pi/180]))
self._default_camera.set_position(np.array([2.4732, -3.2641, 2.9269]))
self._default_camera.set_orientation(np.array([i*np.pi/180 for i in [-134.8, -33.52, 151.26]]))
# public objects
self.cams = [SimCam(cam) for cam in self._vision_sensors[0:3]]
self.target_cam = SimCam(self._vision_sensor_3)
# wait for user input
self._user_prompt(
'\nInitialized scene with 3 acquiring projective cameras (blue rays) and 1 target projective camera (green rays) '
'facing the overturned table.\n\n'
'The simulator visualizer can be translated and rotated by clicking either the left mouse button or the wheel, '
'and then dragging the mouse.\n'
'Scrolling the mouse wheel zooms the view in and out.\n\n'
'You can click on the object "vision_senor_3" in the left hand panel, '
'then select the box icon with four arrows in the top panel of the simulator, '
'and then drag the target camera around dynamically.\n'
'Starting to drag and then holding ctrl allows you to also drag the cameras up and down.\n'
'Clicking the top icon with a box and two rotating arrows similarly allows rotation of the camera.\n\n'
'This demo enables you to capture images from the cameras 10 times, '
'and render the associated images in the target frame.\n\n'
'\nPress enter to use method ivy_vision.render_pixel_coords and show the first renderings for the target frame, '
'produced by projecting the depth and color values from the 3 acquiring frames.'
'\nRenderings are shown both with and without the use of a depth buffer in the ivy_vision method.\n'
'If the image window appears blank at first, maximize it to show the renderings.')
else:
cam_names = ['vs{}'.format(i) for i in range(3)] + ['tvs']
pp_offsets = ivy.array([item/2 - 0.5 for item in [1024, 1024]])
persp_angles = ivy.array([60 * np.pi/180]*2)
intrinsics = ivy_vision.persp_angles_and_pp_offsets_to_intrinsics_object(
persp_angles, pp_offsets, [1024, 1024])
calib_mat = intrinsics.calib_mats
inv_calib_mat = intrinsics.inv_calib_mats
pp_offsets = ivy.array([item/2 - 0.5 for item in [512, 512]])
persp_angles = ivy.array([60 * np.pi/180]*2)
target_intrinsics = ivy_vision.persp_angles_and_pp_offsets_to_intrinsics_object(
persp_angles, pp_offsets, [512, 512])
target_calib_mat = target_intrinsics.calib_mats
target_inv_calib_mat = target_intrinsics.inv_calib_mats
cam_positions = [ivy.array([1.35, -0.05, 1.95]),
ivy.array([1.65, -2.075, 0.875]),
ivy.array([-0.94, -1.71, 0.994]),
ivy.array([0.35, -2.05, 0.68])]
cam_quaternions = [ivy.array([-0.40934521, 0.83571182, 0.35003018, -0.10724328]),
ivy.array([-0.13167774, 0.7011009, 0.65917628, -0.23791856]),
ivy.array([0.44628197, 0.68734518, 0.56583211, 0.09068139]),
ivy.array([-0.00698829, 0.70860066, 0.70552395, -0.00850271])]
cam_quat_poses = [ivy.concatenate((pos, eul), -1) for pos, eul in zip(cam_positions, cam_quaternions)]
cam_inv_ext_mats = [ivy_mech.quaternion_pose_to_mat_pose(qp) for qp in cam_quat_poses]
cam_ext_mats = [ivy.inv(ivy_mech.make_transformation_homogeneous(iem))[..., 0:3, :]
for iem in cam_inv_ext_mats]
self.cams = [DummyCam(calib_mat, inv_calib_mat, em, iem, n)
for em, iem, n in zip(cam_ext_mats, cam_inv_ext_mats[:-1], cam_names[:-1])]
self.target_cam = DummyCam(target_calib_mat, target_inv_calib_mat, cam_ext_mats[-1], cam_inv_ext_mats[-1],
cam_names[-1])
# message
print('\nInitialized dummy scene with 3 acquiring projective cameras and 1 target projective camera '
'facing the overturned table.'
'\nClose the visualization window to use method ivy_vision.render_pixel_coords and show renderings for the target frame, '
'produced by projecting the depth and color values from the 3 acquiring frames.'
'\nRenderings are shown both with and without the use of a depth buffer in the ivy_vision method.\n')
# plot scene before rotation
if interactive:
import matplotlib.pyplot as plt
import matplotlib.image as mpimg
plt.imshow(mpimg.imread(os.path.join(os.path.dirname(os.path.realpath(__file__)),
'ri_no_sim', 'before_capture.png')))
plt.show()
def ext_mat_and_intrinsics_to_cam_geometry_object(ext_mat, intrinsics, batch_shape=None, dev_str=None):
"""
Create camera geometry object from extrinsic matrix :math:`\mathbf{E}\in\mathbb{R}^{3×4}`, and camera intrinsics
object.
:param ext_mat: Extrinsic matrix *[batch_shape,3,4]*
:type ext_mat: array
:param intrinsics: camera intrinsics object
:type intrinsics: camera_intrinsics
:param batch_shape: Shape of batch. Inferred from inputs if None.
:type batch_shape: sequence of ints, optional
:param dev_str: device on which to create the array 'cuda:0', 'cuda:1', 'cpu' etc. Same as x if None.
:type dev_str: str, optional
:return: Camera geometry object
"""
if batch_shape is None:
batch_shape = ext_mat.shape[:-2]
if dev_str is None:
dev_str = _ivy.dev_str(ext_mat)
# shapes as list
batch_shape = list(batch_shape)
# num batch dims
num_batch_dims = len(batch_shape)
# BS x 4 x 4
ext_mat_homo = \
_ivy.concatenate(
(ext_mat, _ivy.tile(_ivy.reshape(_ivy.array([0., 0., 0., 1.], dev_str=dev_str),
[1] * (num_batch_dims + 1) + [4]),
batch_shape + [1, 1])), -2)
# BS x 4 x 4
inv_ext_mat_homo = _ivy.inv(ext_mat_homo)
# BS x 3 x 4
inv_ext_mat = inv_ext_mat_homo[..., 0:3, :]
# BS x 3 x 1
cam_center = inv_ext_mat_to_camera_center(inv_ext_mat)
# BS x 3 x 3
Rs = ext_mat[..., 0:3]
# BS x 3 x 3
inv_Rs = inv_ext_mat[..., 0:3]
# extrinsics object
extrinsics = _Extrinsics(cam_center, Rs, inv_Rs, ext_mat_homo, inv_ext_mat_homo)
# BS x 3 x 4
full_mat = calib_and_ext_to_full_mat(intrinsics.calib_mats, ext_mat)
# BS x 4 x 4
full_mat_homo = \
_ivy.concatenate((
full_mat, _ivy.tile(_ivy.reshape(_ivy.array([0., 0., 0., 1.], dev_str=dev_str),
[1] * (num_batch_dims + 1) + [4]),
batch_shape + [1, 1])), -2)
# BS x 4 x 4
inv_full_mat_homo = _ivy.inv(full_mat_homo)
# camera geometry object
cam_geometry = _CameraGeometry(intrinsics, extrinsics, full_mat_homo, inv_full_mat_homo)
return cam_geometry
def main(interactive=True, f=None):
global INTERACTIVE
INTERACTIVE = interactive
# Framework Setup #
# ----------------#
# choose random framework
f = choose_random_framework() if f is None else f
set_framework(f)
# Camera Geometry #
# ----------------#
# intrinsics
# common intrinsic params
img_dims = [512, 512]
pp_offsets = ivy.array([dim / 2 - 0.5 for dim in img_dims], 'float32')
cam_persp_angles = ivy.array([60 * np.pi / 180] * 2, 'float32')
# ivy cam intrinsics container
intrinsics = ivy_vision.persp_angles_and_pp_offsets_to_intrinsics_object(
cam_persp_angles, pp_offsets, img_dims)
# extrinsics
# 3 x 4
cam1_inv_ext_mat = ivy.array(np.load(data_dir + '/cam1_inv_ext_mat.npy'),
'float32')
cam2_inv_ext_mat = ivy.array(np.load(data_dir + '/cam2_inv_ext_mat.npy'),
'float32')
# full geometry
# ivy cam geometry container
cam1_geom = ivy_vision.inv_ext_mat_and_intrinsics_to_cam_geometry_object(
cam1_inv_ext_mat, intrinsics)
cam2_geom = ivy_vision.inv_ext_mat_and_intrinsics_to_cam_geometry_object(
cam2_inv_ext_mat, intrinsics)
cam_geoms = [cam1_geom, cam2_geom]
# Camera Geometry Check #
# ----------------------#
# assert camera geometry shapes
for cam_geom in cam_geoms:
assert cam_geom.intrinsics.focal_lengths.shape == (2, )
assert cam_geom.intrinsics.persp_angles.shape == (2, )
assert cam_geom.intrinsics.pp_offsets.shape == (2, )
assert cam_geom.intrinsics.calib_mats.shape == (3, 3)
assert cam_geom.intrinsics.inv_calib_mats.shape == (3, 3)
assert cam_geom.extrinsics.cam_centers.shape == (3, 1)
assert cam_geom.extrinsics.Rs.shape == (3, 3)
assert cam_geom.extrinsics.inv_Rs.shape == (3, 3)
assert cam_geom.extrinsics.ext_mats_homo.shape == (4, 4)
assert cam_geom.extrinsics.inv_ext_mats_homo.shape == (4, 4)
assert cam_geom.full_mats_homo.shape == (4, 4)
assert cam_geom.inv_full_mats_homo.shape == (4, 4)
# Image Data #
# -----------#
# load images
# h x w x 3
color1 = ivy.array(
cv2.imread(data_dir + '/rgb1.png').astype(np.float32) / 255)
color2 = ivy.array(
cv2.imread(data_dir + '/rgb2.png').astype(np.float32) / 255)
# h x w x 1
depth1 = ivy.array(
np.reshape(
np.frombuffer(
cv2.imread(data_dir + '/depth1.png', -1).tobytes(),
np.float32), img_dims + [1]))
depth2 = ivy.array(
np.reshape(
np.frombuffer(
cv2.imread(data_dir + '/depth2.png', -1).tobytes(),
np.float32), img_dims + [1]))
# depth scaled pixel coords
# h x w x 3
u_pix_coords = ivy_vision.create_uniform_pixel_coords_image(img_dims)
ds_pixel_coords1 = u_pix_coords * depth1
ds_pixel_coords2 = u_pix_coords * depth2
# depth limits
depth_min = ivy.reduce_min(ivy.concatenate((depth1, depth2), 0))
depth_max = ivy.reduce_max(ivy.concatenate((depth1, depth2), 0))
depth_limits = [depth_min, depth_max]
# show images
show_rgb_and_depth_images(color1, color2, depth1, depth2, depth_limits)
# Flow and Depth Triangulation #
# -----------------------------#
# required mat formats
cam1to2_full_mat_homo = ivy.matmul(cam2_geom.full_mats_homo,
cam1_geom.inv_full_mats_homo)
cam1to2_full_mat = cam1to2_full_mat_homo[..., 0:3, :]
full_mats_homo = ivy.concatenate(
(ivy.expand_dims(cam1_geom.full_mats_homo,
0), ivy.expand_dims(cam2_geom.full_mats_homo, 0)), 0)
full_mats = full_mats_homo[..., 0:3, :]
# flow
flow1to2 = ivy_vision.flow_from_depth_and_cam_mats(ds_pixel_coords1,
cam1to2_full_mat)
# depth again
depth1_from_flow = ivy_vision.depth_from_flow_and_cam_mats(
flow1to2, full_mats)
# show images
show_flow_and_depth_images(depth1, flow1to2, depth1_from_flow,
depth_limits)
# Inverse Warping #
# ----------------#
# inverse warp rendering
warp = u_pix_coords[..., 0:2] + flow1to2
color2_warp_to_f1 = ivy.reshape(ivy.bilinear_resample(color2, warp),
color1.shape)
# projected depth scaled pixel coords 2
ds_pixel_coords1_wrt_f2 = ivy_vision.ds_pixel_to_ds_pixel_coords(
ds_pixel_coords1, cam1to2_full_mat)
# projected depth 2
depth1_wrt_f2 = ds_pixel_coords1_wrt_f2[..., -1:]
# inverse warp depth
depth2_warp_to_f1 = ivy.reshape(ivy.bilinear_resample(depth2, warp),
depth1.shape)
# depth validity
depth_validity = ivy.abs(depth1_wrt_f2 - depth2_warp_to_f1) < 0.01
# inverse warp rendering with mask
color2_warp_to_f1_masked = ivy.where(depth_validity, color2_warp_to_f1,
ivy.zeros_like(color2_warp_to_f1))
# show images
show_inverse_warped_images(depth1_wrt_f2, depth2_warp_to_f1,
depth_validity, color1, color2_warp_to_f1,
color2_warp_to_f1_masked, depth_limits)
# Forward Warping #
# ----------------#
# forward warp rendering
ds_pixel_coords1_proj = ivy_vision.ds_pixel_to_ds_pixel_coords(
ds_pixel_coords2,
ivy.inv(cam1to2_full_mat_homo)[..., 0:3, :])
depth1_proj = ds_pixel_coords1_proj[..., -1:]
ds_pixel_coords1_proj = ds_pixel_coords1_proj[..., 0:2] / depth1_proj
features_to_render = ivy.concatenate((depth1_proj, color2), -1)
# without depth buffer
f1_forward_warp_no_db, _, _ = ivy_vision.quantize_to_image(
ivy.reshape(ds_pixel_coords1_proj, (-1, 2)),
img_dims,
ivy.reshape(features_to_render, (-1, 4)),
ivy.zeros_like(features_to_render),
with_db=False)
# with depth buffer
f1_forward_warp_w_db, _, _ = ivy_vision.quantize_to_image(
ivy.reshape(ds_pixel_coords1_proj, (-1, 2)),
img_dims,
ivy.reshape(features_to_render, (-1, 4)),
ivy.zeros_like(features_to_render),
with_db=False if ivy.get_framework() == 'mxnd' else True)
# show images
show_forward_warped_images(depth1, color1, f1_forward_warp_no_db,
f1_forward_warp_w_db, depth_limits)
# message
print('End of Run Through Demo!')
def get_ext_mat(self):
return ivy.inv(ivy_mech.make_transformation_homogeneous(self.get_inv_ext_mat()))[0:3, :]
def __init__(self, interactive, try_use_sim):
super().__init__(interactive, try_use_sim)
# initialize scene
if self.with_pyrep:
for i in range(6):
[item.remove() for item in self._vision_sensor_rays[i]]
self._spherical_vision_sensor.remove()
self._default_camera.set_position(
np.array([-2.3518, 4.3953, 2.8949]))
self._default_camera.set_orientation(
np.array([i * np.pi / 180 for i in [112.90, 27.329, -10.978]]))
inv_ext_mat = ivy.reshape(
ivy.array(self._default_vision_sensor.get_matrix(), 'float32'),
(3, 4))
self.default_camera_ext_mat_homo = ivy.inv(
ivy_mech.make_transformation_homogeneous(inv_ext_mat))
# public objects
self.cams = [SimCam(cam) for cam in self._vision_sensors]
# wait for user input
self._user_prompt(
'\nInitialized scene with 6 projective cameras in the centre.\n\n'
'You can click on the dummy object "vision_senors" in the left hand panel, '
'then select the box icon with four arrows in the top panel of the simulator, '
'and then drag the cameras around dynamically.\n'
'Starting to drag and then holding ctrl allows you to also drag the cameras up and down. \n\n'
'This demo enables you to capture images from the cameras 10 times, '
'and render the associated 10 voxel grids in an open3D visualizer.\n\n'
'Both visualizers can be translated and rotated by clicking either the left mouse button or the wheel, '
'and then dragging the mouse.\n'
'Scrolling the mouse wheel zooms the view in and out.\n\n'
'Press enter in the terminal to use method ivy_vision.coords_to_bounding_voxel_grid and show the first voxel grid '
'reconstruction of the scene, produced using the 6 depth and color images from the projective cameras.\n'
)
else:
cam_names = ['vs{}'.format(i) for i in range(6)]
pp_offsets = ivy.array([item / 2 - 0.5 for item in [128, 128]])
persp_angles = ivy.array([90 * np.pi / 180] * 2)
intrinsics = ivy_vision.persp_angles_and_pp_offsets_to_intrinsics_object(
persp_angles, pp_offsets, [128, 128])
inv_calib_mat = intrinsics.inv_calib_mats
cam_positions = [ivy.array([0., 0., 1.5]) for _ in range(6)]
cam_quaternions = [
ivy.array([-0.5, 0.5, 0.5, -0.5]),
ivy.array([0.707, 0, 0, 0.707]),
ivy.array([1., 0., 0., 0.]),
ivy.array([0.5, 0.5, 0.5, 0.5]),
ivy.array([0, 0.707, 0.707, 0]),
ivy.array([0., 0., 0., 1.])
]
cam_quat_poses = [
ivy.concatenate((pos, eul), -1)
for pos, eul in zip(cam_positions, cam_quaternions)
]
cam_inv_ext_mats = [
ivy_mech.quaternion_pose_to_mat_pose(qp)
for qp in cam_quat_poses
]
self.cams = [
DummyCam(inv_calib_mat, iem, n)
for iem, n in zip(cam_inv_ext_mats, cam_names)
]
self.default_camera_ext_mat_homo = ivy.array(
[[-0.872, -0.489, 0., 0.099], [-0.169, 0.301, -0.938, 0.994],
[0.459, -0.818, -0.346, 5.677], [0., 0., 0., 1.]])
# message
print(
'\nInitialized dummy scene with 6 projective cameras in the centre.'
'\nClose the visualization window to use method ivy_vision.coords_to_bounding_voxel_grid and show a voxel grid '
'reconstruction of the scene, produced using the 6 depth and color images from the projective cameras.\n'
)
# plot scene before rotation
if interactive:
plt.imshow(
mpimg.imread(
os.path.join(
os.path.dirname(os.path.realpath(__file__)),
'ctvg_no_sim', 'before_capture.png')))
plt.show()