show point cloud data in birdeye view

show_pcd_in_birdeye_view.py

## reference: http://ronny.rest/tutorials/module/pointclouds_01/point_cloud_birdseye/
##     https://blog.csdn.net/weixin_39999955/article/details/83819313

import os
import sys
import numpy as np
import pypcd
from PIL import Image


def scale_to_255(arr, min, max, dtype=np.uint8):
    """ Scales an array of values from specified min, max range to 0-255
        Optionally specify the data type of the output (default is uint8)
    """
    return (((arr - min) / float(max - min)) * 255).astype(dtype)


def point_cloud_2_birdseye(points,
                           res=0.1,
                           side_range=(-50., 50.),  # left-most to right-most
                           fwd_range = (-50., 50.), # back-most to forward-most
                           height_range=(-2., 2.),  # bottom-most to upper-most
                           ):
    """ Creates an 2D birds eye view representation of the point cloud data.
    Args:
        points:     (numpy array)
                    N rows of points data
                    Each point should be specified by at least 3 elements x,y,z
        res:        (float)
                    Desired resolution in metres to use. Each output pixel will
                    represent an square region res x res in size.
        side_range: (tuple of two floats)
                    (-left, right) in metres
                    left and right limits of rectangle to look at.
        fwd_range:  (tuple of two floats)
                    (-behind, front) in metres
                    back and front limits of rectangle to look at.
        height_range: (tuple of two floats)
                    (min, max) heights (in metres) relative to the origin.
                    All height values will be clipped to this min and max value,
                    such that anything below min will be truncated to min, and
                    the same for values above max.
    Returns:
        2D numpy array representing an image of the birds eye view.
    """
    # EXTRACT THE POINTS FOR EACH AXIS
    x_points = points[:, 0]
    y_points = points[:, 1]
    z_points = points[:, 2]

    # FILTER - To return only indices of points within desired cube
    # Three filters for: Front-to-back, side-to-side, and height ranges
    # Note left side is positive y axis in LIDAR coordinates
    f_filt = np.logical_and((x_points > fwd_range[0]), (x_points < fwd_range[1]))
    s_filt = np.logical_and((y_points > -side_range[1]), (y_points < -side_range[0]))
    filter = np.logical_and(f_filt, s_filt)
    indices = np.argwhere(filter).flatten()

    # KEEPERS
    x_points = x_points[indices]
    y_points = y_points[indices]
    z_points = z_points[indices]

    # CONVERT TO PIXEL POSITION VALUES - Based on resolution
    x_img = (-y_points / res).astype(np.int32)  # x axis is -y in LIDAR
    y_img = (-x_points / res).astype(np.int32)  # y axis is -x in LIDAR

    # SHIFT PIXELS TO HAVE MINIMUM BE (0,0)
    # floor & ceil used to prevent anything being rounded to below 0 after shift
    x_img -= int(np.floor(side_range[0] / res))
    y_img += int(np.ceil(fwd_range[1] / res))

    # CLIP HEIGHT VALUES - to between min and max heights
    pixel_values = np.clip(a=z_points,
                           a_min=height_range[0],
                           a_max=height_range[1])

    # RESCALE THE HEIGHT VALUES - to be between the range 0-255
    pixel_values = scale_to_255(pixel_values,
                                min=height_range[0],
                                max=height_range[1])

    # INITIALIZE EMPTY ARRAY - of the dimensions we want
    x_max = 1 + int((side_range[1] - side_range[0]) / res)
    y_max = 1 + int((fwd_range[1] - fwd_range[0]) / res)
    im = np.zeros([y_max, x_max], dtype=np.uint8)

    # FILL PIXEL VALUES IN IMAGE ARRAY
    im[y_img, x_img] = pixel_values

    return im


if __name__ == '__main__':
    if len(sys.argv) != 3:
        print('Usage: python {} <filepath> <output_img_path>'.format( \
                sys.argv[0]))
        sys.exit()

    filepath = sys.argv[1]
    output_img_path = sys.argv[2]
    print('filepath: {}'.format(filepath))
    pc_points = None
    if filepath.endswith('.pcd'):
        print('loading pcd file...')
        pc = pypcd.PointCloud.from_path(filepath)
        print('done.')
        pc_points = np.array([[pt[0], pt[1], pt[2]] for pt in pc.pc_data])
    elif filepath.endswith('.bin'):
        print('loading kitti bin file...')
        pc = np.fromfile(filepath, dtype=np.float32, count=-1).reshape([-1, 4])
        print('done.')
        pc_points = np.array([[pt[0], pt[1], pt[2]] for pt in pc])

    if pc_points is None:
        print('Unsupported file type!')
        sys.exit()

    im = point_cloud_2_birdseye(pc_points)
    Image.fromarray(im).save(output_img_path)