def is_within_box_3d(point, box, name=None):
center = box[:, 0:3] # xc,yc,zc
dim = box[:, 3:6] # L,W,H
heading = box[:, 6]
# [M, 3, 3]
rotation = transform_utils.get_yaw_rotation(heading) # rotation matrix
# [M, 4, 4]
transform = transform_utils.get_transform(rotation,
center) # transform matrix
# [M, 4, 4]
transform = tf.linalg.inv(transform)
# [M, 3, 3]
rotation = transform[:, 0:3, 0:3]
# [M, 3]
translation = transform[:, 0:3, 3] # translation matrix
# [N, M, 3]
point_in_box_frame = tf.einsum('nj,mij->nmi', point,
rotation) + translation #
# [N, M, 3]
point_in_box = tf.logical_and(point_in_box_frame <= dim * 0.5,
point_in_box_frame >= -dim * 0.5)
# [N, M]
point_in_box = tf.cast(tf.reduce_prod(input_tensor=tf.cast(point_in_box,
dtype=tf.uint8),
axis=-1),
dtype=tf.bool)
return point_in_box
def _get_range_image_pose(self, lasers):
"""Fetches the per-pixel pose information for the range image."""
range_image_gbr_pose = None
for laser in lasers:
if laser.name != dataset_pb2.LaserName.TOP:
continue
pose_str = zlib.decompress(
laser.ri_return1.range_image_pose_compressed)
# Deserialize from MatrixFloat serialization.
range_image_gbr_pose = dataset_pb2.MatrixFloat()
range_image_gbr_pose.ParseFromString(pose_str)
assert range_image_gbr_pose is not None
shape = list(range_image_gbr_pose.shape.dims)
range_image_gbr_pose_tensor = np.array(
range_image_gbr_pose.data).reshape(shape)
range_image_gbr_pose_tensor_rotation = transform_utils.get_rotation_matrix(
range_image_gbr_pose_tensor[..., 0],
range_image_gbr_pose_tensor[..., 1],
range_image_gbr_pose_tensor[..., 2])
range_image_gbr_pose_tensor_translation = range_image_gbr_pose_tensor[
..., 3:]
range_image_gbr_pose_tensor = transform_utils.get_transform(
range_image_gbr_pose_tensor_rotation,
range_image_gbr_pose_tensor_translation)
assert range_image_gbr_pose_tensor.shape == (64, 2650, 4, 4)
return range_image_gbr_pose_tensor
def test_transform_point_among_frames(self):
p = tf.constant([[1.0, 0, 0]], dtype=tf.float32)
from_frame_pose = transform_utils.get_transform(
transform_utils.get_yaw_rotation(math.pi * 0.5),
tf.constant([0.0, 0.0, 2.0], dtype=tf.float32))
to_frame_pose = transform_utils.get_transform(
transform_utils.get_yaw_rotation(math.pi * 0.1),
tf.constant([0.0, 0.0, 0.0], dtype=tf.float32))
p = p[tf.newaxis, tf.newaxis, tf.newaxis, ...]
from_frame_pose = from_frame_pose[tf.newaxis, tf.newaxis, tf.newaxis,
...]
to_frame_pose = to_frame_pose[tf.newaxis, tf.newaxis, tf.newaxis, ...]
pp = box_utils.transform_point(p, from_frame_pose, to_frame_pose)
with self.test_session():
self.assertAllClose(
pp[0, 0, 0, ...].eval(),
[[math.cos(math.pi * 0.4),
math.sin(math.pi * 0.4), 2.0]])
def test_transform_box_among_frames(self):
b = tf.constant([[1.0, 0, 0, 2.0, 2.0, 2.0, math.pi * 0.1]],
dtype=tf.float32)
from_frame_pose = transform_utils.get_transform(
transform_utils.get_yaw_rotation(math.pi * 0.5),
tf.constant([0.0, 0.0, 1.0], dtype=tf.float32))
to_frame_pose = transform_utils.get_transform(
transform_utils.get_yaw_rotation(math.pi * 0.25),
tf.constant([0.0, 0.0, 0.0], dtype=tf.float32))
b = b[tf.newaxis, tf.newaxis, tf.newaxis, ...]
from_frame_pose = from_frame_pose[tf.newaxis, tf.newaxis, tf.newaxis,
...]
to_frame_pose = to_frame_pose[tf.newaxis, tf.newaxis, tf.newaxis, ...]
bb = box_utils.transform_box(b, from_frame_pose, to_frame_pose)
with self.test_session():
self.assertAllClose(bb[0, 0, 0, ...].eval(), [[
math.cos(math.pi * 0.25),
math.sin(math.pi * 0.25), 1.0, 2.0, 2.0, 2.0, math.pi * 0.35
]])
def convert_range_image_to_point_cloud(frame, range_images, camera_projections, range_image_top_pose, ri_index = 0):
calibrations = sorted(frame.context.laser_calibrations, key=lambda c: c.name)
lasers = sorted(frame.lasers, key=lambda laser: laser.name)
points = []
cp_points = []
frame_pose = tf.convert_to_tensor(np.reshape(np.array(frame.pose.transform), [4, 4]))
# [H, W, 6]
range_image_top_pose_tensor = tf.reshape(tf.convert_to_tensor(range_image_top_pose.data), range_image_top_pose.shape.dims)
# [H, W, 3, 3]
range_image_top_pose_tensor_rotation = transform_utils.get_rotation_matrix(range_image_top_pose_tensor[..., 0], range_image_top_pose_tensor[..., 1],range_image_top_pose_tensor[..., 2])
range_image_top_pose_tensor_translation = range_image_top_pose_tensor[..., 3:]
range_image_top_pose_tensor = transform_utils.get_transform(range_image_top_pose_tensor_rotation,range_image_top_pose_tensor_translation)
for c in calibrations:
range_image = range_images[c.name][ri_index]
if len(c.beam_inclinations) == 0:
beam_inclinations = range_image_utils.compute_inclination(tf.constant([c.beam_inclination_min, c.beam_inclination_max]),height=range_image.shape.dims[0])
else:
beam_inclinations = tf.constant(c.beam_inclinations)
beam_inclinations = tf.reverse(beam_inclinations, axis=[-1])
extrinsic = np.reshape(np.array(c.extrinsic.transform), [4, 4])
range_image_tensor = tf.reshape(tf.convert_to_tensor(range_image.data), range_image.shape.dims)
pixel_pose_local = None
frame_pose_local = None
if c.name == open_dataset.LaserName.TOP:
pixel_pose_local = range_image_top_pose_tensor
pixel_pose_local = tf.expand_dims(pixel_pose_local, axis=0)
frame_pose_local = tf.expand_dims(frame_pose, axis=0)
range_image_mask = range_image_tensor[..., 0] > 0
range_image_cartesian = range_image_utils.extract_point_cloud_from_range_image(tf.expand_dims(range_image_tensor[..., 0], axis=0),tf.expand_dims(extrinsic, axis=0),tf.expand_dims(tf.convert_to_tensor(beam_inclinations), axis=0),pixel_pose=pixel_pose_local,frame_pose=frame_pose_local)
range_image_cartesian = tf.squeeze(range_image_cartesian, axis=0)
points_tensor = tf.gather_nd(range_image_cartesian,tf.where(range_image_mask))
cp = camera_projections[c.name][0]
cp_tensor = tf.reshape(tf.convert_to_tensor(cp.data), cp.shape.dims)
cp_points_tensor = tf.gather_nd(cp_tensor, tf.where(range_image_mask))
points.append(points_tensor.numpy())
cp_points.append(cp_points_tensor.numpy())
return points, cp_points
def generate_extrinsic(yaw, pitch, roll, translation, batch=1):
"""Generates extrinsic.
Args:
yaw: scalar tensor
pitch: scalar tensor
roll: scalar tensor
translation: [3] tensor
batch: integer
Returns:
[batch, 4, 4] tensor
"""
rotation_matrix = transform_utils.get_rotation_matrix(roll, pitch, yaw)
return tf.tile(
tf.expand_dims(transform_utils.get_transform(
rotation_matrix, tf.constant(translation, dtype=tf.float32)),
axis=0), [batch, 1, 1])
def is_within_box_3d(point, box, name=None):
"""Checks whether a point is in a 3d box given a set of points and boxes.
Args:
point: [N, 3] tensor. Inner dims are: [x, y, z].
box: [M, 7] tensor. Inner dims are: [center_x, center_y, center_z, length,
width, height, heading].
name: tf name scope.
Returns:
point_in_box; [N, M] boolean tensor.
"""
with tf.compat.v1.name_scope(name, 'IsWithinBox3D', [point, box]):
center = box[:, 0:3]
dim = box[:, 3:6]
heading = box[:, 6]
# [M, 3, 3]
rotation = transform_utils.get_yaw_rotation(heading)
# [M, 4, 4]
transform = transform_utils.get_transform(rotation, center)
# [M, 4, 4]
transform = tf.linalg.inv(transform)
# [M, 3, 3]
rotation = transform[:, 0:3, 0:3]
# [M, 3]
translation = transform[:, 0:3, 3]
# [N, M, 3]
point_in_box_frame = tf.einsum('nj,mij->nmi', point,
rotation) + translation
# [N, M, 3]
point_in_box = tf.logical_and(
tf.logical_and(point_in_box_frame <= dim * 0.5,
point_in_box_frame >= -dim * 0.5),
tf.reduce_all(tf.not_equal(dim, 0), axis=-1, keepdims=True))
# [N, M]
point_in_box = tf.cast(tf.reduce_prod(input_tensor=tf.cast(
point_in_box, dtype=tf.uint8),
axis=-1),
dtype=tf.bool)
return point_in_box
def is_within_box_2d(point, box):
"""Checks whether a point is in a BEV box given a set of points and boxes.
Args:
point: [N, 2] tensor. Inner dims are: [x, y].
box: [M, 5] tensor. Inner dims are: [center_x, center_y, length, width,
heading].
Returns:
point_in_box; [N, M] boolean tensor.
"""
with tf.name_scope('IsWithinBox2D'):
center = box[:, 0:2]
dim = box[:, 2:4]
heading = box[:, 4]
# [M, 2, 2]
rotation = transform_utils.get_yaw_rotation_2d(heading)
# [M, 3, 3]
transform = transform_utils.get_transform(rotation, center)
# [M, 3, 3]
transform = tf.linalg.inv(transform)
# [M, 2, 2]
rotation = transform[:, 0:2, 0:2]
# [M, 2]
translation = transform[:, 0:2, 2]
# [N, M, 2]
point_in_box_frame = tf.einsum('nj,mij->nmi', point,
rotation) + translation
# [N, M, 2]
point_in_box = tf.logical_and(
tf.logical_and(point_in_box_frame <= dim * 0.5,
point_in_box_frame >= -dim * 0.5),
tf.reduce_all(tf.not_equal(dim, 0), axis=-1, keepdims=True))
# [N, M]
point_in_box = tf.cast(tf.reduce_prod(input_tensor=tf.cast(
point_in_box, dtype=tf.int32),
axis=-1),
dtype=tf.bool)
return point_in_box
def test_get_transform(self):
# [3, 3]
point = tf.constant([
[5.0, 0.0, 1.5],
[20.0, 0.0, 1.6],
[20.0, 0.0, 2.6],
],
dtype=tf.float32)
transform = transform_utils.get_transform(
transform_utils.get_rotation_matrix(1.0, 2.0, 1.5),
tf.constant([2.0, 3.0, 4.0]))
point_transformed = tf.einsum('ij,kj->ki', transform[0:3, 0:3],
point) + transform[0:3, 3]
transform = tf.matrix_inverse(transform)
point_transformed_back = tf.einsum('ij,kj->ki', transform[0:3, 0:3],
point_transformed) + transform[0:3,
3]
with self.test_session() as sess:
p1, p2 = sess.run([point, point_transformed_back])
self.assertAllClose(p1, p2)
def convert_range_image_to_point_cloud(frame,
range_images,
camera_projections,
range_image_top_pose,
ri_index=0):
"""Convert range images to point cloud.
Args:
frame: open dataset frame
range_images: A dict of {laser_name, [range_image_first_return,
range_image_second_return]}.
camera_projections: A dict of {laser_name,
[camera_projection_from_first_return,
camera_projection_from_second_return]}.
range_image_top_pose: range image pixel pose for top lidar.
ri_index: 0 for the first return, 1 for the second return.
Returns:
points: {[N, 3]} list of 3d lidar points of length 5 (number of lidars).
cp_points: {[N, 6]} list of camera projections of length 5
(number of lidars).
"""
calibrations = sorted(frame.context.laser_calibrations,
key=lambda c: c.name)
points = []
cp_points = []
frame_pose = tf.convert_to_tensor(
np.reshape(np.array(frame.pose.transform), [4, 4]))
# [H, W, 6]
range_image_top_pose_tensor = tf.reshape(
tf.convert_to_tensor(range_image_top_pose.data),
range_image_top_pose.shape.dims)
# [H, W, 3, 3]
range_image_top_pose_tensor_rotation = transform_utils.get_rotation_matrix(
range_image_top_pose_tensor[..., 0],
range_image_top_pose_tensor[..., 1], range_image_top_pose_tensor[...,
2])
range_image_top_pose_tensor_translation = range_image_top_pose_tensor[...,
3:]
range_image_top_pose_tensor = transform_utils.get_transform(
range_image_top_pose_tensor_rotation,
range_image_top_pose_tensor_translation)
for c in calibrations:
range_image = range_images[c.name][ri_index]
if len(c.beam_inclinations) == 0: # pylint: disable=g-explicit-length-test
beam_inclinations = range_image_utils.compute_inclination(
tf.constant([c.beam_inclination_min, c.beam_inclination_max]),
height=range_image.shape.dims[0])
else:
beam_inclinations = tf.constant(c.beam_inclinations)
beam_inclinations = tf.reverse(beam_inclinations, axis=[-1])
extrinsic = np.reshape(np.array(c.extrinsic.transform), [4, 4])
range_image_tensor = tf.reshape(tf.convert_to_tensor(range_image.data),
range_image.shape.dims)
pixel_pose_local = None
frame_pose_local = None
if c.name == dataset_pb2.LaserName.TOP:
pixel_pose_local = range_image_top_pose_tensor
pixel_pose_local = tf.expand_dims(pixel_pose_local, axis=0)
frame_pose_local = tf.expand_dims(frame_pose, axis=0)
range_image_mask = range_image_tensor[..., 0] > 0
range_image_cartesian = range_image_utils.extract_point_cloud_from_range_image(
tf.expand_dims(range_image_tensor[..., 0], axis=0),
tf.expand_dims(extrinsic, axis=0),
tf.expand_dims(tf.convert_to_tensor(beam_inclinations), axis=0),
pixel_pose=pixel_pose_local,
frame_pose=frame_pose_local)
range_image_cartesian = tf.squeeze(range_image_cartesian, axis=0)
points_tensor = tf.gather_nd(range_image_cartesian,
tf.where(range_image_mask))
cp = camera_projections[c.name][0]
cp_tensor = tf.reshape(tf.convert_to_tensor(cp.data), cp.shape.dims)
cp_points_tensor = tf.gather_nd(cp_tensor, tf.where(range_image_mask))
points.append(points_tensor.numpy())
cp_points.append(cp_points_tensor.numpy())
return points, cp_points
def extract_points_from_range_image(laser, calibration, frame_pose):
"""Decode points from lidar."""
if laser.name != calibration.name:
raise ValueError('Laser and calibration do not match')
if laser.name == dataset_pb2.LaserName.TOP:
frame_pose = tf.convert_to_tensor(
np.reshape(np.array(frame_pose.transform), [4, 4]))
range_image_top_pose = dataset_pb2.MatrixFloat.FromString(
zlib.decompress(laser.ri_return1.range_image_pose_compressed))
# [H, W, 6]
range_image_top_pose_tensor = tf.reshape(
tf.convert_to_tensor(range_image_top_pose.data),
range_image_top_pose.shape.dims)
# [H, W, 3, 3]
range_image_top_pose_tensor_rotation = transform_utils.get_rotation_matrix(
range_image_top_pose_tensor[..., 0],
range_image_top_pose_tensor[..., 1],
range_image_top_pose_tensor[..., 2])
range_image_top_pose_tensor_translation = range_image_top_pose_tensor[
..., 3:]
range_image_top_pose_tensor = transform_utils.get_transform(
range_image_top_pose_tensor_rotation,
range_image_top_pose_tensor_translation)
frame_pose = tf.expand_dims(frame_pose, axis=0)
pixel_pose = tf.expand_dims(range_image_top_pose_tensor, axis=0)
else:
pixel_pose = None
frame_pose = None
first_return = zlib.decompress(laser.ri_return1.range_image_compressed)
second_return = zlib.decompress(laser.ri_return2.range_image_compressed)
points_list = []
for range_image_str in [first_return, second_return]:
range_image = dataset_pb2.MatrixFloat.FromString(range_image_str)
if not calibration.beam_inclinations:
beam_inclinations = range_image_utils.compute_inclination(
tf.constant([
calibration.beam_inclination_min,
calibration.beam_inclination_max
]),
height=range_image.shape.dims[0])
else:
beam_inclinations = tf.constant(calibration.beam_inclinations)
beam_inclinations = tf.reverse(beam_inclinations, axis=[-1])
extrinsic = np.reshape(np.array(calibration.extrinsic.transform),
[4, 4])
range_image_tensor = tf.reshape(tf.convert_to_tensor(range_image.data),
range_image.shape.dims)
range_image_mask = range_image_tensor[..., 0] > 0
range_image_cartesian = (
range_image_utils.extract_point_cloud_from_range_image(
tf.expand_dims(range_image_tensor[..., 0], axis=0),
tf.expand_dims(extrinsic, axis=0),
tf.expand_dims(tf.convert_to_tensor(beam_inclinations),
axis=0),
pixel_pose=pixel_pose,
frame_pose=frame_pose))
range_image_cartesian = tf.squeeze(range_image_cartesian, axis=0)
points_tensor = tf.gather_nd(
tf.concat([range_image_cartesian, range_image_tensor[..., 1:4]],
axis=-1), tf.where(range_image_mask))
points_list.append(points_tensor.numpy())
return points_list
def convert_range_image_to_point_cloud(frame,
range_images,
range_image_top_pose,
tfp,
ri_index=0):
"""Convert range images to point cloud.
Args:
frame: open dataset frame
range_images: A dict of {laser_name, [range_image_first_return,
range_image_second_return]}.
range_image_top_pose: range image pixel pose for top lidar.
ri_index: 0 for the first return, 1 for the second return.
// * channel 0: range
// * channel 1: intensity
// * channel 2: elongation
// * channel 3: is in any no label zone.
Returns:
points: {[N, 3]} list of 3d lidar points of length 5 (number of lidars).
cp_points: {[N, 6]} list of camera projections of length 5
(number of lidars).
"""
from waymo_open_dataset.utils import transform_utils, range_image_utils
calibrations = sorted(frame.context.laser_calibrations, key=lambda c: c.name)
points = []
cp_points = []
frame_pose = tfp.convert_to_tensor(
value=np.reshape(np.array(frame.pose.transform), [4, 4]))
# [H, W, 6]
range_image_top_pose_tensor = tfp.reshape(
tfp.convert_to_tensor(value=range_image_top_pose.data),
range_image_top_pose.shape.dims)
# [H, W, 3, 3]
range_image_top_pose_tensor_rotation = transform_utils.get_rotation_matrix(
range_image_top_pose_tensor[..., 0], range_image_top_pose_tensor[..., 1],
range_image_top_pose_tensor[..., 2])
range_image_top_pose_tensor_translation = range_image_top_pose_tensor[..., 3:]
range_image_top_pose_tensor = transform_utils.get_transform(
range_image_top_pose_tensor_rotation,
range_image_top_pose_tensor_translation)
for c in calibrations:
range_image = range_images[c.name][ri_index]
if len(c.beam_inclinations) == 0: # pylint: disable=g-explicit-length-test
beam_inclinations = range_image_utils.compute_inclination(
tfp.constant([c.beam_inclination_min, c.beam_inclination_max]),
height=range_image.shape.dims[0])
else:
beam_inclinations = tfp.constant(c.beam_inclinations)
beam_inclinations = tfp.reverse(beam_inclinations, axis=[-1])
extrinsic = np.reshape(np.array(c.extrinsic.transform), [4, 4])
range_image_tensor = tfp.reshape(
tfp.convert_to_tensor(value=range_image.data), range_image.shape.dims)
pixel_pose_local = None
frame_pose_local = None
if c.name == open_dataset.LaserName.TOP:
pixel_pose_local = range_image_top_pose_tensor
pixel_pose_local = tfp.expand_dims(pixel_pose_local, axis=0)
frame_pose_local = tfp.expand_dims(frame_pose, axis=0)
range_image_mask = range_image_tensor[..., 0] > 0
range_image_cartesian = range_image_utils.extract_point_cloud_from_range_image(
tfp.expand_dims(range_image_tensor[..., 0], axis=0),
tfp.expand_dims(extrinsic, axis=0),
tfp.expand_dims(tfp.convert_to_tensor(value=beam_inclinations), axis=0),
pixel_pose=pixel_pose_local,
frame_pose=frame_pose_local)
range_image_cartesian = tfp.squeeze(range_image_cartesian, axis=0)
points_tensor = tfp.gather_nd(range_image_cartesian,
tfp.compat.v1.where(range_image_mask))
intensity_tensor = tfp.gather_nd(tfp.expand_dims(range_image_tensor[..., 1], axis=2),
tfp.compat.v1.where(range_image_mask))
elongation_tensor = tfp.gather_nd(tfp.expand_dims(range_image_tensor[..., 2], axis=2),
tfp.compat.v1.where(range_image_mask))
stacked_tensor = np.hstack((points_tensor.numpy(),intensity_tensor.numpy(),elongation_tensor.numpy()))
points.append(stacked_tensor)
return points
def convert_range_image_to_point_cloud(self,
frame,
range_images,
camera_projections,
range_image_top_pose,
ri_index=0):
"""Convert range images to point cloud.
Args:
frame (:obj:`Frame`): Open dataset frame.
range_images (dict): Mapping from laser_name to list of two
range images corresponding with two returns.
camera_projections (dict): Mapping from laser_name to list of two
camera projections corresponding with two returns.
range_image_top_pose (:obj:`Transform`): Range image pixel pose for
top lidar.
ri_index (int): 0 for the first return, 1 for the second return.
Default: 0.
Returns:
tuple[list[np.ndarray]]: (List of points with shape [N, 3],
camera projections of points with shape [N, 6], intensity
with shape [N, 1], elongation with shape [N, 1]). All the
lists have the length of lidar numbers (5).
"""
calibrations = sorted(frame.context.laser_calibrations,
key=lambda c: c.name)
points = []
cp_points = []
intensity = []
elongation = []
frame_pose = tf.convert_to_tensor(
value=np.reshape(np.array(frame.pose.transform), [4, 4]))
# [H, W, 6]
range_image_top_pose_tensor = tf.reshape(
tf.convert_to_tensor(value=range_image_top_pose.data),
range_image_top_pose.shape.dims)
# [H, W, 3, 3]
range_image_top_pose_tensor_rotation = \
transform_utils.get_rotation_matrix(
range_image_top_pose_tensor[..., 0],
range_image_top_pose_tensor[..., 1],
range_image_top_pose_tensor[..., 2])
range_image_top_pose_tensor_translation = \
range_image_top_pose_tensor[..., 3:]
range_image_top_pose_tensor = transform_utils.get_transform(
range_image_top_pose_tensor_rotation,
range_image_top_pose_tensor_translation)
for c in calibrations:
range_image = range_images[c.name][ri_index]
if len(c.beam_inclinations) == 0:
beam_inclinations = range_image_utils.compute_inclination(
tf.constant(
[c.beam_inclination_min, c.beam_inclination_max]),
height=range_image.shape.dims[0])
else:
beam_inclinations = tf.constant(c.beam_inclinations)
beam_inclinations = tf.reverse(beam_inclinations, axis=[-1])
extrinsic = np.reshape(np.array(c.extrinsic.transform), [4, 4])
range_image_tensor = tf.reshape(
tf.convert_to_tensor(value=range_image.data),
range_image.shape.dims)
pixel_pose_local = None
frame_pose_local = None
if c.name == dataset_pb2.LaserName.TOP:
pixel_pose_local = range_image_top_pose_tensor
pixel_pose_local = tf.expand_dims(pixel_pose_local, axis=0)
frame_pose_local = tf.expand_dims(frame_pose, axis=0)
range_image_mask = range_image_tensor[..., 0] > 0
if self.filter_no_label_zone_points:
nlz_mask = range_image_tensor[..., 3] != 1.0 # 1.0: in NLZ
range_image_mask = range_image_mask & nlz_mask
range_image_cartesian = \
range_image_utils.extract_point_cloud_from_range_image(
tf.expand_dims(range_image_tensor[..., 0], axis=0),
tf.expand_dims(extrinsic, axis=0),
tf.expand_dims(tf.convert_to_tensor(
value=beam_inclinations), axis=0),
pixel_pose=pixel_pose_local,
frame_pose=frame_pose_local)
range_image_cartesian = tf.squeeze(range_image_cartesian, axis=0)
points_tensor = tf.gather_nd(range_image_cartesian,
tf.compat.v1.where(range_image_mask))
cp = camera_projections[c.name][ri_index]
cp_tensor = tf.reshape(tf.convert_to_tensor(value=cp.data),
cp.shape.dims)
cp_points_tensor = tf.gather_nd(
cp_tensor, tf.compat.v1.where(range_image_mask))
points.append(points_tensor.numpy())
cp_points.append(cp_points_tensor.numpy())
intensity_tensor = tf.gather_nd(range_image_tensor[..., 1],
tf.where(range_image_mask))
intensity.append(intensity_tensor.numpy())
elongation_tensor = tf.gather_nd(range_image_tensor[..., 2],
tf.where(range_image_mask))
elongation.append(elongation_tensor.numpy())
return points, cp_points, intensity, elongation
def convert_range_image_to_cartesian(frame,
range_images,
range_image_top_pose,
ri_index=0,
keep_polar_features=False):
"""Convert range images from polar coordinates to Cartesian coordinates.
Args:
frame: open dataset frame
range_images: A dict of {laser_name, [range_image_first_return,
range_image_second_return]}.
range_image_top_pose: range image pixel pose for top lidar.
ri_index: 0 for the first return, 1 for the second return.
keep_polar_features: If true, keep the features from the polar range image
(i.e. range, intensity, and elongation) as the first features in the
output range image.
Returns:
dict of {laser_name, (H, W, D)} range images in Cartesian coordinates. D
will be 3 if keep_polar_features is False (x, y, z) and 6 if
keep_polar_features is True (range, intensity, elongation, x, y, z).
"""
cartesian_range_images = {}
frame_pose = tf.convert_to_tensor(
value=np.reshape(np.array(frame.pose.transform), [4, 4]))
# [H, W, 6]
range_image_top_pose_tensor = tf.reshape(
tf.convert_to_tensor(value=range_image_top_pose.data),
range_image_top_pose.shape.dims)
# [H, W, 3, 3]
range_image_top_pose_tensor_rotation = transform_utils.get_rotation_matrix(
range_image_top_pose_tensor[..., 0], range_image_top_pose_tensor[..., 1],
range_image_top_pose_tensor[..., 2])
range_image_top_pose_tensor_translation = range_image_top_pose_tensor[..., 3:]
range_image_top_pose_tensor = transform_utils.get_transform(
range_image_top_pose_tensor_rotation,
range_image_top_pose_tensor_translation)
for c in frame.context.laser_calibrations:
range_image = range_images[c.name][ri_index]
if len(c.beam_inclinations) == 0: # pylint: disable=g-explicit-length-test
beam_inclinations = range_image_utils.compute_inclination(
tf.constant([c.beam_inclination_min, c.beam_inclination_max]),
height=range_image.shape.dims[0])
else:
beam_inclinations = tf.constant(c.beam_inclinations)
beam_inclinations = tf.reverse(beam_inclinations, axis=[-1])
extrinsic = np.reshape(np.array(c.extrinsic.transform), [4, 4])
range_image_tensor = tf.reshape(
tf.convert_to_tensor(value=range_image.data), range_image.shape.dims)
pixel_pose_local = None
frame_pose_local = None
if c.name == dataset_pb2.LaserName.TOP:
pixel_pose_local = range_image_top_pose_tensor
pixel_pose_local = tf.expand_dims(pixel_pose_local, axis=0)
frame_pose_local = tf.expand_dims(frame_pose, axis=0)
range_image_cartesian = range_image_utils.extract_point_cloud_from_range_image(
tf.expand_dims(range_image_tensor[..., 0], axis=0),
tf.expand_dims(extrinsic, axis=0),
tf.expand_dims(tf.convert_to_tensor(value=beam_inclinations), axis=0),
pixel_pose=pixel_pose_local,
frame_pose=frame_pose_local)
range_image_cartesian = tf.squeeze(range_image_cartesian, axis=0)
if keep_polar_features:
# If we want to keep the polar coordinate features of range, intensity,
# and elongation, concatenate them to be the initial dimensions of the
# returned Cartesian range image.
range_image_cartesian = tf.concat(
[range_image_tensor[..., 0:3], range_image_cartesian], axis=-1)
cartesian_range_images[c.name] = range_image_cartesian
return cartesian_range_images
def yzl_convert_range_image_to_point_cloud(frame,
range_images,
camera_projections,
range_image_top_pose,
ri_index=0):
"""Convert range images to point cloud.
Args:
frame: open dataset frame
range_images: A dict of {laser_name, [range_image_first_return,
range_image_second_return]}.
camera_projections: A dict of {laser_name,
[camera_projection_from_first_return,
camera_projection_from_second_return]}.
range_image_top_pose: range image pixel pose for top lidar.
ri_index: 0 for the first return, 1 for the second return.
Returns:
points: {[N, 12]} list of 3d lidar points of length 5 (number of lidars).
x y z intensity elongation is_in_nlz
channel 6: camera name
channel 7: x (axis along image width)
channel 8: y (axis along image height)
channel 9: camera name of 2nd projection (set to UNKNOWN if no projection)
channel 10: x (axis along image width)
channel 11: y (axis along image height)
"""
calibrations = sorted(frame.context.laser_calibrations, key=lambda c: c.name)
points = []
cp_points = []
frame_pose = tf.convert_to_tensor(
value=np.reshape(np.array(frame.pose.transform), [4, 4]))
# [H, W, 6]
range_image_top_pose_tensor = tf.reshape(
tf.convert_to_tensor(value=range_image_top_pose.data),
range_image_top_pose.shape.dims)
# [H, W, 3, 3]
range_image_top_pose_tensor_rotation = transform_utils.get_rotation_matrix(
range_image_top_pose_tensor[..., 0], range_image_top_pose_tensor[..., 1],
range_image_top_pose_tensor[..., 2])
range_image_top_pose_tensor_translation = range_image_top_pose_tensor[..., 3:]
range_image_top_pose_tensor = transform_utils.get_transform(
range_image_top_pose_tensor_rotation,
range_image_top_pose_tensor_translation)
for c in calibrations:
range_image = range_images[c.name][ri_index]
if len(c.beam_inclinations) == 0: # pylint: disable=g-explicit-length-test
beam_inclinations = range_image_utils.compute_inclination(
tf.constant([c.beam_inclination_min, c.beam_inclination_max]),
height=range_image.shape.dims[0])
else:
beam_inclinations = tf.constant(c.beam_inclinations)
beam_inclinations = tf.reverse(beam_inclinations, axis=[-1])
extrinsic = np.reshape(np.array(c.extrinsic.transform), [4, 4])
range_image_tensor = tf.reshape(
tf.convert_to_tensor(value=range_image.data), range_image.shape.dims)
pixel_pose_local = None
frame_pose_local = None
if c.name == open_dataset.LaserName.TOP:
pixel_pose_local = range_image_top_pose_tensor
pixel_pose_local = tf.expand_dims(pixel_pose_local, axis=0)
frame_pose_local = tf.expand_dims(frame_pose, axis=0)
range_image_mask = range_image_tensor[..., 0] > 0 #tf.ones_like(range_image_tensor[..., 0])
range_image_cartesian = range_image_utils.extract_point_cloud_from_range_image(
tf.expand_dims(range_image_tensor[..., 0], axis=0),
tf.expand_dims(extrinsic, axis=0),
tf.expand_dims(tf.convert_to_tensor(value=beam_inclinations), axis=0),
pixel_pose=pixel_pose_local,
frame_pose=frame_pose_local)
range_image_cartesian = tf.squeeze(range_image_cartesian, axis=0)
range_image_cartesian = tf.concat([range_image_cartesian,range_image_tensor[..., 1:]],axis=2)
points_tensor = tf.gather_nd(range_image_cartesian,
tf.compat.v1.where(range_image_mask))
cp = camera_projections[c.name][0]
cp_tensor = tf.reshape(tf.convert_to_tensor(value=cp.data,dtype=tf.float32), cp.shape.dims)
cp_points_tensor = tf.gather_nd(cp_tensor,
tf.compat.v1.where(range_image_mask))
points_tensor = tf.concat([points_tensor, cp_points_tensor],axis=1)
points.append(points_tensor.numpy())
return points
