def _omni_frame_to_omni_frame_projection(
self, agent_rel_pose, agent_rel_mat, uniform_sphere_pixel_coords,
sphere_pix_coords_f1, sphere_depth_f1, sphere_feat_f1,
agent_rel_pose_cov, image_var_f1, holes_prior, holes_prior_var,
batch_size):
"""
Project mean and variance values from omni frame 1 to omni frame 2, using scatter and quantize operation.
:param agent_rel_pose: Relative pose of agent to the previous step *[batch_size, 6]*
:param agent_rel_mat: Relative transformation matrix of agent to the previous step
*[batch_size, 3, 4]*
:param uniform_sphere_pixel_coords: Pixel coords *[batch_size, h, w, 3]*
:param sphere_pix_coords_f1: Pixel co-ordinates to project *[batch_size, h, w, 2]*
:param sphere_depth_f1: Mean radius values to project *[batch_size, h, w, 1]*
:param sphere_feat_f1: Mean feature values to project *[batch_size, h, w, f]*
:param agent_rel_pose_cov: Agent relative pose covariance *[batch_size, 6, 6]*
:param image_var_f1: Angular pixels, radius and feature variance values to project *[batch_size, h, w, 3+f]*
:param holes_prior: Prior values for holes which arise during the projection *[batch_size, h, w, 1+f]*
:param holes_prior_var: Prior variance for holes which arise during the projection
*[batch_size, h, w, 3+f]*
:param batch_size: Size of batch
:return: Projected mean *[batch_size, h, w, 3+f]* and variance *[batch_size, h, w, 3+f]*
"""
# Frame 1 #
# --------#
# combined
# B x OH x OW x 3
angular_pixel_coords_f1 = ivy.concatenate(
(sphere_pix_coords_f1, sphere_depth_f1), -1)
# sphere coords
# B x OH x OW x 3
sphere_coords_f1 = \
ivy_vision.angular_pixel_to_sphere_coords(angular_pixel_coords_f1, self._pixels_per_degree)
# Frame 2 #
# --------#
# sphere to sphere pixel projection
sphere_coords_f2 = ivy_vision.sphere_to_sphere_coords(
sphere_coords_f1, agent_rel_mat, [batch_size],
self._sphere_img_dims)
image_var_f2 = image_var_f1
# to angular pixel coords
# B x OH x OW x 3
angular_pixel_coords_f2 = \
ivy_vision.sphere_to_angular_pixel_coords(sphere_coords_f2, self._pixels_per_degree)
# constant feature projection
# B x OH x OW x (3+F)
projected_coords_f2 = ivy.concatenate([angular_pixel_coords_f2] +
[sphere_feat_f1], -1)
# reshaping to fit quantization dimension requirements
# B x (OHxOW) x (3+F)
projected_coords_f2_flat = ivy.reshape(
projected_coords_f2, [batch_size] +
[self._sphere_img_dims[0] * self._sphere_img_dims[1]] +
[3 + self._feat_dim])
# B x (OHxOW) x (3+F)
image_var_f2_flat = ivy.reshape(
image_var_f2, [batch_size] +
[self._sphere_img_dims[0] * self._sphere_img_dims[1]] +
[3 + self._feat_dim])
# quantize the projection
# B x N x OH x OW x (3+F) # B x N x OH x OW x (3+F)
return ivy_vision.quantize_to_image(
pixel_coords=projected_coords_f2_flat[..., 0:2],
final_image_dims=self._sphere_img_dims,
feat=projected_coords_f2_flat[..., 2:],
feat_prior=holes_prior,
with_db=self._with_depth_buffer,
pixel_coords_var=image_var_f2_flat[..., 0:2],
feat_var=image_var_f2_flat[..., 2:],
pixel_coords_prior_var=holes_prior_var[..., 0:2],
feat_prior_var=holes_prior_var[..., 2:],
var_threshold=self._var_threshold[:, 0],
uniform_pixel_coords=uniform_sphere_pixel_coords,
batch_shape=(batch_size, ),
dev_str=self._dev_str)[0:2]
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 _frame_to_omni_frame_projection(
self, cam_rel_poses, cam_rel_mats, uniform_sphere_pixel_coords,
cam_coords_f1, cam_feat_f1, rel_pose_covs, image_var_f1,
holes_prior, holes_prior_var, batch_size, num_timesteps, num_cams,
image_dims):
"""
Project mean and variance values from frame 1 to omni frame 2, using scatter and quantize operations.
:param cam_rel_poses: Relative pose of camera to agent *[batch_size, n, c, 6]*
:param cam_rel_mats: Relative transformation matrix from camera to agent *[batch_size, n, c, 3, 4]*
:param uniform_sphere_pixel_coords: Pixel coords *[batch_size, n, h, w, 3]*
:param cam_coords_f1: Camera co-ordinates to project *[batch_size, n, c, h_in, w_in, 3]*
:param cam_feat_f1: Mean feature values to project *[batch_size, n, c, h_in, w_in, f]*
:param rel_pose_covs: Pose covariances *[batch_size, n, c, 6, 6]*
:param image_var_f1: Angular pixel, radius and feature variance values to project *[batch_size, n, c, h_in, w_in, 3+f]*
:param holes_prior: Prior values for holes which arise during the projection *[batch_size, n, h, w, 3+f]*
:param holes_prior_var: Prior variance for holes which arise during the projection *[batch_size, n, h, w, 3+f]*
:param batch_size: Size of batch
:param num_timesteps: Number of timesteps
:param num_cams: Number of cameras
:param image_dims: Image dimensions
:return: Projected mean *[batch_size, 1, h, w, 3+f]* and variance *[batch_size, 1, h, w, 3+f]*
"""
# cam 1 to cam 2 coords
cam_coords_f2 = ivy_vision.cam_to_cam_coords(
ivy_mech.make_coordinates_homogeneous(
cam_coords_f1,
[batch_size, num_timesteps, num_cams] + image_dims),
cam_rel_mats, [batch_size, num_timesteps, num_cams], image_dims)
# cam 2 to sphere 2 coords
sphere_coords_f2 = ivy_vision.cam_to_sphere_coords(cam_coords_f2)
image_var_f2 = image_var_f1
# angular pixel coords
# B x N x C x H x W x 3
angular_pixel_coords_f2 = \
ivy_vision.sphere_to_angular_pixel_coords(sphere_coords_f2, self._pixels_per_degree)
# constant feature projection
# B x N x C x H x W x (3+F)
projected_coords_f2 = ivy.concatenate([angular_pixel_coords_f2] +
[cam_feat_f1], -1)
# reshaping to fit quantization dimension requirements
# B x N x (CxHxW) x (3+F)
projected_coords_f2_flat = \
ivy.reshape(projected_coords_f2,
[batch_size, num_timesteps, num_cams * image_dims[0] * image_dims[1], -1])
# B x N x (CxHxW) x (3+F)
image_var_f2_flat = ivy.reshape(image_var_f2, [
batch_size, num_timesteps,
num_cams * image_dims[0] * image_dims[1], -1
])
# quantized result from all scene cameras
# B x N x OH x OW x (3+F) # B x N x OH x OW x (3+F)
return ivy_vision.quantize_to_image(
pixel_coords=projected_coords_f2_flat[..., 0:2],
final_image_dims=self._sphere_img_dims,
feat=projected_coords_f2_flat[..., 2:],
feat_prior=holes_prior,
with_db=self._with_depth_buffer,
pixel_coords_var=image_var_f2_flat[..., 0:2],
feat_var=image_var_f2_flat[..., 2:],
pixel_coords_prior_var=holes_prior_var[..., 0:2],
feat_prior_var=holes_prior_var[..., 2:],
var_threshold=self._var_threshold,
uniform_pixel_coords=uniform_sphere_pixel_coords,
batch_shape=(batch_size, num_timesteps),
dev_str=self._dev_str)[0:2]
def main(interactive=True, try_use_sim=True, f=None):
f = choose_random_framework() if f is None else f
set_framework(f)
with_mxnd = f is ivy.mxnd
if with_mxnd:
print('\nMXnet does not support "sum" or "min" reductions for scatter_nd,\n'
'instead it only supports non-deterministic replacement for duplicates.\n'
'Depth buffer rendering (requies min) or fusion buffer (requies sum) are therefore unsupported.\n'
'The rendering in this demo with MXNet backend exhibits non-deterministic jagged edges as a result.')
sim = Simulator(interactive, try_use_sim)
import matplotlib.pyplot as plt
xyzs = list()
rgbs = list()
iterations = 10 if sim.with_pyrep else 1
for _ in range(iterations):
for cam in sim.cams:
depth, rgb = cam.cap()
xyz = sim.depth_to_xyz(depth, cam.get_inv_ext_mat(), cam.inv_calib_mat, [1024]*2)
xyzs.append(xyz)
rgbs.append(rgb)
xyz = ivy.reshape(ivy.concatenate(xyzs, 1), (-1, 3))
rgb = ivy.reshape(ivy.concatenate(rgbs, 1), (-1, 3))
cam_coords = ivy_vision.world_to_cam_coords(ivy_mech.make_coordinates_homogeneous(ivy.expand_dims(xyz, 1)),
sim.target_cam.get_ext_mat())
ds_pix_coords = ivy_vision.cam_to_ds_pixel_coords(cam_coords, sim.target_cam.calib_mat)
depth = ds_pix_coords[..., -1]
pix_coords = ds_pix_coords[..., 0, 0:2] / depth
final_image_dims = [512]*2
feat = ivy.concatenate((depth, rgb), -1)
rendered_img_no_db, _, _ = ivy_vision.quantize_to_image(
pix_coords, final_image_dims, feat, ivy.zeros(final_image_dims + [4]), with_db=False)
with_db = not with_mxnd
rendered_img_with_db, _, _ = ivy_vision.quantize_to_image(
pix_coords, final_image_dims, feat, ivy.zeros(final_image_dims + [4]), with_db=with_db)
a_img = cv2.resize(ivy.to_numpy(rgbs[0]), (256, 256))
a_img[0:50, 0:50] = np.zeros_like(a_img[0:50, 0:50])
a_img[5:45, 5:45] = np.ones_like(a_img[5:45, 5:45])
cv2.putText(a_img, 'a', (13, 33), cv2.FONT_HERSHEY_SIMPLEX, 1.2, tuple([0] * 3), 2)
b_img = cv2.resize(ivy.to_numpy(rgbs[1]), (256, 256))
b_img[0:50, 0:50] = np.zeros_like(b_img[0:50, 0:50])
b_img[5:45, 5:45] = np.ones_like(b_img[5:45, 5:45])
cv2.putText(b_img, 'b', (13, 33), cv2.FONT_HERSHEY_SIMPLEX, 1.2, tuple([0] * 3), 2)
c_img = cv2.resize(ivy.to_numpy(rgbs[2]), (256, 256))
c_img[0:50, 0:50] = np.zeros_like(c_img[0:50, 0:50])
c_img[5:45, 5:45] = np.ones_like(c_img[5:45, 5:45])
cv2.putText(c_img, 'c', (13, 33), cv2.FONT_HERSHEY_SIMPLEX, 1.2, tuple([0] * 3), 2)
target_img = cv2.resize(ivy.to_numpy(sim.target_cam.cap()[1]), (256, 256))
target_img[0:50, 0:140] = np.zeros_like(target_img[0:50, 0:140])
target_img[5:45, 5:135] = np.ones_like(target_img[5:45, 5:135])
cv2.putText(target_img, 'target', (13, 33), cv2.FONT_HERSHEY_SIMPLEX, 1.2, tuple([0] * 3), 2)
msg = 'non-deterministic' if with_mxnd else 'no depth buffer'
width = 360 if with_mxnd else 320
no_db_img = np.copy(ivy.to_numpy(rendered_img_no_db[..., 3:]))
no_db_img[0:50, 0:width+5] = np.zeros_like(no_db_img[0:50, 0:width+5])
no_db_img[5:45, 5:width] = np.ones_like(no_db_img[5:45, 5:width])
cv2.putText(no_db_img, msg, (13, 33), cv2.FONT_HERSHEY_SIMPLEX, 1.2, tuple([0] * 3), 2)
with_db_img = np.copy(ivy.to_numpy(rendered_img_with_db[..., 3:]))
with_db_img[0:50, 0:350] = np.zeros_like(with_db_img[0:50, 0:350])
with_db_img[5:45, 5:345] = np.ones_like(with_db_img[5:45, 5:345])
cv2.putText(with_db_img, 'with depth buffer', (13, 33), cv2.FONT_HERSHEY_SIMPLEX, 1.2, tuple([0] * 3), 2)
raw_imgs = np.concatenate((np.concatenate((a_img, b_img), 1),
np.concatenate((c_img, target_img), 1)), 0)
to_concat = (raw_imgs, no_db_img) if with_mxnd else (raw_imgs, no_db_img, with_db_img)
final_img = np.concatenate(to_concat, 1)
if interactive:
print('\nClose the image window when you are ready.\n')
plt.imshow(final_img)
plt.show()
xyzs.clear()
rgbs.clear()
sim.close()
unset_framework()