def main(interactive=True, try_use_sim=True, f=None):
f = choose_random_framework() if f is None else f
set_framework(f)
sim = Simulator(interactive, try_use_sim)
vis = Visualizer(ivy.to_numpy(sim.default_camera_ext_mat_homo))
pix_per_deg = 2
om_pix = sim.get_pix_coords()
plr_degs = om_pix / pix_per_deg
plr_rads = plr_degs * math.pi / 180
iterations = 10 if sim.with_pyrep else 1
for _ in range(iterations):
depth, rgb = sim.omcam.cap()
plr = ivy.concatenate([plr_rads, depth], -1)
xyz_wrt_cam = ivy_mech.polar_to_cartesian_coords(plr)
xyz_wrt_cam = ivy.reshape(xyz_wrt_cam, (-1, 3))
xyz_wrt_cam_homo = ivy_mech.make_coordinates_homogeneous(xyz_wrt_cam)
inv_ext_mat_trans = ivy.transpose(sim.omcam.get_inv_ext_mat(), (1, 0))
xyz_wrt_world = ivy.matmul(xyz_wrt_cam_homo, inv_ext_mat_trans)[..., 0:3]
with ivy.numpy.use:
omni_cam_inv_ext_mat = ivy_mech.make_transformation_homogeneous(
ivy.to_numpy(sim.omcam.get_inv_ext_mat()))
vis.show_point_cloud(xyz_wrt_world, rgb, interactive,
sphere_inv_ext_mats=[omni_cam_inv_ext_mat], sphere_radii=[0.025])
if not interactive:
sim.omcam.set_pos(sim.omcam.get_pos()
+ ivy.array([-0.01, 0.01, 0.]))
sim.close()
unset_framework()
def main(interactive=True, try_use_sim=True, f=None):
# config
this_dir = os.path.dirname(os.path.realpath(__file__))
f = choose_random_framework(excluded=['numpy']) if f is None else f
set_framework(f)
sim = Simulator(interactive, try_use_sim)
lr = 0.5
num_anchors = 3
num_sample_points = 100
# spline start
anchor_points = ivy.cast(
ivy.expand_dims(ivy.linspace(0, 1, 2 + num_anchors), -1), 'float32')
query_points = ivy.cast(
ivy.expand_dims(ivy.linspace(0, 1, num_sample_points), -1), 'float32')
# learnable parameters
robot_start_config = ivy.array(ivy.cast(sim.robot_start_config, 'float32'))
robot_target_config = ivy.array(
ivy.cast(sim.robot_target_config, 'float32'))
learnable_anchor_vals = ivy.variable(
ivy.cast(
ivy.transpose(
ivy.linspace(robot_start_config, robot_target_config,
2 + num_anchors)[..., 1:-1], (1, 0)), 'float32'))
# optimizer
optimizer = ivy.SGD(lr=lr)
# optimize
it = 0
colliding = True
clearance = 0
joint_query_vals = None
while colliding:
total_cost, grads, joint_query_vals, link_positions, sdf_vals = ivy.execute_with_gradients(
lambda xs: compute_cost_and_sdfs(xs[
'w'], anchor_points, robot_start_config, robot_target_config,
query_points, sim),
Container({'w': learnable_anchor_vals}))
colliding = ivy.reduce_min(sdf_vals[2:]) < clearance
sim.update_path_visualization(
link_positions, sdf_vals,
os.path.join(this_dir, 'msp_no_sim', 'path_{}.png'.format(it)))
learnable_anchor_vals = optimizer.step(
Container({'w': learnable_anchor_vals}), grads)['w']
it += 1
sim.execute_motion(joint_query_vals)
sim.close()
unset_framework()
def main(interactive=True, try_use_sim=True, f=None):
f = choose_random_framework() if f is None else f
set_framework(f)
sim = Simulator(interactive, try_use_sim)
cam_pos = sim.cam.get_pos()
iterations = 250 if sim.with_pyrep else 1
msg = 'tracking plant pot for 250 simulator steps...' if sim.with_pyrep else ''
print(msg)
for i in range(iterations):
target_pos = sim.target.get_pos()
tfrm = ivy_mech.target_facing_rotation_matrix(cam_pos, target_pos)
sim.cam.set_rot_mat(tfrm)
if not interactive:
sim.target.set_pos(sim.target.get_pos()
+ ivy.array([-0.01, 0.01, 0.]))
time.sleep(0.05)
sim.close()
unset_framework()
def main(interactive=True, try_use_sim=True, f=None):
f = choose_random_framework() if f is None else f
set_framework(f)
sim = Simulator(interactive, try_use_sim)
vis = Visualizer(ivy.to_numpy(sim.default_camera_ext_mat_homo))
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, [128] * 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))
voxels = [
ivy.to_numpy(item)
for item in ivy_vision.coords_to_voxel_grid(xyz, [100] * 3,
features=rgb)
]
cuboid_inv_ext_mats = [
ivy.to_numpy(
ivy_mech.make_transformation_homogeneous(
cam.get_inv_ext_mat())) for cam in sim.cams
]
with ivy.numpy.use:
vis.show_voxel_grid(voxels,
interactive,
cuboid_inv_ext_mats=cuboid_inv_ext_mats,
cuboid_dims=[
np.array([0.045, 0.045, 0.112])
for _ in sim.cams
])
xyzs.clear()
rgbs.clear()
sim.close()
unset_framework()
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 main(batch_size=32,
num_train_steps=31250,
compile_flag=True,
num_bits=8,
seq_len=28,
ctrl_output_size=100,
memory_size=128,
memory_vector_dim=28,
overfit_flag=False,
interactive=True,
f=None):
f = choose_random_framework() if f is None else f
set_framework(f)
# train config
lr = 1e-3 if not overfit_flag else 1e-2
batch_size = batch_size if not overfit_flag else 1
num_train_steps = num_train_steps if not overfit_flag else 150
max_grad_norm = 50
# logging config
vis_freq = 250 if not overfit_flag else 1
# optimizer
optimizer = ivy.Adam(lr=lr)
# ntm
ntm = NTM(input_dim=num_bits + 1,
output_dim=num_bits,
ctrl_output_size=ctrl_output_size,
ctrl_layers=1,
memory_size=memory_size,
memory_vector_dim=memory_vector_dim,
read_head_num=1,
write_head_num=1)
# compile loss fn
total_seq_example = ivy.random_uniform(shape=(batch_size, 2 * seq_len + 1,
num_bits + 1))
target_seq_example = total_seq_example[:, 0:seq_len, :-1]
if compile_flag:
loss_fn_maybe_compiled = ivy.compile_fn(
lambda v, ttl_sq, trgt_sq, sq_ln: loss_fn(ntm, v, ttl_sq, trgt_sq,
sq_ln),
dynamic=False,
example_inputs=[
ntm.v, total_seq_example, target_seq_example, seq_len
])
else:
loss_fn_maybe_compiled = lambda v, ttl_sq, trgt_sq, sq_ln: loss_fn(
ntm, v, ttl_sq, trgt_sq, sq_ln)
# init
input_seq_m1 = ivy.cast(
ivy.random_uniform(0., 1., (batch_size, seq_len, num_bits)) > 0.5,
'float32')
mw = None
vw = None
for i in range(num_train_steps):
# sequence to copy
if not overfit_flag:
input_seq_m1 = ivy.cast(
ivy.random_uniform(0., 1.,
(batch_size, seq_len, num_bits)) > 0.5,
'float32')
target_seq = input_seq_m1
input_seq = ivy.concatenate(
(input_seq_m1, ivy.zeros((batch_size, seq_len, 1))), -1)
eos = ivy.ones((batch_size, 1, num_bits + 1))
output_seq = ivy.zeros_like(input_seq)
total_seq = ivy.concatenate((input_seq, eos, output_seq), -2)
# train step
loss, pred_vals = train_step(loss_fn_maybe_compiled, optimizer, ntm,
total_seq, target_seq, seq_len, mw, vw,
ivy.array(i + 1,
'float32'), max_grad_norm)
# log
print('step: {}, loss: {}'.format(i, ivy.to_numpy(loss).item()))
# visualize
if i % vis_freq == 0:
target_to_vis = (ivy.to_numpy(target_seq[0] * 255)).astype(
np.uint8)
target_to_vis = np.transpose(
cv2.resize(target_to_vis, (560, 160),
interpolation=cv2.INTER_NEAREST), (1, 0))
pred_to_vis = (ivy.to_numpy(pred_vals[0] * 255)).astype(np.uint8)
pred_to_vis = np.transpose(
cv2.resize(pred_to_vis, (560, 160),
interpolation=cv2.INTER_NEAREST), (1, 0))
img_to_vis = np.concatenate((pred_to_vis, target_to_vis), 0)
img_to_vis = cv2.resize(img_to_vis, (1120, 640),
interpolation=cv2.INTER_NEAREST)
img_to_vis[0:60, -200:] = 0
img_to_vis[5:55, -195:-5] = 255
cv2.putText(img_to_vis, 'step {}'.format(i), (935, 42),
cv2.FONT_HERSHEY_SIMPLEX, 1.2, tuple([0] * 3), 2)
img_to_vis[0:60, 0:200] = 0
img_to_vis[5:55, 5:195] = 255
cv2.putText(img_to_vis, 'prediction', (7, 42),
cv2.FONT_HERSHEY_SIMPLEX, 1.2, tuple([0] * 3), 2)
img_to_vis[320:380, 0:130] = 0
img_to_vis[325:375, 5:125] = 255
cv2.putText(img_to_vis, 'target', (7, 362),
cv2.FONT_HERSHEY_SIMPLEX, 1.2, tuple([0] * 3), 2)
if interactive:
cv2.imshow('prediction_and_target', img_to_vis)
if overfit_flag:
cv2.waitKey(1)
else:
cv2.waitKey(100)
cv2.destroyAllWindows()
def main(interactive=True, f=None):
global INTERACTIVE
INTERACTIVE = interactive
# Framework Setup #
# ----------------#
# choose random framework
set_framework(choose_random_framework() if f is None else f)
# Spline Interpolation #
# ---------------------#
# config
num_free_anchors = 3
num_samples = 100
constant_rot_vec = ivy.array([[0., 0., 0.]])
constant_z = ivy.array([[0.]])
# xy positions
# 1 x 2
start_xy = ivy.array([[0., 0.]])
target_xy = ivy.array([[1., 1.]])
# 1 x 2
anchor1_xy = ivy.array([[0.6, 0.2]])
anchor2_xy = ivy.array([[0.5, 0.5]])
anchor3_xy = ivy.array([[0.4, 0.8]])
# as 6DOF poses
# 1 x 6
start_pose = ivy.concatenate((start_xy, constant_z, constant_rot_vec), -1)
anchor1_pose = ivy.concatenate((anchor1_xy, constant_z, constant_rot_vec),
-1)
anchor2_pose = ivy.concatenate((anchor2_xy, constant_z, constant_rot_vec),
-1)
anchor3_pose = ivy.concatenate((anchor3_xy, constant_z, constant_rot_vec),
-1)
target_pose = ivy.concatenate((target_xy, constant_z, constant_rot_vec),
-1)
num_anchors = num_free_anchors + 2
# num_anchors x 6
anchor_poses = ivy.concatenate(
(start_pose, anchor1_pose, anchor2_pose, anchor3_pose, target_pose), 0)
# uniform sampling for spline
# num_anchors x 1
anchor_points = ivy.expand_dims(ivy.linspace(0., 1., num_anchors), -1)
# num_samples x 1
query_points = ivy.expand_dims(ivy.linspace(0., 1., num_samples), -1)
# interpolated spline poses
# num_samples x 6
interpolated_poses = ivy_robot.sample_spline_path(anchor_points,
anchor_poses,
query_points)
# xy motion
# num_samples x 2
anchor_xy_positions = anchor_poses[..., 0:2]
# num_samples x 2
interpolated_xy_positions = interpolated_poses[..., 0:2]
# show xy path
show_2d_spline_path(anchor_xy_positions, interpolated_xy_positions,
[-0.095, 0.055], [0.638, 0.171], [0.544, 0.486],
[0.445, 0.766], [0.9, 0.9], 'x', 'y',
'Interpolated Drone Pose Spline in XY Plane',
'start point', 'target point')
# Rigid Mobile #
# -------------#
# drone relative body points
rel_body_points = ivy.array([[0., 0., 0.], [-0.1, -0.1, 0.],
[-0.1, 0.1, 0.], [0.1, -0.1, 0.],
[0.1, 0.1, 0.]])
# create drone as ivy rigid mobile robot
drone = RigidMobile(rel_body_points)
# rotatin vectors
# 1 x 3
start_rot_vec = ivy.array([[0., 0., 0.]])
target_rot_vec = ivy.array([[0., 0., np.pi]])
# 1 x 3
anchor1_rot_vec = ivy.array([[0., 0., np.pi / 4]])
anchor2_rot_vec = ivy.array([[0., 0., 2 * np.pi / 4]])
anchor3_rot_vec = ivy.array([[0., 0., 3 * np.pi / 4]])
# as 6DOF poses
# 1 x 6
start_pose = ivy.concatenate((start_xy, constant_z, start_rot_vec), -1)
anchor1_pose = ivy.concatenate((anchor1_xy, constant_z, anchor1_rot_vec),
-1)
anchor2_pose = ivy.concatenate((anchor2_xy, constant_z, anchor2_rot_vec),
-1)
anchor3_pose = ivy.concatenate((anchor3_xy, constant_z, anchor3_rot_vec),
-1)
target_pose = ivy.concatenate((target_xy, constant_z, target_rot_vec), -1)
# num_anchors x 6
anchor_poses = ivy.concatenate(
(start_pose, anchor1_pose, anchor2_pose, anchor3_pose, target_pose), 0)
# interpolated spline poses
# num_samples x 6
interpolated_poses = ivy_robot.sample_spline_path(anchor_points,
anchor_poses,
query_points)
# as matrices
# num_anchors x 3 x 4
anchor_matrices = ivy_mech.rot_vec_pose_to_mat_pose(anchor_poses)
# num_samples x 3 x 4
interpolated_matrices = ivy_mech.rot_vec_pose_to_mat_pose(
interpolated_poses)
# sample drone body
# num_anchors x num_body_points x 3
anchor_body_points = drone.sample_body(anchor_matrices)
# num_samples x num_body_points x 3
interpolated_body_points = drone.sample_body(interpolated_matrices)
# show
show_full_spline_path(anchor_body_points, interpolated_body_points,
[-0.168, 0.19], [0.88, 0.73], 'x', 'y',
'Sampled Drone Body Positions in XY Plane',
'start points', 'target points', False)
# Manipulator #
# ------------#
class SimpleManipulator(Manipulator):
# noinspection PyShadowingNames
def __init__(self, base_inv_ext_mat=None):
a_s = ivy.array([0.5, 0.5])
d_s = ivy.array([0., 0.])
alpha_s = ivy.array([0., 0.])
dh_joint_scales = ivy.ones((2, ))
dh_joint_offsets = ivy.array([-np.pi / 2, 0.])
super().__init__(a_s, d_s, alpha_s, dh_joint_scales,
dh_joint_offsets, base_inv_ext_mat)
# create manipulator as ivy manipulator
manipulator = SimpleManipulator()
# joint angles
# 1 x 2
start_joint_angles = ivy.array([[0., 0.]])
target_joint_angles = ivy.array([[-np.pi / 4, -np.pi / 4]])
# 1 x 2
anchor1_joint_angles = -ivy.array([[0.2, 0.6]]) * np.pi / 4
anchor2_joint_angles = -ivy.array([[0.5, 0.5]]) * np.pi / 4
anchor3_joint_angles = -ivy.array([[0.8, 0.4]]) * np.pi / 4
# num_anchors x 2
anchor_joint_angles = ivy.concatenate(
(start_joint_angles, anchor1_joint_angles, anchor2_joint_angles,
anchor3_joint_angles, target_joint_angles), 0)
# interpolated joint angles
# num_anchors x 2
interpolated_joint_angles = ivy_robot.sample_spline_path(
anchor_points, anchor_joint_angles, query_points)
# sample links
# num_anchors x num_link_points x 3
anchor_link_points = manipulator.sample_links(anchor_joint_angles,
samples_per_metre=5)
# num_anchors x num_link_points x 3
interpolated_link_points = manipulator.sample_links(
interpolated_joint_angles, samples_per_metre=5)
# show
show_2d_spline_path(anchor_joint_angles, interpolated_joint_angles,
[-0.222, -0.015], [-0.147, -0.52], [-0.38, -0.366],
[-0.64, -0.263], [-0.752, -0.79], r'$\theta_1$',
r'$\theta_2$',
'Interpolated Manipulator Joint Angles Spline',
'start angles', 'target angles')
show_full_spline_path(anchor_link_points, interpolated_link_points,
[0.026, 0.8], [0.542, 0.26], 'x', 'y',
'Sampled Manipulator Links in XY Plane',
'start config', 'target config', True)
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 main(interactive=True, try_use_sim=True, f=None):
# setup framework
f = choose_random_framework() if f is None else f
set_framework(f)
# simulator and drone
sim = Simulator(interactive, try_use_sim)
drone = sim.drone
# ESM
esm = ivy_mem.ESM()
esm_mem = esm.empty_memory(1, 1)
# demo loop
for _ in range(1000 if interactive and sim.with_pyrep else 100):
# log iteration
print('timestep {}'.format(_))
# acquire image measurements
depth, rgb = drone.cam.cap()
# convert to ESM format
ds_pix_coords = ivy_vision.depth_to_ds_pixel_coords(depth)
cam_coords = ivy_vision.ds_pixel_to_cam_coords(ds_pix_coords, drone.cam.inv_calib_mat)[..., 0:3]
img_mean = ivy.concatenate((cam_coords, rgb), -1)
# acquire pose measurements
cam_rel_mat = drone.cam.mat_rel_to_drone
agent_rel_mat = ivy.array(drone.measure_incremental_mat())
# single esm camera measurement
esm_cam_meas = ESMCamMeasurement(
img_mean=img_mean,
cam_rel_mat=cam_rel_mat
)
# total esm observation
obs = ESMObservation(
img_meas={'cam0': esm_cam_meas},
agent_rel_mat=agent_rel_mat)
esm_mem = esm(obs, esm_mem)
# update esm visualization
if not interactive:
continue
rgb_img = _add_image_border(
cv2.resize(ivy.to_numpy(rgb).copy(), (180, 180)))
rgb_img = _add_title(rgb_img, 25, 75, 2, 'raw rgb', 70)
depth_img = _add_image_border(cv2.resize(np.clip(
np.tile(ivy.to_numpy(depth), (1, 1, 3))/3, 0, 1).copy(), (180, 180)))
depth_img = _add_title(depth_img, 25, 90, 2, 'raw depth', 85)
raw_img_concatted = np.concatenate((rgb_img, depth_img), 0)
esm_feat = _add_image_border(np.clip(ivy.to_numpy(esm_mem.mean[0, 0, ..., 3:]), 0, 1).copy())
esm_feat = _add_title(esm_feat, 25, 80, 2, 'esm rgb', 75)
esm_depth = _add_image_border(np.clip(np.tile(ivy.to_numpy(esm_mem.mean[0, 0, ..., 2:3])/3,
(1, 1, 3)), 0, 1).copy())
esm_depth = _add_title(esm_depth, 25, 95, 2, 'esm depth', 90)
esm_img_concatted = np.concatenate((esm_feat, esm_depth), 0)
img_to_show = np.concatenate((raw_img_concatted, esm_img_concatted), 1)
plt.imshow(img_to_show)
plt.show(block=False)
plt.pause(0.001)
# end of demo
sim.close()
unset_framework()
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()