Initial commit

crates/models/rtx-vision-advanced/src/autonomous/mod.rs

@@ -0,0 +1,816 @@
+//! Autonomous vehicle perception module
+//!
+//! Provides:
+//! - LiDAR point cloud processing and segmentation
+//! - Sensor fusion (camera, LiDAR, radar integration)
+//! - Path planning integration with perception
+//! - Object tracking and trajectory prediction
+//! - Safety-critical validation and verification
+
+// Modules to be implemented
+// pub mod lidar;
+// pub mod fusion;
+// pub mod tracking;
+// pub mod planning;
+// pub mod safety;
+// pub mod calibration;
+
+// Placeholder structs for unimplemented modules
+/// Path plan result
+#[derive(Debug, Clone)]
+pub struct PathPlan {
+    pub waypoints: Vec<[f32; 3]>,
+    pub trajectory_cost: f32,
+}
+
+/// Placeholder PathPlanner
+#[derive(Debug, Clone)]
+pub struct PathPlanner {
+    // Placeholder implementation
+}
+
+impl PathPlanner {
+    /// Plan a path based on tracked objects and sensor frame
+    pub fn plan(
+        &mut self,
+        _objects: &[TrackedObject],
+        _frame: &SensorFrame,
+    ) -> VisionResult<PathPlan> {
+        // Placeholder implementation
+        Ok(PathPlan {
+            waypoints: vec![],
+            trajectory_cost: 0.0,
+        })
+    }
+}
+
+use crate::detection::three_d::{BoundingBox3D, Detection3DResult, PointCloud};
+use crate::{DetectionResult, VisionError, VisionResult};
+use rtx_tensor::Tensor;
+use std::collections::HashMap;
+use tracing::debug;
+
+/// Autonomous driving sensor types
+#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
+pub enum SensorType {
+    /// Light Detection and Ranging
+    LiDAR,
+    /// Camera (RGB)
+    Camera,
+    /// Radar
+    Radar,
+    /// Inertial Measurement Unit
+    IMU,
+    /// Global Navigation Satellite System
+    GNSS,
+    /// Wheel odometry
+    Odometry,
+}
+
+/// Sensor data timestamp
+#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)]
+pub struct Timestamp {
+    pub seconds: u64,
+    pub nanoseconds: u32,
+}
+
+impl Timestamp {
+    pub fn now() -> Self {
+        let duration = std::time::SystemTime::now()
+            .duration_since(std::time::UNIX_EPOCH)
+            .unwrap();
+
+        Self {
+            seconds: duration.as_secs(),
+            nanoseconds: duration.subsec_nanos(),
+        }
+    }
+
+    pub fn to_seconds(&self) -> f64 {
+        self.seconds as f64 + self.nanoseconds as f64 * 1e-9
+    }
+
+    pub fn duration_since(&self, other: &Self) -> f64 {
+        self.to_seconds() - other.to_seconds()
+    }
+}
+
+/// Multi-sensor data frame
+#[derive(Debug, Clone)]
+pub struct SensorFrame {
+    /// Frame timestamp
+    pub timestamp: Timestamp,
+    /// LiDAR point cloud data
+    pub lidar_data: Option<LidarData>,
+    /// Camera image data
+    pub camera_data: Option<CameraData>,
+    /// Radar detection data
+    pub radar_data: Option<RadarData>,
+    /// IMU measurement
+    pub imu_data: Option<ImuData>,
+    /// GNSS position
+    pub gnss_data: Option<GnssData>,
+    /// Odometry data
+    pub odometry_data: Option<OdometryData>,
+}
+
+/// LiDAR sensor data
+#[derive(Debug, Clone)]
+pub struct LidarData {
+    /// Point cloud
+    pub point_cloud: PointCloud,
+    /// Sensor pose relative to vehicle
+    pub sensor_pose: Pose3D,
+    /// Sensor configuration
+    pub config: LidarConfig,
+}
+
+/// LiDAR configuration
+#[derive(Debug, Clone)]
+pub struct LidarConfig {
+    /// Sensor model name
+    pub model: String,
+    /// Vertical field of view (degrees)
+    pub vertical_fov: (f32, f32), // (min, max)
+    /// Horizontal field of view (degrees)
+    pub horizontal_fov: f32, // typically 360
+    /// Vertical resolution (number of beams)
+    pub vertical_resolution: usize,
+    /// Angular resolution (degrees per step)
+    pub angular_resolution: f32,
+    /// Maximum range (meters)
+    pub max_range: f32,
+    /// Minimum range (meters)
+    pub min_range: f32,
+}
+
+impl Default for LidarConfig {
+    fn default() -> Self {
+        // Default configuration similar to Velodyne VLP-16
+        Self {
+            model: "VLP-16".to_string(),
+            vertical_fov: (-15.0, 15.0),
+            horizontal_fov: 360.0,
+            vertical_resolution: 16,
+            angular_resolution: 0.2, // 0.2 degrees
+            max_range: 100.0,
+            min_range: 0.3,
+        }
+    }
+}
+
+/// Camera sensor data
+#[derive(Debug, Clone)]
+pub struct CameraData {
+    /// RGB image tensor (3, H, W)
+    pub image: Tensor,
+    /// Camera intrinsic parameters
+    pub intrinsics: CameraIntrinsics,
+    /// Camera pose relative to vehicle
+    pub sensor_pose: Pose3D,
+    /// Exposure and timing info
+    pub metadata: CameraMetadata,
+}
+
+/// Camera intrinsic parameters
+#[derive(Debug, Clone)]
+pub struct CameraIntrinsics {
+    /// Focal length (fx, fy)
+    pub focal_length: (f32, f32),
+    /// Principal point (cx, cy)
+    pub principal_point: (f32, f32),
+    /// Distortion coefficients [k1, k2, p1, p2, k3]
+    pub distortion: [f32; 5],
+    /// Image size (width, height)
+    pub image_size: (usize, usize),
+}
+
+/// Camera metadata
+#[derive(Debug, Clone)]
+pub struct CameraMetadata {
+    /// Exposure time (seconds)
+    pub exposure_time: f32,
+    /// ISO sensitivity
+    pub iso: u32,
+    /// Frame sequence number
+    pub frame_id: u64,
+}
+
+/// Radar sensor data
+#[derive(Debug, Clone)]
+pub struct RadarData {
+    /// Radar detections
+    pub detections: Vec<RadarDetection>,
+    /// Sensor pose relative to vehicle
+    pub sensor_pose: Pose3D,
+    /// Radar configuration
+    pub config: RadarConfig,
+}
+
+/// Individual radar detection
+#[derive(Debug, Clone)]
+pub struct RadarDetection {
+    /// Range (meters)
+    pub range: f32,
+    /// Azimuth angle (radians)
+    pub azimuth: f32,
+    /// Elevation angle (radians)
+    pub elevation: f32,
+    /// Radial velocity (m/s)
+    pub velocity: f32,
+    /// Radar cross section (dBsm)
+    pub rcs: f32,
+    /// Signal-to-noise ratio (dB)
+    pub snr: f32,
+}
+
+/// Radar configuration
+#[derive(Debug, Clone)]
+pub struct RadarConfig {
+    /// Sensor model
+    pub model: String,
+    /// Frequency (GHz)
+    pub frequency: f32,
+    /// Maximum range (meters)
+    pub max_range: f32,
+    /// Range resolution (meters)
+    pub range_resolution: f32,
+    /// Azimuth field of view (degrees)
+    pub azimuth_fov: f32,
+    /// Elevation field of view (degrees)
+    pub elevation_fov: f32,
+}
+
+/// IMU sensor data
+#[derive(Debug, Clone)]
+pub struct ImuData {
+    /// Linear acceleration (m/s²) in body frame
+    pub acceleration: [f32; 3],
+    /// Angular velocity (rad/s) in body frame
+    pub angular_velocity: [f32; 3],
+    /// Magnetic field (µT) in body frame
+    pub magnetic_field: Option<[f32; 3]>,
+    /// Measurement covariance
+    pub covariance: Option<[[f32; 6]; 6]>,
+}
+
+/// GNSS sensor data
+#[derive(Debug, Clone)]
+pub struct GnssData {
+    /// Latitude (degrees)
+    pub latitude: f64,
+    /// Longitude (degrees)
+    pub longitude: f64,
+    /// Altitude (meters above sea level)
+    pub altitude: f32,
+    /// Position accuracy (meters)
+    pub position_accuracy: f32,
+    /// Fix quality
+    pub fix_quality: GnssFixQuality,
+    /// Number of satellites
+    pub num_satellites: u8,
+}
+
+/// GNSS fix quality
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub enum GnssFixQuality {
+    NoFix,
+    GPS,
+    DGPS,
+    RTK,
+    FloatRTK,
+}
+
+/// Vehicle odometry data
+#[derive(Debug, Clone)]
+pub struct OdometryData {
+    /// Vehicle velocity (m/s) in body frame
+    pub velocity: [f32; 3],
+    /// Vehicle pose estimate
+    pub pose: Pose3D,
+    /// Pose covariance
+    pub covariance: [[f32; 6]; 6],
+    /// Wheel speeds (rad/s) [front_left, front_right, rear_left, rear_right]
+    pub wheel_speeds: Option<[f32; 4]>,
+}
+
+/// 3D pose representation
+#[derive(Debug, Clone)]
+pub struct Pose3D {
+    /// Position (x, y, z) in meters
+    pub position: [f32; 3],
+    /// Orientation quaternion (w, x, y, z)
+    pub orientation: [f32; 4],
+}
+
+impl Default for Pose3D {
+    fn default() -> Self {
+        Self {
+            position: [0.0, 0.0, 0.0],
+            orientation: [1.0, 0.0, 0.0, 0.0], // Identity quaternion
+        }
+    }
+}
+
+impl Pose3D {
+    /// Create identity pose
+    pub fn identity() -> Self {
+        Self::default()
+    }
+
+    /// Create pose from position and Euler angles
+    pub fn from_position_euler(position: [f32; 3], euler: [f32; 3]) -> Self {
+        let (roll, pitch, yaw) = (euler[0], euler[1], euler[2]);
+
+        // Convert Euler angles to quaternion
+        let cr = (roll * 0.5).cos();
+        let sr = (roll * 0.5).sin();
+        let cp = (pitch * 0.5).cos();
+        let sp = (pitch * 0.5).sin();
+        let cy = (yaw * 0.5).cos();
+        let sy = (yaw * 0.5).sin();
+
+        let w = cr * cp * cy + sr * sp * sy;
+        let x = sr * cp * cy - cr * sp * sy;
+        let y = cr * sp * cy + sr * cp * sy;
+        let z = cr * cp * sy - sr * sp * cy;
+
+        Self {
+            position,
+            orientation: [w, x, y, z],
+        }
+    }
+
+    /// Get transformation matrix (4x4)
+    pub fn to_matrix(&self) -> [[f32; 4]; 4] {
+        let [x, y, z] = self.position;
+        let [w, qx, qy, qz] = self.orientation;
+
+        // Convert quaternion to rotation matrix
+        let xx = qx * qx;
+        let yy = qy * qy;
+        let zz = qz * qz;
+        let xy = qx * qy;
+        let xz = qx * qz;
+        let yz = qy * qz;
+        let wx = w * qx;
+        let wy = w * qy;
+        let wz = w * qz;
+
+        [
+            [1.0 - 2.0 * (yy + zz), 2.0 * (xy - wz), 2.0 * (xz + wy), x],
+            [2.0 * (xy + wz), 1.0 - 2.0 * (xx + zz), 2.0 * (yz - wx), y],
+            [2.0 * (xz - wy), 2.0 * (yz + wx), 1.0 - 2.0 * (xx + yy), z],
+            [0.0, 0.0, 0.0, 1.0],
+        ]
+    }
+
+    /// Transform point from this coordinate frame to world
+    pub fn transform_point(&self, point: [f32; 3]) -> [f32; 3] {
+        let matrix = self.to_matrix();
+        [
+            matrix[0][0] * point[0]
+                + matrix[0][1] * point[1]
+                + matrix[0][2] * point[2]
+                + matrix[0][3],
+            matrix[1][0] * point[0]
+                + matrix[1][1] * point[1]
+                + matrix[1][2] * point[2]
+                + matrix[1][3],
+            matrix[2][0] * point[0]
+                + matrix[2][1] * point[1]
+                + matrix[2][2] * point[2]
+                + matrix[2][3],
+        ]
+    }
+}
+
+/// Autonomous driving perception pipeline
+pub struct AutonomousPipeline {
+    /// Object detection models
+    detectors: HashMap<SensorType, Box<dyn AutonomousDetector>>,
+    /// Sensor fusion module
+    // fusion: fusion::SensorFusion,
+    /// Object tracker
+    // tracker: tracking::MultiObjectTracker,
+    /// Path planner integration
+    path_planner: Option<PathPlanner>,
+    // Safety validator
+    // safety_validator: safety::SafetyValidator,
+}
+
+impl AutonomousPipeline {
+    /// Create new autonomous perception pipeline
+    pub fn new() -> VisionResult<Self> {
+        let detectors = HashMap::new();
+
+        // Initialize default detectors
+        // detectors.insert(SensorType::LiDAR, Box::new(lidar::LidarDetector::new()?));
+
+        Ok(Self {
+            detectors,
+            // fusion: fusion::SensorFusion::new()?,
+            // tracker: tracking::MultiObjectTracker::new()?,
+            path_planner: None,
+            // safety_validator: safety::SafetyValidator::new(),
+        })
+    }
+
+    /// Process multi-sensor frame
+    pub fn process_frame(
+        &mut self,
+        frame: &SensorFrame,
+    ) -> VisionResult<AutonomousPerceptionResult> {
+        let start_time = std::time::Instant::now();
+
+        let mut detections = Vec::new();
+        let mut detection_results = HashMap::new();
+
+        // Process LiDAR data
+        if let Some(ref lidar_data) = frame.lidar_data
+            && let Some(detector) = self.detectors.get_mut(&SensorType::LiDAR)
+        {
+            let result = detector.detect_lidar(lidar_data)?;
+            detections.extend(result.boxes_3d.clone());
+            detection_results.insert(
+                SensorType::LiDAR,
+                DetectionResultVariant::Detection3D(result),
+            );
+        }
+
+        // Process camera data
+        if let Some(ref camera_data) = frame.camera_data
+            && let Some(detector) = self.detectors.get_mut(&SensorType::Camera)
+        {
+            let result = detector.detect_camera(camera_data)?;
+            // Convert 2D detections to 3D estimates (simplified)
+            detection_results.insert(
+                SensorType::Camera,
+                DetectionResultVariant::Detection2D(result),
+            );
+        }
+
+        // Process radar data
+        if let Some(ref radar_data) = frame.radar_data {
+            let radar_detections = self.process_radar_data(radar_data)?;
+            detections.extend(radar_detections);
+        }
+
+        // Sensor fusion
+        // let fused_detections = self.fusion.fuse_detections(&detections, frame)?;
+        let fused_detections = detections.clone(); // Temporary: use raw detections
+
+        // Object tracking
+        // let tracked_objects = self.tracker.update(&fused_detections, frame.timestamp)?;
+        let tracked_objects = vec![]; // Temporary: empty tracking
+
+        // Safety validation
+        // let safety_status = self.safety_validator.validate(&tracked_objects, frame)?;
+        let safety_status = SafetyStatus {
+            safety_level: SafetyLevel::Safe,
+            collision_risks: vec![],
+            recommended_actions: vec![],
+            time_to_collision: None,
+        }; // Temporary default
+
+        // Path planning (if available)
+        let path_planning_result = if let Some(ref mut planner) = self.path_planner {
+            Some(planner.plan(&tracked_objects, frame)?)
+        } else {
+            None
+        };
+
+        let processing_time = start_time.elapsed().as_millis() as f32;
+
+        Ok(AutonomousPerceptionResult {
+            timestamp: frame.timestamp,
+            raw_detections: detection_results,
+            fused_detections,
+            tracked_objects,
+            safety_status,
+            path_plan: path_planning_result,
+            processing_time_ms: processing_time,
+        })
+    }
+
+    /// Add detector for specific sensor type
+    pub fn add_detector(&mut self, sensor_type: SensorType, detector: Box<dyn AutonomousDetector>) {
+        self.detectors.insert(sensor_type, detector);
+    }
+
+    /// Set path planner
+    pub fn set_path_planner(&mut self, planner: PathPlanner) {
+        self.path_planner = Some(planner);
+    }
+
+    /// Process radar data into 3D detections
+    fn process_radar_data(&self, radar_data: &RadarData) -> VisionResult<Vec<BoundingBox3D>> {
+        let mut detections = Vec::new();
+
+        for detection in &radar_data.detections {
+            if detection.range > 0.5 && detection.snr > 10.0 {
+                // Basic filtering
+                // Convert spherical to cartesian coordinates
+                let x = detection.range * detection.azimuth.cos() * detection.elevation.cos();
+                let y = detection.range * detection.azimuth.sin() * detection.elevation.cos();
+                let z = detection.range * detection.elevation.sin();
+
+                // Transform to vehicle coordinates
+                let point_sensor = [x, y, z];
+                let point_vehicle = radar_data.sensor_pose.transform_point(point_sensor);
+
+                // Create 3D bounding box (with estimated size)
+                let bbox_3d = BoundingBox3D::new(
+                    point_vehicle,
+                    [2.0, 1.0, 1.5],      // Estimated vehicle dimensions
+                    [0.0, 0.0, 0.0],      // No rotation estimate from radar
+                    detection.snr / 50.0, // Convert SNR to confidence
+                    0,                    // Unknown class from radar alone
+                );
+
+                detections.push(bbox_3d);
+            }
+        }
+
+        debug!(
+            "Processed {} radar detections into {} 3D boxes",
+            radar_data.detections.len(),
+            detections.len()
+        );
+        Ok(detections)
+    }
+}
+
+/// Autonomous detector trait
+pub trait AutonomousDetector {
+    /// Detect objects from LiDAR data
+    fn detect_lidar(&mut self, _lidar_data: &LidarData) -> VisionResult<Detection3DResult> {
+        Err(VisionError::invalid_input("LiDAR detection not supported"))
+    }
+
+    /// Detect objects from camera data
+    fn detect_camera(&mut self, _camera_data: &CameraData) -> VisionResult<DetectionResult> {
+        Err(VisionError::invalid_input("Camera detection not supported"))
+    }
+
+    /// Get supported sensor types
+    fn supported_sensors(&self) -> Vec<SensorType>;
+}
+
+/// Detection result variants for different sensors
+#[derive(Debug, Clone)]
+pub enum DetectionResultVariant {
+    Detection2D(DetectionResult),
+    Detection3D(Detection3DResult),
+}
+
+/// Tracked object in 3D space
+#[derive(Debug, Clone)]
+pub struct TrackedObject {
+    /// Unique track ID
+    pub track_id: usize,
+    /// Current 3D bounding box
+    pub bbox_3d: BoundingBox3D,
+    /// Object velocity (m/s) in world coordinates
+    pub velocity: [f32; 3],
+    /// Track confidence
+    pub track_confidence: f32,
+    /// Object classification
+    pub object_class: ObjectClass,
+    /// Track age (number of frames)
+    pub age: usize,
+    /// Number of consecutive detections
+    pub hits: usize,
+    /// Time since last detection
+    pub time_since_update: f32,
+    /// Predicted trajectory
+    pub predicted_trajectory: Vec<TrajectoryPoint>,
+}
+
+/// Object classification for autonomous driving
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub enum ObjectClass {
+    Vehicle,
+    Pedestrian,
+    Cyclist,
+    Motorcycle,
+    Truck,
+    Bus,
+    TrafficSign,
+    TrafficLight,
+    Barrier,
+    Construction,
+    Animal,
+    Unknown,
+}
+
+/// Point in predicted trajectory
+#[derive(Debug, Clone)]
+pub struct TrajectoryPoint {
+    /// Position at time t
+    pub position: [f32; 3],
+    /// Time from now (seconds)
+    pub time_offset: f32,
+    /// Uncertainty covariance
+    pub covariance: [[f32; 3]; 3],
+}
+
+/// Safety assessment result
+#[derive(Debug, Clone)]
+pub struct SafetyStatus {
+    /// Overall safety level
+    pub safety_level: SafetyLevel,
+    /// Potential collision risks
+    pub collision_risks: Vec<CollisionRisk>,
+    /// Recommended actions
+    pub recommended_actions: Vec<SafetyAction>,
+    /// Time to collision estimates
+    pub time_to_collision: Option<f32>,
+}
+
+/// Safety levels
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub enum SafetyLevel {
+    Safe,
+    Caution,
+    Warning,
+    Critical,
+    Emergency,
+}
+
+/// Collision risk assessment
+#[derive(Debug, Clone)]
+pub struct CollisionRisk {
+    /// Object involved in potential collision
+    pub object_id: usize,
+    /// Risk probability [0, 1]
+    pub probability: f32,
+    /// Time to collision (seconds)
+    pub time_to_collision: f32,
+    /// Collision severity estimate
+    pub severity: CollisionSeverity,
+}
+
+/// Collision severity levels
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub enum CollisionSeverity {
+    Minor,
+    Moderate,
+    Major,
+    Severe,
+}
+
+/// Safety actions
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub enum SafetyAction {
+    None,
+    SlowDown,
+    Stop,
+    SteerLeft,
+    SteerRight,
+    EmergencyBrake,
+    PullOver,
+}
+
+/// Complete perception result
+#[derive(Debug, Clone)]
+pub struct AutonomousPerceptionResult {
+    /// Frame timestamp
+    pub timestamp: Timestamp,
+    /// Raw detection results per sensor
+    pub raw_detections: HashMap<SensorType, DetectionResultVariant>,
+    /// Fused 3D detections
+    pub fused_detections: Vec<BoundingBox3D>,
+    /// Tracked objects with motion
+    pub tracked_objects: Vec<TrackedObject>,
+    /// Safety assessment
+    pub safety_status: SafetyStatus,
+    /// Path planning result
+    pub path_plan: Option<PathPlan>,
+    /// Total processing time
+    pub processing_time_ms: f32,
+}
+
+/// Vehicle state for motion planning
+#[derive(Debug, Clone)]
+pub struct VehicleState {
+    /// Current pose
+    pub pose: Pose3D,
+    /// Linear velocity (m/s)
+    pub velocity: [f32; 3],
+    /// Angular velocity (rad/s)
+    pub angular_velocity: [f32; 3],
+    /// Acceleration (m/s²)
+    pub acceleration: [f32; 3],
+    /// Steering angle (radians)
+    pub steering_angle: f32,
+}
+
+/// Coordinate system transformations
+pub struct CoordinateTransforms;
+
+impl CoordinateTransforms {
+    /// Convert from LiDAR coordinates to vehicle coordinates
+    pub fn lidar_to_vehicle(point: [f32; 3], sensor_pose: &Pose3D) -> [f32; 3] {
+        sensor_pose.transform_point(point)
+    }
+
+    /// Convert from vehicle coordinates to world coordinates
+    pub fn vehicle_to_world(point: [f32; 3], vehicle_pose: &Pose3D) -> [f32; 3] {
+        vehicle_pose.transform_point(point)
+    }
+
+    /// Project 3D point to camera image
+    pub fn world_to_camera_image(
+        point: [f32; 3],
+        camera_pose: &Pose3D,
+        intrinsics: &CameraIntrinsics,
+    ) -> Option<(f32, f32)> {
+        // Transform to camera coordinates
+        let matrix = camera_pose.to_matrix();
+
+        // Inverse transform (world to camera)
+        let cam_x = matrix[0][0] * (point[0] - matrix[0][3])
+            + matrix[1][0] * (point[1] - matrix[1][3])
+            + matrix[2][0] * (point[2] - matrix[2][3]);
+        let cam_y = matrix[0][1] * (point[0] - matrix[0][3])
+            + matrix[1][1] * (point[1] - matrix[1][3])
+            + matrix[2][1] * (point[2] - matrix[2][3]);
+        let cam_z = matrix[0][2] * (point[0] - matrix[0][3])
+            + matrix[1][2] * (point[1] - matrix[1][3])
+            + matrix[2][2] * (point[2] - matrix[2][3]);
+
+        // Check if point is in front of camera
+        if cam_z <= 0.0 {
+            return None;
+        }
+
+        // Project to image plane
+        let fx = intrinsics.focal_length.0;
+        let fy = intrinsics.focal_length.1;
+        let cx = intrinsics.principal_point.0;
+        let cy = intrinsics.principal_point.1;
+
+        let u = fx * cam_x / cam_z + cx;
+        let v = fy * cam_y / cam_z + cy;
+
+        // Check if projection is within image bounds
+        if u >= 0.0
+            && u < intrinsics.image_size.0 as f32
+            && v >= 0.0
+            && v < intrinsics.image_size.1 as f32
+        {
+            Some((u, v))
+        } else {
+            None
+        }
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn test_timestamp() {
+        let ts1 = Timestamp {
+            seconds: 100,
+            nanoseconds: 500_000_000,
+        };
+        let ts2 = Timestamp {
+            seconds: 101,
+            nanoseconds: 0,
+        };
+
+        assert_eq!(ts1.to_seconds(), 100.5);
+        assert_eq!(ts2.duration_since(&ts1), 0.5);
+    }
+
+    #[test]
+    fn test_pose_3d() {
+        let pose =
+            Pose3D::from_position_euler([1.0, 2.0, 3.0], [0.0, 0.0, std::f32::consts::PI / 2.0]);
+        let point = [1.0, 0.0, 0.0];
+        let transformed = pose.transform_point(point);
+
+        // After 90 degree yaw rotation, (1,0,0) should become (0,1,0), then add translation
+        assert!((transformed[0] - 1.0).abs() < 0.01); // x = 1 + 0
+        assert!((transformed[1] - 3.0).abs() < 0.01); // y = 2 + 1
+        assert!((transformed[2] - 3.0).abs() < 0.01); // z = 3 + 0
+    }
+
+    #[test]
+    fn test_sensor_frame_creation() {
+        let frame = SensorFrame {
+            timestamp: Timestamp::now(),
+            lidar_data: None,
+            camera_data: None,
+            radar_data: None,
+            imu_data: None,
+            gnss_data: None,
+            odometry_data: None,
+        };
+
+        assert!(frame.timestamp.seconds > 0);
+    }
+}