Skip to content

autoware_calibration_status_classifier#

Purpose#

The autoware_calibration_status_classifier package provides real-time LiDAR-camera calibration validation using deep learning inference. It detects miscalibration between LiDAR and camera sensors by analyzing projected point clouds overlaid on camera images through a neural network-based approach.

Inner-workings / Algorithms#

The calibration status detection system operates through the following pipeline:

1. Data Preprocessing (CUDA-accelerated)#

  • Image Undistortion: Corrects camera distortion
  • Point Cloud Projection: Projects 3D LiDAR points onto undistorted 2D image plane - adds depth and intensity information
  • Morphological Dilation: Enhances point visibility for neural network input

2. Neural Network Inference (TensorRT)#

  • Input Format: 5-channel normalized data (RGB + depth + intensity)
  • Architecture: Deep neural network trained on calibrated/miscalibrated data
  • Output: Binary classification with confidence scores for calibration status

3. Runtime Modes#

  • MANUAL: On-demand validation via service calls
  • PERIODIC: Regular validation at configurable intervals
  • ACTIVE: Continuous monitoring with synchronized sensor data

Inputs / Outputs#

Input#

Name Type Description
~/input/velocity prerequisite.velocity_source parameter Vehicle velocity (multiple message types supported)
input.cloud_topics sensor_msgs::msg::PointCloud2 LiDAR point cloud data
input.image_topics sensor_msgs::msg::Image Camera image data (BGR8 format)
Camera info topics sensor_msgs::msg::CameraInfo Camera intrinsic parameters and distortion coefficients

Output#

Name Type Description
/diagnostics diagnostic_msgs::msg::DiagnosticArray ROS diagnostics with calibration status
~/validate_calibration_srv std_srvs::srv::Trigger Manual validation service (MANUAL mode)
Preview image topics sensor_msgs::msg::Image Visualization images with projected points

Services#

Name Type Description
~/input/validate_calibration_srv std_srvs::srv::Trigger Manual calibration validation request

Parameters#

Node Parameters#

Name Type Description Default Range
trt_precision string A precision of TensorRT engine. fp16 ['fp16', 'fp32']
cloud_capacity integer Capacity of the point cloud buffer (should be set to at least the maximum theoretical number of points). 2000000 ≥1
onnx_path string Path to the ONNX model file used for calibration status verification. \((var model_path)/\)(var model_name).onnx N/A
runtime_mode string The mode of calibration status node, which determines when the calibration status verification is performed. 'manual' triggers by service, 'periodic' runs at a fixed interval, and 'active' runs continuously. active ['manual', 'periodic', 'active']
period float The period duration in seconds. For manual mode it determines the timeout duration for the service request. For periodic mode it determines how often the calibration status is verified. 5.0 N/A
queue_size integer The queue size in synchronized callbacks. 5 N/A
input.cloud_topics array List of LiDAR topics to subscribe for calibration status verification. If only a single LiDAR topic is specified with multiple camera topics, all the camera topics will be validated against that single LiDAR topic. If multiple LiDAR topics are specified with multiple camera topics, each camera topic will be validated against the corresponding LiDAR topic in the list (both lists size must match). If multiple LiDAR topics are specified with a single camera topic, the single camera topic will be validated against all the LiDAR topics in the list. ['/sensing/lidar/concatenated/pointcloud'] N/A
input.image_topics array List of camera topics to subscribe for calibration status verification. Same rules as for cloud_topics apply here. ['/sensing/camera/camera8/image_raw'] N/A
input.approx_deltas array The approximate time deltas in seconds for each topic pair (LiDAR and camera) for Approximate Time Synchronization Policy. The length of this array must be equal to maximum length of cloud_topics and image_topics arrays or be a single value which will be applied to all topic pairs. ['0.1'] N/A
input.already_rectified array List of flags indicating whether each camera topic provides already rectified images. The length of this array must be equal to maximum length of image_topics array or be a single value which will be applied to all camera topics. [True] N/A
prerequisite.check_velocity boolean Flag to enable or disable velocity check for calibration status verification. If enabled, the node will subscribe to the specified velocity source topics and verify that the vehicle is moving at maximum velocity defined by velocity_threshold. True N/A
prerequisite.velocity_source string The msg type to subscribe for velocity check. Available options are 'twist', 'twist_stamped', 'twist_with_cov', 'twist_with_cov_stamped', 'odometry'. twist_with_cov_stamped ['twist', 'twist_with_cov', 'twist_stamped', 'twist_with_cov_stamped', 'odometry']
prerequisite.velocity_threshold float The maximum velocity threshold in meters per second for the vehicle to be considered moving. If the vehicle's velocity is below this threshold, the calibration status verification will not be performed. 5.0 N/A
prerequisite.check_objects boolean Flag to enable or disable object check for calibration status verification. If enabled, the node will subscribe to the specified object source topics and verify that the number of objects is within the defined limit. True N/A
prerequisite.objects_limit integer The maximum number of objects allowed for the vehicle to be considered in a valid state for calibration status verification. If the number of detected objects exceeds this limit, the calibration status verification will not be performed. 100 ≥0
miscalibration_confidence_threshold float The confidence threshold for determining miscalibration. Positive values shifts the decision boundary towards detecting miscalibration, while negative values shifts it towards detecting proper calibration. A value of 0.0 means that the decision is based solely on the model's output without any bias. 0.0 N/A

Network Parameters#

Name Type Description Default Range
max_depth float Maximum depth for projected LiDAR points in the camera frame. Points with depth values greater than this are considered invalid. 128.0 N/A
dilation_size integer The size of the dilation kernel used for point cloud processing, 1 means 3x3 kernel, 2 means 5x5 kernel, etc. 1 N/A
height array The height of the input image for optimization profile of the ML model, specified as [min, opt, max]. [1080, 1860, 2160] N/A
width array The width of the input image for the optimization profile of the ML model, specified as [min, opt, max]. [1920, 2880, 3840] N/A

Assumptions / Known Limits#

  • Input images must be in BGR8 format (8-bit per channel)
  • Input point clouds should contain intensity information (XYZIRC format)

Usage Example#

ros2 launch autoware_calibration_status_classifier calibration_status_classifier.launch.xml

Future Extensions / Unimplemented Parts#

  • Manual runtime mode with detailed response (custom srv)
  • Replace filter for objects on the scene counter to objects within the camera FOV counter (raytracing)
  • Multithreading for multiple camera-LiDAR pairs
  • More filters (e.g. yaw rate)
  • cuda_blackboard support
  • Replace custom kernels with NPP functions where applicable

References#