Any type of LiDAR point cloud to ring-based LiDAR style

ToRing.py

import numpy.linalg as LA
import numpy as np

def any_LiDAR_to_ring(pc, num_beams=32, ring_height=8e-4):
    """
    convert any type of LiDAR point cloud to ring-based LiDAR style
    :param pc: input point cloud, shape of Nx4(x,y,z,intensity)
    :param num_beams: number of beams
    :param ring_height: the "line width" of a ring
    :return: ring-stype point cloud, shape of Nx5(x,y,z,intensity, ring ID)
    """
    pitch = np.arctan(pc[:, 2] / (LA.norm(pc[:, :2], axis=1, ord=2) + 1e-10))
    pitch = np.nan_to_num(pitch)

    beams = np.linspace(pitch.min(), pitch.max(), num=num_beams + 1)

    rings = []
    for beam_id, beam_angle in enumerate(beams):
        ring = pc[np.where(
            (pitch > beam_angle) &
            (pitch < beam_angle + ring_height)
        )]
        ring_ids = beam_id * np.ones_like(ring[:, 0])

        # sort according to azimuth
        az = np.arctan(ring[:, 1] / ring[:, 0])
        ring = ring[np.argsort(az), :]

        rings.append(np.column_stack([ring, ring_ids]))
    ring_pc = np.row_stack(rings)
    return ring_pc