601 lines
23 KiB
Rust
601 lines
23 KiB
Rust
//! Autonomous vehicle perception benchmarks
|
|
//!
|
|
//! Performance benchmarks for:
|
|
//! - LiDAR point cloud processing
|
|
//! - Multi-sensor fusion
|
|
//! - 3D object detection and tracking
|
|
//! - Real-time perception pipelines
|
|
//! - Safety-critical validation
|
|
|
|
use criterion::{BenchmarkId, Criterion, Throughput, criterion_group, criterion_main};
|
|
use rtx_tensor::{DType, Device, Tensor};
|
|
use rtx_vision_advanced::*;
|
|
|
|
/// Autonomous vehicle benchmark helper
|
|
struct AutonomousBenchHelper;
|
|
|
|
impl AutonomousBenchHelper {
|
|
fn create_point_cloud(num_points: usize) -> Tensor {
|
|
// Create realistic LiDAR point cloud with x, y, z, intensity
|
|
let mut points = Tensor::randn(&[num_points, 4], &Device::default())
|
|
.expect("Failed to create point cloud");
|
|
|
|
// Scale to realistic LiDAR ranges (±50m x, ±50m y, ±3m z)
|
|
let scale = Tensor::new(&[[50.0, 50.0, 3.0, 255.0]], DType::F32, &Device::default())
|
|
.expect("Failed to create scale tensor");
|
|
points = points
|
|
.broadcast_mul(&scale)
|
|
.expect("Failed to scale points");
|
|
|
|
// Add realistic intensity values (0-255)
|
|
let intensity_col = points.narrow(1, 3, 1).expect("Failed to get intensity");
|
|
let scaled_intensity = intensity_col.abs().expect("Failed to abs intensity");
|
|
|
|
points
|
|
.slice_assign(&[None, Some((3, 4))], &scaled_intensity)
|
|
.expect("Failed to assign intensity");
|
|
|
|
points
|
|
}
|
|
|
|
fn create_camera_image(height: usize, width: usize) -> Tensor {
|
|
Tensor::randn(&[1, 3, height, width], &Device::default())
|
|
.expect("Failed to create camera image")
|
|
}
|
|
|
|
fn create_radar_data(num_targets: usize) -> Tensor {
|
|
// Radar data: range, azimuth, elevation, velocity, RCS
|
|
Tensor::randn(&[num_targets, 5], &Device::default()).expect("Failed to create radar data")
|
|
}
|
|
}
|
|
|
|
/// Benchmark LiDAR point cloud processing
|
|
fn bench_lidar_processing(c: &mut Criterion) {
|
|
let mut group = c.benchmark_group("lidar_processing");
|
|
|
|
// Different point cloud sizes (typical for various LiDAR sensors)
|
|
let point_cloud_sizes = vec![
|
|
("velodyne_16", 20000), // Velodyne VLP-16
|
|
("velodyne_32", 40000), // Velodyne VLP-32
|
|
("velodyne_64", 80000), // Velodyne HDL-64E
|
|
("ouster_128", 160000), // Ouster OS1-128
|
|
("solid_state", 300000), // High-resolution solid-state LiDAR
|
|
];
|
|
|
|
for (lidar_name, num_points) in point_cloud_sizes {
|
|
let point_cloud = AutonomousBenchHelper::create_point_cloud(num_points);
|
|
let config = autonomous::LidarConfig::default();
|
|
let mut processor =
|
|
autonomous::LidarProcessor::new(config).expect("Failed to create LiDAR processor");
|
|
|
|
group.throughput(Throughput::Elements(num_points as u64));
|
|
group.bench_with_input(
|
|
BenchmarkId::new("lidar_processing", lidar_name),
|
|
&point_cloud,
|
|
|b, pc| {
|
|
b.iter(|| {
|
|
processor
|
|
.process_point_cloud(pc)
|
|
.expect("LiDAR processing failed")
|
|
});
|
|
},
|
|
);
|
|
}
|
|
|
|
group.finish();
|
|
}
|
|
|
|
/// Benchmark point cloud filtering operations
|
|
fn bench_point_cloud_filtering(c: &mut Criterion) {
|
|
let mut group = c.benchmark_group("point_cloud_filtering");
|
|
|
|
let point_cloud = AutonomousBenchHelper::create_point_cloud(100000);
|
|
|
|
// Different filtering operations
|
|
let filtering_ops = vec![
|
|
("ground_removal", "statistical_outlier_removal"),
|
|
("statistical_outlier", "ground_plane_removal"),
|
|
("roi_filtering", "region_of_interest"),
|
|
("intensity_filter", "intensity_threshold"),
|
|
("range_filter", "distance_threshold"),
|
|
];
|
|
|
|
for (filter_name, filter_type) in filtering_ops {
|
|
group.bench_with_input(
|
|
BenchmarkId::new("filtering", filter_name),
|
|
&point_cloud,
|
|
|b, pc| {
|
|
b.iter(|| {
|
|
match filter_type {
|
|
"statistical_outlier_removal" => {
|
|
// Remove statistical outliers based on neighbor distance
|
|
// In practice, this would use k-d tree for nearest neighbors
|
|
let filtered_indices = pc
|
|
.narrow(1, 2, 1)
|
|
.expect("Z coordinate")
|
|
.gt_scalar(-2.0)
|
|
.expect("Height filter")
|
|
.logical_and(
|
|
&pc.narrow(1, 2, 1)
|
|
.expect("Z coordinate")
|
|
.lt_scalar(3.0)
|
|
.expect("Height filter"),
|
|
)
|
|
.expect("Logical and failed");
|
|
filtered_indices
|
|
}
|
|
"ground_plane_removal" => {
|
|
// RANSAC-based ground plane removal
|
|
let z_coords = pc.narrow(1, 2, 1).expect("Z coordinates");
|
|
let non_ground_mask =
|
|
z_coords.gt_scalar(-1.5).expect("Ground threshold");
|
|
non_ground_mask
|
|
}
|
|
"region_of_interest" => {
|
|
// Filter points within ROI (e.g., ±25m x, ±25m y)
|
|
let x_coords = pc.narrow(1, 0, 1).expect("X coordinates");
|
|
let y_coords = pc.narrow(1, 1, 1).expect("Y coordinates");
|
|
|
|
let x_mask = x_coords
|
|
.abs()
|
|
.expect("X abs")
|
|
.lt_scalar(25.0)
|
|
.expect("X ROI");
|
|
let y_mask = y_coords
|
|
.abs()
|
|
.expect("Y abs")
|
|
.lt_scalar(25.0)
|
|
.expect("Y ROI");
|
|
x_mask.logical_and(&y_mask).expect("ROI mask")
|
|
}
|
|
"intensity_threshold" => {
|
|
// Filter by intensity values
|
|
let intensity = pc.narrow(1, 3, 1).expect("Intensity");
|
|
intensity.gt_scalar(50.0).expect("Intensity threshold")
|
|
}
|
|
"distance_threshold" => {
|
|
// Filter by distance from sensor
|
|
let x = pc.narrow(1, 0, 1).expect("X");
|
|
let y = pc.narrow(1, 1, 1).expect("Y");
|
|
let z = pc.narrow(1, 2, 1).expect("Z");
|
|
|
|
let dist_sq = x
|
|
.pow_scalar(2.0)
|
|
.expect("X squared")
|
|
.add(&y.pow_scalar(2.0).expect("Y squared"))
|
|
.expect("X+Y squared")
|
|
.add(&z.pow_scalar(2.0).expect("Z squared"))
|
|
.expect("Distance squared");
|
|
|
|
dist_sq.lt_scalar(50.0 * 50.0).expect("Distance filter") // 50m max range
|
|
}
|
|
_ => pc
|
|
.narrow(1, 0, 1)
|
|
.expect("Default filter")
|
|
.gt_scalar(-1000.0)
|
|
.expect("Default mask"),
|
|
}
|
|
});
|
|
},
|
|
);
|
|
}
|
|
|
|
group.finish();
|
|
}
|
|
|
|
/// Benchmark 3D object detection in point clouds
|
|
fn bench_3d_object_detection(c: &mut Criterion) {
|
|
let mut group = c.benchmark_group("3d_object_detection");
|
|
|
|
let point_cloud_configs = vec![
|
|
("urban_scene", 100000),
|
|
("highway_scene", 150000),
|
|
("parking_lot", 80000),
|
|
("intersection", 120000),
|
|
];
|
|
|
|
for (scene_name, num_points) in point_cloud_configs {
|
|
let point_cloud = AutonomousBenchHelper::create_point_cloud(num_points);
|
|
let mut detector =
|
|
detection::three_d::PointRCNN::new().expect("Failed to create 3D detector");
|
|
|
|
group.throughput(Throughput::Elements(num_points as u64));
|
|
group.bench_with_input(
|
|
BenchmarkId::new("3d_detection", scene_name),
|
|
&point_cloud,
|
|
|b, pc| {
|
|
b.iter(|| detector.detect_3d(pc).expect("3D detection failed"));
|
|
},
|
|
);
|
|
}
|
|
|
|
group.finish();
|
|
}
|
|
|
|
/// Benchmark sensor fusion
|
|
fn bench_sensor_fusion(c: &mut Criterion) {
|
|
let mut group = c.benchmark_group("sensor_fusion");
|
|
|
|
// Different sensor fusion scenarios
|
|
let fusion_scenarios = vec![
|
|
("camera_lidar", true, true, false, false, false),
|
|
("camera_lidar_radar", true, true, true, false, false),
|
|
("full_sensor_suite", true, true, true, true, true),
|
|
("lidar_only", false, true, false, false, false),
|
|
("camera_radar", true, false, true, false, false),
|
|
];
|
|
|
|
for (scenario_name, use_camera, use_lidar, use_radar, use_imu, use_gnss) in fusion_scenarios {
|
|
let camera_image = if use_camera {
|
|
Some(AutonomousBenchHelper::create_camera_image(720, 1280))
|
|
} else {
|
|
None
|
|
};
|
|
|
|
let lidar_points = if use_lidar {
|
|
Some(AutonomousBenchHelper::create_point_cloud(50000))
|
|
} else {
|
|
None
|
|
};
|
|
|
|
let radar_data = if use_radar {
|
|
Some(AutonomousBenchHelper::create_radar_data(20))
|
|
} else {
|
|
None
|
|
};
|
|
|
|
let sensor_data = autonomous::MultiSensorData {
|
|
camera_image,
|
|
lidar_points,
|
|
radar_data,
|
|
imu_data: if use_imu {
|
|
Some([0.1, 0.2, 9.8, 0.01, 0.02, 0.03])
|
|
} else {
|
|
None
|
|
},
|
|
gnss_data: if use_gnss {
|
|
Some([37.7749, -122.4194, 10.0, 1.0])
|
|
} else {
|
|
None
|
|
},
|
|
timestamp: 1234567890.0,
|
|
};
|
|
|
|
let fusion_config = autonomous::FusionConfig::default();
|
|
let mut fusion_processor = autonomous::SensorFusionProcessor::new(fusion_config)
|
|
.expect("Failed to create fusion processor");
|
|
|
|
group.bench_with_input(
|
|
BenchmarkId::new("sensor_fusion", scenario_name),
|
|
&sensor_data,
|
|
|b, data| {
|
|
b.iter(|| {
|
|
fusion_processor
|
|
.fuse_sensors(data)
|
|
.expect("Sensor fusion failed")
|
|
});
|
|
},
|
|
);
|
|
}
|
|
|
|
group.finish();
|
|
}
|
|
|
|
/// Benchmark multi-object tracking in 3D
|
|
fn bench_3d_multi_object_tracking(c: &mut Criterion) {
|
|
let mut group = c.benchmark_group("3d_multi_object_tracking");
|
|
|
|
let mut tracker = autonomous::MultiObjectTracker::new().expect("Failed to create tracker");
|
|
|
|
// Different tracking scenarios
|
|
let tracking_scenarios = vec![
|
|
("sparse_traffic", 3),
|
|
("normal_traffic", 8),
|
|
("dense_traffic", 15),
|
|
("traffic_jam", 25),
|
|
("intersection", 20),
|
|
];
|
|
|
|
for (scenario_name, num_objects) in tracking_scenarios {
|
|
// Create synthetic 3D detections
|
|
let mut detections_3d = Vec::new();
|
|
for i in 0..num_objects {
|
|
detections_3d.push(detection::three_d::BoundingBox3D {
|
|
center: [
|
|
(i as f32 - num_objects as f32 / 2.0) * 5.0, // Spread along X
|
|
10.0 + (i % 3) as f32 * 3.0, // Varying Y distance
|
|
0.8, // Ground level
|
|
],
|
|
dimensions: [4.5, 2.0, 1.8], // Typical car dimensions
|
|
rotation: 0.0,
|
|
confidence: 0.8 + (i % 3) as f32 * 0.05,
|
|
class_id: i % 3, // Mix of different object types
|
|
});
|
|
}
|
|
|
|
group.throughput(Throughput::Elements(num_objects as u64));
|
|
group.bench_with_input(
|
|
BenchmarkId::new("3d_tracking", scenario_name),
|
|
&detections_3d,
|
|
|b, detections| {
|
|
b.iter(|| tracker.update(detections, 0.0).expect("3D tracking failed"));
|
|
},
|
|
);
|
|
}
|
|
|
|
group.finish();
|
|
}
|
|
|
|
/// Benchmark real-time perception pipeline
|
|
fn bench_realtime_perception_pipeline(c: &mut Criterion) {
|
|
let mut group = c.benchmark_group("realtime_perception_pipeline");
|
|
|
|
// Different real-time scenarios with frame rates
|
|
let realtime_scenarios = vec![
|
|
("autonomous_highway", 30), // 30 Hz highway driving
|
|
("urban_navigation", 20), // 20 Hz urban driving
|
|
("parking_assistance", 10), // 10 Hz parking
|
|
("emergency_braking", 100), // 100 Hz emergency systems
|
|
];
|
|
|
|
for (scenario_name, target_fps) in realtime_scenarios {
|
|
let camera_image = AutonomousBenchHelper::create_camera_image(720, 1280);
|
|
let point_cloud = AutonomousBenchHelper::create_point_cloud(60000);
|
|
let radar_data = AutonomousBenchHelper::create_radar_data(15);
|
|
|
|
let sensor_data = autonomous::MultiSensorData {
|
|
camera_image: Some(camera_image),
|
|
lidar_points: Some(point_cloud),
|
|
radar_data: Some(radar_data),
|
|
imu_data: Some([0.1, 0.2, 9.8, 0.01, 0.02, 0.03]),
|
|
gnss_data: Some([37.7749, -122.4194, 10.0, 1.0]),
|
|
timestamp: 1234567890.0,
|
|
};
|
|
|
|
// Create perception pipeline components
|
|
let mut detector_2d = detection::YOLOv8::new(detection::YOLOConfig {
|
|
model_size: detection::YOLOSize::Small, // Optimized for speed
|
|
..Default::default()
|
|
})
|
|
.expect("Failed to create 2D detector");
|
|
|
|
let mut detector_3d =
|
|
detection::three_d::PointRCNN::new().expect("Failed to create 3D detector");
|
|
|
|
let fusion_config = autonomous::FusionConfig::default();
|
|
let mut fusion_processor = autonomous::SensorFusionProcessor::new(fusion_config)
|
|
.expect("Failed to create fusion processor");
|
|
|
|
let mut tracker = autonomous::MultiObjectTracker::new().expect("Failed to create tracker");
|
|
|
|
group.throughput(Throughput::Elements(target_fps as u64));
|
|
group.bench_with_input(
|
|
BenchmarkId::new("realtime_pipeline", scenario_name),
|
|
&sensor_data,
|
|
|b, data| {
|
|
b.iter(|| {
|
|
// Complete perception pipeline
|
|
|
|
// 1. 2D object detection on camera
|
|
let detections_2d = if let Some(ref camera) = data.camera_image {
|
|
detector_2d.detect(camera).expect("2D detection failed")
|
|
} else {
|
|
Vec::new()
|
|
};
|
|
|
|
// 2. 3D object detection on LiDAR
|
|
let detections_3d = if let Some(ref lidar) = data.lidar_points {
|
|
detector_3d.detect_3d(lidar).expect("3D detection failed")
|
|
} else {
|
|
Vec::new()
|
|
};
|
|
|
|
// 3. Sensor fusion
|
|
let fusion_result = fusion_processor
|
|
.fuse_sensors(data)
|
|
.expect("Sensor fusion failed");
|
|
|
|
// 4. Multi-object tracking
|
|
let tracked_objects = tracker
|
|
.update(&detections_3d, data.timestamp)
|
|
.expect("Tracking failed");
|
|
|
|
// 5. Safety assessment
|
|
let safety_assessor = autonomous::SafetyAssessment::new();
|
|
let risk_assessment = safety_assessor
|
|
.assess_collision_risk(&tracked_objects)
|
|
.expect("Safety assessment failed");
|
|
|
|
// Return some result to prevent optimization
|
|
(
|
|
detections_2d.len(),
|
|
detections_3d.len(),
|
|
risk_assessment.overall_risk,
|
|
)
|
|
});
|
|
},
|
|
);
|
|
}
|
|
|
|
group.finish();
|
|
}
|
|
|
|
/// Benchmark path planning and prediction
|
|
fn bench_path_planning(c: &mut Criterion) {
|
|
let mut group = c.benchmark_group("path_planning");
|
|
|
|
// Create tracked objects for path planning scenarios
|
|
let planning_scenarios = vec![
|
|
("highway_merge", 5),
|
|
("intersection_turn", 8),
|
|
("lane_change", 3),
|
|
("parking_maneuver", 6),
|
|
];
|
|
|
|
for (scenario_name, num_objects) in planning_scenarios {
|
|
let mut tracked_objects = Vec::new();
|
|
|
|
for i in 0..num_objects {
|
|
tracked_objects.push(autonomous::TrackedObject {
|
|
track_id: i,
|
|
bbox_3d: detection::three_d::BoundingBox3D {
|
|
center: [i as f32 * 8.0, 15.0, 0.8],
|
|
dimensions: [4.5, 2.0, 1.8],
|
|
rotation: 0.0,
|
|
confidence: 0.9,
|
|
class_id: 0,
|
|
},
|
|
velocity: [-5.0 + i as f32 * 2.0, 0.0, 0.0],
|
|
track_confidence: 0.9,
|
|
object_class: autonomous::ObjectClass::Vehicle,
|
|
age: 10,
|
|
hits: 10,
|
|
time_since_update: 0.1,
|
|
predicted_trajectory: Vec::new(),
|
|
});
|
|
}
|
|
|
|
group.bench_with_input(
|
|
BenchmarkId::new("path_planning", scenario_name),
|
|
&tracked_objects,
|
|
|b, objects| {
|
|
b.iter(|| {
|
|
// Simulate path planning algorithm
|
|
for obj in objects {
|
|
// 1. Trajectory prediction
|
|
let mut predicted_trajectory = Vec::new();
|
|
for t in 1..6 {
|
|
// 5 future time steps
|
|
let dt = t as f32 * 0.2; // 0.2 second intervals
|
|
predicted_trajectory.push(autonomous::TrajectoryPoint {
|
|
position: [
|
|
obj.bbox_3d.center[0] + obj.velocity[0] * dt,
|
|
obj.bbox_3d.center[1] + obj.velocity[1] * dt,
|
|
obj.bbox_3d.center[2] + obj.velocity[2] * dt,
|
|
],
|
|
time_offset: dt,
|
|
covariance: [[0.1, 0.0, 0.0], [0.0, 0.1, 0.0], [0.0, 0.0, 0.1]],
|
|
});
|
|
}
|
|
|
|
// 2. Collision risk assessment
|
|
let risk_score = predicted_trajectory
|
|
.iter()
|
|
.map(|point| {
|
|
let distance =
|
|
(point.position[0].powi(2) + point.position[1].powi(2)).sqrt();
|
|
if distance < 5.0 { 1.0 } else { 0.1 / distance }
|
|
})
|
|
.fold(0.0f32, f32::max);
|
|
|
|
// 3. Path optimization (simplified)
|
|
let _ = risk_score < 0.5; // Safe path flag
|
|
}
|
|
});
|
|
},
|
|
);
|
|
}
|
|
|
|
group.finish();
|
|
}
|
|
|
|
/// Benchmark safety-critical validation
|
|
fn bench_safety_validation(c: &mut Criterion) {
|
|
let mut group = c.benchmark_group("safety_validation");
|
|
|
|
// Safety-critical scenarios
|
|
let safety_scenarios = vec![
|
|
("emergency_braking", 1),
|
|
("collision_imminent", 2),
|
|
("pedestrian_crossing", 3),
|
|
("obstacle_avoidance", 5),
|
|
("multi_vehicle_interaction", 8),
|
|
];
|
|
|
|
for (scenario_name, num_critical_objects) in safety_scenarios {
|
|
let mut tracked_objects = Vec::new();
|
|
|
|
// Create critical objects (close, fast-moving)
|
|
for i in 0..num_critical_objects {
|
|
tracked_objects.push(autonomous::TrackedObject {
|
|
track_id: i,
|
|
bbox_3d: detection::three_d::BoundingBox3D {
|
|
center: [5.0 + i as f32 * 2.0, 2.0, 0.8], // Close objects
|
|
dimensions: if i == 0 {
|
|
[0.6, 0.6, 1.7]
|
|
} else {
|
|
[4.5, 2.0, 1.8]
|
|
}, // Mix pedestrian/vehicle
|
|
rotation: 0.0,
|
|
confidence: 0.95,
|
|
class_id: if i == 0 { 1 } else { 0 }, // Pedestrian vs vehicle
|
|
},
|
|
velocity: [-10.0, -2.0, 0.0], // Fast approaching
|
|
track_confidence: 0.95,
|
|
object_class: if i == 0 {
|
|
autonomous::ObjectClass::Pedestrian
|
|
} else {
|
|
autonomous::ObjectClass::Vehicle
|
|
},
|
|
age: 5,
|
|
hits: 5,
|
|
time_since_update: 0.05,
|
|
predicted_trajectory: Vec::new(),
|
|
});
|
|
}
|
|
|
|
let safety_assessor = autonomous::SafetyAssessment::new();
|
|
|
|
group.throughput(Throughput::Elements(num_critical_objects as u64));
|
|
group.bench_with_input(
|
|
BenchmarkId::new("safety_validation", scenario_name),
|
|
&tracked_objects,
|
|
|b, objects| {
|
|
b.iter(|| {
|
|
// Safety-critical assessment
|
|
let risk_assessment = safety_assessor
|
|
.assess_collision_risk(objects)
|
|
.expect("Safety assessment failed");
|
|
|
|
// Critical decision making
|
|
let requires_emergency_action = risk_assessment.overall_risk > 0.8;
|
|
let critical_objects = &risk_assessment.critical_objects;
|
|
|
|
// Time to collision calculation for each object
|
|
for obj in objects {
|
|
let relative_velocity =
|
|
(obj.velocity[0].powi(2) + obj.velocity[1].powi(2)).sqrt();
|
|
let distance =
|
|
(obj.bbox_3d.center[0].powi(2) + obj.bbox_3d.center[1].powi(2)).sqrt();
|
|
let ttc = if relative_velocity > 0.1 {
|
|
distance / relative_velocity
|
|
} else {
|
|
f32::INFINITY
|
|
};
|
|
|
|
let _ = ttc < 2.0; // Critical TTC threshold
|
|
}
|
|
|
|
(requires_emergency_action, critical_objects.len())
|
|
});
|
|
},
|
|
);
|
|
}
|
|
|
|
group.finish();
|
|
}
|
|
|
|
criterion_group!(
|
|
benches,
|
|
bench_lidar_processing,
|
|
bench_point_cloud_filtering,
|
|
bench_3d_object_detection,
|
|
bench_sensor_fusion,
|
|
bench_3d_multi_object_tracking,
|
|
bench_realtime_perception_pipeline,
|
|
bench_path_planning,
|
|
bench_safety_validation
|
|
);
|
|
|
|
criterion_main!(benches);
|