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:**

.. list-table::
   :widths: 40 30 30
   :header-rows: 1

   * - Topic
     - Type
     - Description
   * - ``rslidar_points``
     - sensor_msgs/PointCloud2
     - Raw LiDAR point cloud with XYZ and intensity

**Published Topics:**

.. list-table::
   :widths: 40 30 30
   :header-rows: 1

   * - 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:

   .. code-block:: python

      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:

   .. code-block:: python

      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:

   .. math::

      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:

   .. code-block:: python

      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:

   .. code-block:: python

      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:

   .. code-block:: python

      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:

   .. code-block:: python

      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:

   .. code-block:: python

      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``:

.. list-table::
   :widths: 40 60
   :header-rows: 1

   * - 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:

.. code-block:: python

   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:

.. code-block:: python

   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:

.. code-block:: text

   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
-------------

.. code-block:: python

   import rclpy
   from lidar_perception.main import LiDARCones

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

Launch with ROS2:

.. code-block:: bash

   ros2 run lidar_perception main

API Reference
-------------

.. automodule:: perception.lidar_perception.lidar_perception.main
   :members:
   :undoc-members:
   :show-inheritance:

.. automodule:: perception.lidar_perception.lidar_perception.visual_debugging
   :members:
   :undoc-members:
   :show-inheritance: