StoreTracker: Real-Time People Detection and Tracking in Point Cloud Data

Overview

StoreTracker is a ROS 2 node designed to detect and track people in real-time using point cloud data from LiDAR sensors. The node integrates built-in Livox Mid-360 Inertial Measurement Unit (IMU) data for orientation correction, processes the point cloud through a series of filters, and then extracts and tracks clusters of points that likely represent people. StoreTracker is suitable for use in environments where monitoring human presence and movement is essential, such as in retail stores, warehouses, or public spaces.

Node Logic and Processing Pipeline

1. Initialization and Configuration

Upon launching, StoreTracker initializes various parameters from a configuration file or command-line arguments. The node subscribes to point cloud data from a LiDAR sensor and IMU data, if available. It also initializes publishers for filtered point clouds, detected people, and optionally, posts detection data to a remote server.

Key Components Initialized:

• IMU Averaging (imu_average_): Accumulates IMU data to compute an average orientation, which is used to correct the orientation of the point cloud.
• Point Cloud Storage (mapCloud_, scans_): Stores incoming point cloud data for further processing.
• Filters: Configures voxel grid filters, pass-through filters, and plane extraction filters based on the specified parameters.
• Cluster Extraction: Sets up parameters for extracting clusters of points that likely represent people.
• People Tracker (tracker_): Manages the detection and tracking of people over time.

2. IMU Data Handling

The IMU data is crucial for ensuring the point cloud is correctly oriented. The node subscribes to IMU data and stores it in an averaging buffer (imu_average_). This buffer accumulates IMU readings and computes an average orientation. This orientation data is then applied to the incoming point cloud data to align it with the real-world coordinates.

IMU Data Processing Workflow:

1. Accumulation: IMU readings are collected in the buffer.
2. Averaging: The node calculates the average roll and pitch from the accumulated data.
3. Transformation: The calculated orientation is applied to the point cloud data to correct its alignment.

3. Point Cloud Processing

The core of StoreTracker's functionality lies in its ability to process point cloud data efficiently. The node applies a sequence of filters and transformations to refine the point cloud and extract meaningful clusters that represent people.

Processing Steps:

1. Voxel Grid Filtering:

   • Purpose: Reduces the number of points in the cloud by approximating points within a voxel to a single representative point.
   • Parameters: leaf_size controls the resolution of the downsampling.
   • Outcome: A downsampled point cloud that retains the essential structure while reducing computational load.

2. Pass-Through Filtering:

   • Purpose: Removes points that are outside a specified height range, effectively filtering out irrelevant data (e.g., points on the floor or ceiling).
   • Parameters: filter_x, filter_y, filter_z enable or disable filtering along each axis; z_min, z_max define the height range.
   • Parameters: height and width to define detection range, in meters.
   • Outcome: A filtered point cloud focusing on the region of interest.

3. Plane Extraction:

   • Purpose: Uses a RANSAC algorithm to identify and remove planar surfaces, such as the ground, from the point cloud.
   • Parameters: max_iterations, distance_threshold, removal_threshold control the behavior of the plane extraction.
   • Outcome: A point cloud with planar surfaces removed, leaving potential human clusters intact.

4. Cluster Extraction:

   • Purpose: Groups points into clusters based on their spatial proximity, with the assumption that each cluster represents an individual person.
   • Parameters: tolerance sets the maximum distance between points in a cluster; min_cluster_size, max_cluster_size define the acceptable cluster size.
   • Outcome: A set of clusters, each likely representing a detected person.

4. Calibration Phase and Map Creation

During the calibration phase, StoreTracker creates a map of the environment by aggregating multiple scans. This map serves as a reference against which incoming scans are compared to identify dynamic points (new or moving objects).

Calibration Workflow:

1. Scan Accumulation: Multiple scans are collected and aligned using IMU data.
2. Map Generation: The aligned scans are merged into a single point cloud that represents the environment.
3. Map Storage: The generated map is stored and used for dynamic point extraction during regular operation.

5. Dynamic Point Extraction

During operation, StoreTracker compares incoming point cloud scans with the pre-built map of the environment to identify and extract dynamic points—those that are new or moving. This process is crucial for detecting moving objects or changes in the environment.

Dynamic Point Extraction Workflow:

1. Map Comparison: Each incoming point cloud is compared against a pre-built map stored in a KD-tree (kdtreeMap_).
2. Radius Search: For each point in the incoming scan, a radius search is performed using the KD-tree to determine if the point exists in the map.
3. Dynamic Point Identification: Points that do not have enough neighbors within the search radius (as defined by min_neighbors_) are considered dynamic and are stored in a separate point cloud.
4. Publishing: The dynamic points are then published to a dedicated topic for further processing or visualization.

6. People Tracking

After extracting clusters from the point cloud, StoreTracker uses the PeopleTracker class to track these clusters over time. The tracker updates the position of each detected person and prunes old tracks that have not been updated recently.

Tracking Logic:

1. Detection Update: Each new set of clusters is compared with existing tracks.
2. Track Association: New clusters are matched with existing tracks based on proximity.
3. Pruning: Tracks that have not been updated within a specified time (time_threshold) are removed.
4. Data Posting: If enabled, the positions of detected people are sent to a remote server as JSON data.

7. Data Publishing and Posting

StoreTracker publishes the processed point cloud data and detected clusters to various topics. It can also post the detection data to a remote server for further processing or logging.

Published Topics:

• /map/pointcloud: The final processed point cloud map.
• /dynamic/pointcloud: Points identified as dynamic (moving) objects.
• /voxel/pointcloud: The voxel grid-filtered point cloud.
• /pass/pointcloud: The pass-through-filtered point cloud.
• /plane/pointcloud: The point cloud after plane extraction.
• /detections: Markers representing detected human clusters.

Data Posting:

• If post_data_enabled_ is set to True, the node sends the detection data (including cluster positions) to a specified endpoint via HTTP POST requests.

8. Timers and Periodic Operations

StoreTracker uses ROS 2 timers to perform periodic operations such as updating the map or recalculating height from LiDAR data. These operations ensure that the node remains responsive and up-to-date with the latest sensor readings.

Periodic Operations:

• Map Update: Periodically merges incoming scans into the map.
• Height Calculation: Computes the average height of the LiDAR sensor based on plane extraction results.

Configuration Parameters

You can configure StoreTracker through a YAML file or via command-line arguments. The key parameters are listed below:

store_tracker:
  ros__parameters:
    frame_id: 'livox_frame'
    identifier: 'lidar-1'
    post_data_enabled: False  # Enables or disables HTTP data posting
    endpoint: "https://us.wavespot.net/onebox/api/v1/radar_data.json"
    width: 9.3  # Room width in meters
    height: 5.5  # Room height in meters
    map_path: "/home/user/ros2_ws/install/share/store_tracker/data/"
    calibration_file: "/home/user/ros2_ws/install/share/store_tracker/data/calibration.json"
    calibration: True
    save_pcd: False  # Whether to save point clouds to disk
    publish_detections: True  # Whether to publish detection markers
    voxel_filter:
      scan:
       

 leaf_size: 0.1  # Leaf size for the scan voxel grid filter
      map:
        leaf_size: 0.1  # Leaf size for the map voxel grid filter
      publish: True  # Publish voxel-filtered cloud
    pass_filter:
      filter_x: True  # Enable X-axis filtering
      filter_y: True  # Enable Y-axis filtering
      filter_z: True  # Enable Z-axis filtering
      z_min: -2.8  # Minimum Z value for pass-through filter
      z_max: 2.0  # Maximum Z value for pass-through filter
      publish: True  # Publish pass-through-filtered cloud
    plane_filter:
      max_iterations: 100  # RANSAC maximum iterations
      distance_threshold: 0.01  # RANSAC distance threshold
      removal_threshold: 0.5  # Plane removal threshold
      optimize_coefficients: True  # Optimize coefficients in plane extraction
      publish: True  # Publish plane-extracted cloud
    cluster_extraction:
      tolerance: 0.25  # Cluster extraction tolerance
      min_cluster_size: 5  # Minimum cluster size
      max_cluster_size: 100  # Maximum cluster size
    search.radius: 0.3  # Radius for neighbor search
    search.min_neighbors: 4  # Minimum neighbors for a point to be considered
    tracker.distance_threshold: 1.0  # Maximum distance for track association
    tracker.time_threshold: 5.0  # Maximum time before a track is pruned

Running the Node

Running with Recorded Data (Bag File)

To run StoreTracker with a recorded ROS 2 bag file, you can use the following launch command:

ros2 launch store_tracker storetracker.launch.py

This command launches the StoreTracker node configured to process point cloud data from a recorded bag file. It uses the configuration specified in the storetracker.yaml file.

Running with Real Sensors (Live Launch)

To run StoreTracker with live sensors, use the following command:

ros2 launch store_tracker storetracker_pronto2.launch.py

This command launches StoreTracker to process real-time point cloud data from LiDAR and IMU sensors. The node is configured to work with both indoor and outdoor LiDARs, as specified in the storetracker_pronto2.launch.py file.

Example Usage with Specific Configuration

To run StoreTracker with a custom configuration file, use:

ros2 launch store_tracker storetracker.launch.py storetracker_config_dir:=/path/to/your/config.yaml

This allows you to specify a different configuration file for your specific use case.

Additional Details

People Tracking

StoreTracker integrates the PeopleTracker class to provide real-time tracking of detected individuals. The tracker maintains a list of active tracks, updates them with new detections, and prunes tracks that haven't been updated for a specified time. The tracker ensures that the node can keep up with moving individuals and maintain accurate positions over time.

Data Posting

If enabled, StoreTracker can post detection data to a remote server. The data includes the positions of detected people and is sent as a JSON object. This feature is useful for integrating StoreTracker with larger systems that monitor or analyze human activity.

Filters and Custom Processing

StoreTracker is highly configurable and allows for detailed control over how the point cloud is processed. Users can adjust the parameters for each filter to fine-tune the node's performance for different environments or sensor setups.

Publishers and Subscribers

• Publishers:

  • /map/pointcloud: Publishes the final processed point cloud map.
  • /dynamic/pointcloud: Publishes dynamic points in the cloud, i.e., points that represent moving objects.
  • /voxel/pointcloud: Publishes the point cloud after voxel grid filtering.
  • /pass/pointcloud: Publishes the point cloud after pass-through filtering.
  • /plane/pointcloud: Publishes the point cloud after plane extraction.
  • /detections: Publishes markers for detected human clusters.

• Subscribers:

  • /livox/lidar: Subscribes to point cloud data from a LiDAR sensor.
  • /livox/imu: Subscribes to IMU data for real-time orientation correction.

Conclusion

StoreTracker is a powerful ROS 2 node for real-time people detection and tracking using point cloud data. Its robust filtering and clustering algorithms, coupled with its flexibility in configuration, make it an ideal solution for environments that require continuous monitoring and analysis of human activity. Whether using recorded data or live sensors, StoreTracker provides accurate and efficient detection and tracking capabilities.