LiDAR Only

Overview

The LiDAR Only perception system detects cones using 3D point cloud data from a Robosense LiDAR sensor. This approach relies purely on geometric features and clustering algorithms to identify cone-like objects without requiring visual information or color classification.

Key Concept: Point clouds are filtered to remove ground points, then clustered using GPU-accelerated DBSCAN. Clusters matching cone-like dimensions are identified and their positions estimated.

System Architecture

The LiDAR-only system provides:

  • Ground Plane Filtering: Removes ground points using fitted plane equation

  • GPU-Accelerated Clustering: Uses RAPIDS cuML DBSCAN for fast clustering on NVIDIA GPUs

  • Geometric Filtering: Identifies cone clusters based on bounding box dimensions

  • Position Estimation: Computes cone centroids from clustered points

Advantages:

  • Works in all lighting conditions (day/night)

  • Provides accurate 3D measurements

  • No camera calibration required

  • Simpler sensor setup

Limitations:

  • No color classification (all cones marked as unknown)

  • Sensitive to LiDAR point density

  • May struggle with distant/small cones

  • Requires GPU for real-time performance

LiDAR Cones Node

The LiDARCones class processes LiDAR point clouds to detect cone positions.

Key Features

  • Ground Plane Removal: Vectorized filtering using plane equation

  • GPU Acceleration: cuML DBSCAN clustering on NVIDIA GPUs with CUDA

  • Geometric Validation: Bounding box filtering to identify cone-shaped clusters

  • Performance Monitoring: Detailed timing breakdowns for each processing stage

  • Visualization Support: Optional filtered point cloud publishing

ROS Topics

Subscribed Topics:

Topic

Type

Description

rslidar_points

sensor_msgs/PointCloud2

Raw LiDAR point cloud with XYZ and intensity

Published Topics:

Topic

Type

Description

/lidar/cones

ConesCartesian

Detected cone positions (x, y, color=-1)

cones/viz/lidar/new

sensor_msgs/PointCloud

Visualization of detected cone positions

/filtered_pointcloud

sensor_msgs/PointCloud2

Ground-filtered point cloud (optional)

Processing Pipeline

The LiDAR processing pipeline consists of the following stages:

  1. Point Cloud Extraction

    Convert ROS PointCloud2 message to NumPy arrays:

    points_xyz = ros2_numpy.point_cloud2.point_cloud2_to_array(pointcloud)['xyz']
intensities = ros2_numpy.point_cloud2.point_cloud2_to_array(pointcloud)['intensity']

points = np.hstack([points_xyz, intensities])
points = points[~np.isnan(points).any(axis=1)]  # Remove NaN values

  2. Ground Plane Filtering

    Remove ground points using the calibrated ground plane equation:

    filtered_points = filter_points_by_plane_and_distance(
    points,
    threshold=ground_filter_threshold,
    top_threshold=ground_filter_top_threshold,
    max_distance=max_distance
)

    The filter uses the plane equation:

    \[ax + by + cz + d = 0\]

    Points are kept if:

    • Distance from plane: threshold < distance < top\_threshold

    • Distance from origin: distance ≤ max\_distance

    This vectorized operation efficiently removes thousands of ground points:

    distance_to_plane = (a*x + b*y + c*z + d) / sqrt(a**2 + b**2 + c**2)
distance_from_origin = sqrt(x**2 + y**2 + z**2)

mask = (threshold < distance_to_plane) &
       (distance_to_plane < top_threshold) &
       (distance_from_origin <= max_distance)

  3. GPU-Accelerated DBSCAN Clustering

    Transfer data to GPU and perform density-based clustering:

    import cupy as cp
from cuml.cluster import DBSCAN as cuDBSCAN

# Transfer to GPU
filtered_points_gpu = cp.asarray(filtered_points[:, :3])

# Cluster using cuML DBSCAN
clusterer = cuDBSCAN(eps=eps, min_samples=min_samples)
clusterer.fit(filtered_points_gpu)
labels = clusterer.labels_

    DBSCAN Parameters:

    • eps: Maximum distance between points in a cluster (neighborhood radius)

    • min_samples: Minimum points required to form a dense cluster

    Points not belonging to any cluster are labeled as -1 (noise).

  4. Cone Cluster Filtering

    Filter clusters to identify cone-like objects based on bounding box dimensions:

    for label in unique_labels:
    cluster = points[labels == label]

    min_values = cluster.min(axis=0)
    max_values = cluster.max(axis=0)

    x_box = max_values[0] - min_values[0]
    y_box = max_values[1] - min_values[1]
    z_box = max_values[2] - min_values[2]

    # Check if dimensions match cone size
    if (x_size[0] < x_box < x_size[1] and
        y_size[0] < y_box < y_size[1] and
        z_size[0] < z_box < z_size[1]):
        cones.append(label)

    This geometric filtering rejects clusters that are too large (vehicles, walls) or too small (noise).

  5. Cone Position Estimation

    Compute the representative position for each cone cluster:

    def find_cone_position(pointcloud, method='median'):
    if method == 'median':
        cone_positions = np.median(pointcloud, axis=0)
    elif method == 'mean':
        cone_positions = np.mean(pointcloud, axis=0)

    return cone_positions[0:2]  # Return (x, y)

    Median vs Mean:

    • Median (default): Robust to outliers, preferred for noisy data

    • Mean: Faster computation, sensitive to outlier points

  6. Publishing

    Publish detected cones:

    cone_msg = ConesCartesian()
cone_msg.x.append(cone_pose[0])
cone_msg.y.append(cone_pose[1])
cone_msg.color.append(-1)  # Unknown color
cone_msg.header.stamp = self.get_clock().now().to_msg()

self.perception_pub.publish(cone_msg)

    Note: Color is always -1 since LiDAR cannot distinguish cone colors.

Configuration Parameters

From LiDAROnlySettings:

Parameter

Description

ground_plane_coefficients

Tuple (a, b, c, d) defining ground plane equation

ground_filter_threshold

Minimum distance from ground plane (meters)

ground_filter_top_threshold

Maximum distance from ground plane (meters)

max_distance

Maximum range for point filtering (meters)

eps

DBSCAN epsilon parameter (cluster radius in meters)

min_samples

DBSCAN minimum points per cluster

x_size

Tuple (min, max) for cone bounding box X dimension (meters)

y_size

Tuple (min, max) for cone bounding box Y dimension (meters)

z_size

Tuple (min, max) for cone bounding box Z dimension (meters)

publish_ground_filtered_pointcloud

Enable publishing of filtered point cloud for debugging

Ground Plane Calibration

The ground plane must be calibrated to match the vehicle’s LiDAR mounting position. The plane equation coefficients (a, b, c, d) are typically obtained through:

  1. Recording a static point cloud with the vehicle on flat ground

  2. Fitting a plane to the ground points using RANSAC or least squares

  3. Storing the resulting coefficients in settings

The module includes helper utilities for ground plane calibration:

  • find_ground_gui.py: Interactive GUI for visualizing and fitting ground planes

  • find_ground_plane.py: Automated plane fitting

Bounding Box Filtering

Cone dimensions are validated using 3D bounding boxes. Typical Formula Student cone dimensions:

  • Height: ~0.3 m

  • Base diameter: ~0.23 m

With LiDAR point cloud scatter, appropriate size ranges might be:

  • x_size: (0.05, 0.35) meters

  • y_size: (0.05, 0.25) meters

  • z_size: (0.10, 0.40) meters

These ranges should be tuned based on:

  • LiDAR vertical/horizontal resolution

  • Typical detection range

  • Cone visibility and occlusion patterns

Visualization and Debugging

Filtered Point Cloud

Enable publishing of ground-filtered points:

publish_ground_filtered_pointcloud: True

View in RViz on /filtered_pointcloud topic to verify ground removal.

3D Cluster Visualization

The plot_clusters_3d() function creates interactive matplotlib plots:

plot_clusters_3d(filtered_points[:, :3], labels)

Shows all clusters with unique colors, useful for tuning DBSCAN parameters.

Console Logging

The node prints detailed timing information:

Original: 52341 pts, Filtered: 1523 pts
Extracting Pointcloud: 0.008 s
Filtering Points: 0.012 s
Clustering Points: 0.015 s
Finding Cones From Clusters: 0.003 s
Total Time: 0.038 s

Usage Example

import rclpy
from lidar_perception.main import LiDARCones

rclpy.init()
node = LiDARCones()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()

Launch with ROS2:

ros2 run lidar_perception main

API Reference

class perception.lidar_perception.lidar_perception.main.LiDARCones[source]

Bases: rclpy.node.Node

ROS2 node for detecting cones from LiDAR point clouds using DBSCAN clustering.

lidar_callback(pointcloud)[source]

Process LiDAR point cloud to detect cones.

Filters ground points, clusters using GPU-accelerated DBSCAN, and validates cone-like clusters.

Parameters

pointcloud (PointCloud2) – Raw LiDAR point cloud message

filter_points_by_plane_and_distance(points, threshold=0.05, top_threshold=0.5, max_distance=10.0)[source]

Filter points using ground plane equation and distance constraints.

Parameters

  • points (np.ndarray) – Nx4 array (x, y, z, intensity)

  • threshold (float) – Minimum distance from ground plane (m)

  • top_threshold (float) – Maximum distance from ground plane (m)

  • max_distance (float) – Maximum range from origin (m)

Returns

Filtered points above ground within range

Return type

np.ndarray

filter_cluster_for_cones(points, labels)[source]

Filter DBSCAN clusters to identify cone-like objects by bounding box dimensions.

Parameters

  • points (np.ndarray) – Filtered point cloud

  • labels (np.ndarray) – DBSCAN cluster labels

Returns

Cluster labels that match cone dimensions

Return type

list

filter_cluster_for_cones_cuML(points, labels)[source]

Filter cuML DBSCAN clusters for cone-like objects using configurable bounding box constraints.

Parameters

  • points (np.ndarray) – Filtered point cloud

  • labels (cupy.ndarray) – cuML DBSCAN cluster labels (GPU array)

Returns

Cluster labels matching cone size criteria from settings

Return type

list

find_cone_position(pointcloud, method='median')[source]

Compute cone position from clustered points.

Parameters

  • pointcloud (np.ndarray) – Nx3 array of 3D points in cone cluster

  • method (str) – ‘median’ (robust) or ‘mean’ (faster)

Returns

2D position (x, y) in meters

Return type

np.ndarray

truth_sub_callback(msg)[source]
perception.lidar_perception.lidar_perception.main.main(args=None)[source]
perception.lidar_perception.lidar_perception.visual_debugging.publish_filtered_pointcloud(self, points, header)[source]

Publish ground-filtered point cloud for visualization in RViz.

Parameters

  • points (np.ndarray) – Nx4 array of filtered points (x, y, z, intensity)

  • header – ROS message header with frame_id and timestamp

perception.lidar_perception.lidar_perception.visual_debugging.plot_clusters_3d(self, points, labels)[source]

Plot clustered 3D points using matplotlib for DBSCAN parameter tuning.

Parameters

  • points (np.ndarray) – Nx3 array of 3D points

  • labels (np.ndarray) – Cluster labels for each point (-1 for noise)

perception.lidar_perception.lidar_perception.visual_debugging.publish_raw_pointcloud(self, filtered_points, header)[source]

Publish raw filtered point cloud using ros2_numpy conversion.

Parameters

  • filtered_points (np.ndarray) – Nx4 array (x, y, z, intensity)

  • header – ROS message header

perception.lidar_perception.lidar_perception.visual_debugging.publish_ground_plane_marker(self)[source]

Publish RViz marker visualizing the calibrated ground plane.