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