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 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 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) points = [] cp_points = [] channels = [] 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 == 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(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)) channels_tensor = tf.gather_nd( range_image_tensor, tf.compat.v1.where(range_image_mask)) # <- modified cp = camera_projections[c.name][0] 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()) channels.append(channels_tensor.numpy()[:, [1, 2]]) # <- modified return points, cp_points, channels
def convert_range_image_to_point_cloud(self, frame, range_images, camera_projections, range_image_top_pose, ri_index=0): 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 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