# Path: ldif/util/np_util.py
# def batch_np(arr, batch_size):
# def make_coordinate_grid(height, width, is_screen_space, is_homogeneous):
# def make_coordinate_grid_3d(length, height, width, is_screen_space,
#                             is_homogeneous):
# def filter_valid(mask, vals):
# def zero_by_mask(mask, vals, replace_with=0.0):
# def make_mask(im, thresh=0.0):
# def make_pixel_mask(im):
# def thresh_and_radius_to_distance(radius, thresh):
# def plot_rbfs_at_thresh(centers, radii, thresh=0.5):
# def plot_rbfs(centers, radii, scale=10.0):
# def cube_and_render(volume, thresh):
# def sample_surface(quadrics, centers, radii, length, height, width,
#                    renormalize):
# def visualize_prediction(quadrics,
#                          centers,
#                          radii,
#                          renormalize,
#                          thresh=0.0,
#                          input_volumes=None):
# 
# Path: ldif/util/camera_util.py
# def look_at(eye, center, world_up):
# def roll_pitch_yaw_to_rotation_matrices(roll_pitch_yaw):
# 
# Path: ldif/util/tf_util.py
# def assert_shape(tensor, shape, name):
# def log(msg, t):
# def tile_new_axis(t, axis, length):
# def zero_by_mask(mask, vals, replace_with=0.0):
# def remove_element(t, elt, axis):


import math
import numpy as np
import tensorflow as tf
from ldif.util import np_util
from ldif.util import camera_util
from ldif.util import tf_util
  # flat_batch_count = int(np.prod(batching_dims))

  sample_dims = tensor.get_shape().as_list()[batch_rank:-1]
  sample_rank = len(sample_dims)
  if expected_sample_rank is not None:
    assert sample_rank == expected_sample_rank
  flat_sample_count = int(np.prod(sample_dims))
  tensor = tf.ensure_shape(tensor, batching_dims + sample_dims + [3])
  tx = tf.ensure_shape(tx, batching_dims + [4, 4])
  assert sample_rank >= 1
  assert batch_rank >= 0
  if sample_rank > 1:
    tensor = tf.reshape(tensor, batching_dims + [flat_sample_count, 3])
  initializer = tf.ones if are_points else tf.zeros
  w = initializer(batching_dims + [flat_sample_count, 1], dtype=tf.float32)
  tensor = tf.concat([tensor, w], axis=-1)
  tensor = tf.matmul(tensor, tx, transpose_b=True)
  tensor = tensor[..., :3]
  if sample_rank > 1:
    tensor = tf.reshape(tensor, batching_dims + sample_dims + [3])
  return tensor


def depth_image_to_xyz_image(depth_images, world_to_camera, xfov=0.5):
  """Converts GAPS depth images to world space."""
  batch_size, height, width, channel_count = depth_images.get_shape().as_list()
  assert channel_count == 1

  camera_to_world_mat = tf.matrix_inverse(world_to_camera)


# Path: ldif/util/np_util.py
# def batch_np(arr, batch_size):
# def make_coordinate_grid(height, width, is_screen_space, is_homogeneous):
# def make_coordinate_grid_3d(length, height, width, is_screen_space,
#                             is_homogeneous):
# def filter_valid(mask, vals):
# def zero_by_mask(mask, vals, replace_with=0.0):
# def make_mask(im, thresh=0.0):
# def make_pixel_mask(im):
# def thresh_and_radius_to_distance(radius, thresh):
# def plot_rbfs_at_thresh(centers, radii, thresh=0.5):
# def plot_rbfs(centers, radii, scale=10.0):
# def cube_and_render(volume, thresh):
# def sample_surface(quadrics, centers, radii, length, height, width,
#                    renormalize):
# def visualize_prediction(quadrics,
#                          centers,
#                          radii,
#                          renormalize,
#                          thresh=0.0,
#                          input_volumes=None):
# 
# Path: ldif/util/camera_util.py
# def look_at(eye, center, world_up):
# def roll_pitch_yaw_to_rotation_matrices(roll_pitch_yaw):
# 
# Path: ldif/util/tf_util.py
# def assert_shape(tensor, shape, name):
# def log(msg, t):
# def tile_new_axis(t, axis, length):
# def zero_by_mask(mask, vals, replace_with=0.0):
# def remove_element(t, elt, axis):


# Path: ldif/util/geom_util.py
import math
import numpy as np
import tensorflow as tf
from ldif.util import np_util
from ldif.util import camera_util
from ldif.util import tf_util

  # flat_batch_count = int(np.prod(batching_dims))

  sample_dims = tensor.get_shape().as_list()[batch_rank:-1]
  sample_rank = len(sample_dims)
  if expected_sample_rank is not None:
    assert sample_rank == expected_sample_rank
  flat_sample_count = int(np.prod(sample_dims))
  tensor = tf.ensure_shape(tensor, batching_dims + sample_dims + [3])
  tx = tf.ensure_shape(tx, batching_dims + [4, 4])
  assert sample_rank >= 1
  assert batch_rank >= 0
  if sample_rank > 1:
    tensor = tf.reshape(tensor, batching_dims + [flat_sample_count, 3])
  initializer = tf.ones if are_points else tf.zeros
  w = initializer(batching_dims + [flat_sample_count, 1], dtype=tf.float32)
  tensor = tf.concat([tensor, w], axis=-1)
  tensor = tf.matmul(tensor, tx, transpose_b=True)
  tensor = tensor[..., :3]
  if sample_rank > 1:
    tensor = tf.reshape(tensor, batching_dims + sample_dims + [3])
  return tensor


def depth_image_to_xyz_image(depth_images, world_to_camera, xfov=0.5):
  """Converts GAPS depth images to world space."""
  batch_size, height, width, channel_count = depth_images.get_shape().as_list()
  assert channel_count == 1

  camera_to_world_mat = tf.matrix_inverse(world_to_camera)


  pixel_coords = np_util.make_coordinate_grid(