""" Example: Complete Camera Tracking System Integration This example demonstrates how to use all three components together: 1. pose_tracker.py - Camera pose tracking with sensor fusion 2. orientation_manager.cpp - High-frequency orientation tracking 3. position_broadcast.py - Real-time position broadcasting Author: Pixeltovoxelprojector Team """ import sys import os import numpy as np import time from scipy.spatial.transform import Rotation # Add src to path sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..')) from src.camera.pose_tracker import ( CameraPoseTracker, IMUMeasurement, GPSMeasurement, VIOMeasurement ) from src.camera.position_broadcast import ( PositionBroadcaster, PositionReceiver, BroadcastPose, CoordinateFrame, CameraCalibration ) def simulate_sensor_data(camera_id, base_time, iteration): """ Simulate realistic sensor data for testing In a real system, this would be replaced with actual sensor readings from RTK GPS, IMU, and VIO systems. """ current_time = base_time + iteration * 1_000_000 # 1ms increments (nanoseconds) # Simulate IMU data (1000Hz) imu = IMUMeasurement( timestamp=current_time, angular_velocity=np.array([ 0.01 + 0.001 * np.sin(iteration * 0.01), 0.02 + 0.001 * np.cos(iteration * 0.01), 0.005 ]), linear_acceleration=np.array([ 0.1 * np.sin(iteration * 0.001), 0.05 * np.cos(iteration * 0.001), 9.81 + 0.02 * np.random.randn() ]), camera_id=camera_id ) # Simulate GPS data (10Hz) gps = None if iteration % 100 == 0: # ECEF coordinates near San Francisco base_ecef = np.array([-2707066.0, -4260952.0, 3884595.0]) gps = GPSMeasurement( timestamp=current_time, position=base_ecef + np.array([camera_id * 10, 0, 0]) + np.random.randn(3) * 0.02, position_covariance=np.eye(3) * 0.0025, # 5cm standard deviation rtk_fix_quality=2, # RTK fixed camera_id=camera_id ) # Simulate VIO data (30Hz) vio = None if iteration % 33 == 0: vio_pose = np.eye(4) vio_pose[0:3, 3] = np.array([camera_id * 10, 0, 0]) vio = VIOMeasurement( timestamp=current_time, relative_pose=vio_pose, covariance=np.eye(6) * 0.01, camera_id=camera_id, feature_count=50 + int(10 * np.random.randn()) ) return imu, gps, vio def main(): """Main example demonstrating full system integration""" print("=" * 80) print("Camera Tracking System - Complete Integration Example") print("=" * 80) print() # Configuration NUM_CAMERAS = 20 UPDATE_RATE_HZ = 1000.0 DURATION_SECONDS = 10 print(f"Configuration:") print(f" Number of cameras: {NUM_CAMERAS}") print(f" Update rate: {UPDATE_RATE_HZ} Hz") print(f" Duration: {DURATION_SECONDS} seconds") print() # ========================================================================= # Step 1: Initialize Pose Tracker # ========================================================================= print("Step 1: Initializing Camera Pose Tracker...") pose_tracker = CameraPoseTracker( num_cameras=NUM_CAMERAS, update_rate_hz=UPDATE_RATE_HZ ) pose_tracker.start() print(" ✓ Pose tracker started") print() # ========================================================================= # Step 2: Initialize Position Broadcaster # ========================================================================= print("Step 2: Initializing Position Broadcaster...") broadcaster = PositionBroadcaster(num_cameras=NUM_CAMERAS) # Set ENU reference point (San Francisco) broadcaster.set_enu_reference( lat_deg=37.7749, lon_deg=-122.4194, alt_m=0.0 ) # Set world frame (voxel grid reference) world_frame = CoordinateFrame( name="VoxelGrid", origin=np.array([0, 0, 0]), orientation=Rotation.from_euler('xyz', [0, 0, 0]), timestamp=time.time() ) broadcaster.set_world_frame(world_frame) broadcaster.start() print(" ✓ Position broadcaster started") print() # ========================================================================= # Step 3: Configure Camera Calibrations # ========================================================================= print("Step 3: Configuring camera calibrations...") for camera_id in range(NUM_CAMERAS): # Example calibration (would come from actual calibration procedure) calibration = CameraCalibration( camera_id=camera_id, intrinsic_matrix=np.array([ [4000.0, 0.0, 3840.0], [0.0, 4000.0, 2160.0], [0.0, 0.0, 1.0] ]), distortion_coeffs=np.array([0.0, 0.0, 0.0, 0.0, 0.0]), resolution=(7680, 4320), # 8K resolution fov_horizontal=90.0, fov_vertical=60.0, timestamp=time.time() ) broadcaster.update_calibration(calibration) print(f" ✓ Calibrated {NUM_CAMERAS} cameras") print() # ========================================================================= # Step 4: Optional - Start Position Receiver (for testing) # ========================================================================= print("Step 4: Starting position receiver (for testing)...") receiver = PositionReceiver(server_address="localhost", port=5555) received_poses_count = [0] * NUM_CAMERAS def pose_callback(pose): """Callback for received poses""" received_poses_count[pose.camera_id] += 1 receiver.start(pose_callback=pose_callback) print(" ✓ Position receiver started") print() # ========================================================================= # Step 5: Main Processing Loop # ========================================================================= print("Step 5: Running main processing loop...") print("-" * 80) base_time = time.time_ns() start_time = time.time() try: for iteration in range(int(DURATION_SECONDS * UPDATE_RATE_HZ)): # Process each camera for camera_id in range(NUM_CAMERAS): # Get simulated sensor data imu, gps, vio = simulate_sensor_data(camera_id, base_time, iteration) # Add measurements to pose tracker pose_tracker.add_imu_measurement(imu) if gps is not None: pose_tracker.add_gps_measurement(gps) if vio is not None: pose_tracker.add_vio_measurement(vio) # Get and broadcast poses every 10ms (100Hz broadcasting) if iteration % 10 == 0: for camera_id in range(NUM_CAMERAS): # Get current pose from tracker pose = pose_tracker.get_pose(camera_id) if pose is not None: # Create broadcast message broadcast_pose = BroadcastPose( camera_id=camera_id, timestamp=pose.timestamp, position_ecef=pose.position, position_enu=np.zeros(3), # Will be computed by broadcaster position_world=np.zeros(3), # Will be computed by broadcaster orientation_quat=pose.orientation.as_quat(), orientation_euler=pose.orientation.as_euler('xyz'), velocity_ecef=pose.velocity, angular_velocity=pose.angular_velocity, position_std=np.sqrt(np.diag(pose.position_covariance)), orientation_std=np.sqrt(np.diag(pose.orientation_covariance)), rtk_fix_quality=2, feature_count=50, imu_health=0.95 ) # Broadcast pose broadcaster.broadcast_pose(broadcast_pose) # Progress update every second if iteration % int(UPDATE_RATE_HZ) == 0: elapsed = time.time() - start_time progress = (iteration / (DURATION_SECONDS * UPDATE_RATE_HZ)) * 100 print(f" Progress: {progress:.1f}% ({elapsed:.1f}s elapsed)") # Maintain timing (1000Hz) time.sleep(1.0 / UPDATE_RATE_HZ) print("-" * 80) print("✓ Processing complete!") print() # ===================================================================== # Step 6: Display Final Statistics # ===================================================================== print("Step 6: Final Statistics") print("=" * 80) # Pose accuracy statistics print("\nPose Accuracy (Sample: Camera 0):") stats = pose_tracker.get_accuracy_statistics(0) if stats: print(f" Position accuracy (3D): {stats['position_3d_std_cm']:.3f} cm") print(f" Position X: {stats['position_std_x_cm']:.3f} cm") print(f" Position Y: {stats['position_std_y_cm']:.3f} cm") print(f" Position Z: {stats['position_std_z_cm']:.3f} cm") print(f" Orientation accuracy (3D): {stats['orientation_3d_std_deg']:.4f}°") print(f" Roll: {stats['orientation_std_roll_deg']:.4f}°") print(f" Pitch: {stats['orientation_std_pitch_deg']:.4f}°") print(f" Yaw: {stats['orientation_std_yaw_deg']:.4f}°") # Broadcast/receive statistics print("\nBroadcast Statistics:") total_received = sum(received_poses_count) print(f" Total poses received: {total_received}") for camera_id in range(min(5, NUM_CAMERAS)): # Show first 5 cameras print(f" Camera {camera_id}: {received_poses_count[camera_id]} poses received") # System verification print("\nSystem Verification:") requirements_met = True # Check position accuracy if stats and stats['position_3d_std_cm'] < 5.0: print(" ✓ Position accuracy <5cm requirement: MET") else: print(" ✗ Position accuracy <5cm requirement: NOT MET") requirements_met = False # Check orientation accuracy if stats and stats['orientation_3d_std_deg'] < 0.1: print(" ✓ Orientation accuracy <0.1° requirement: MET") else: print(" ✗ Orientation accuracy <0.1° requirement: NOT MET") requirements_met = False # Check update rate actual_rate = (DURATION_SECONDS * UPDATE_RATE_HZ) / (time.time() - start_time) if actual_rate >= UPDATE_RATE_HZ * 0.95: # 95% of target rate print(f" ✓ Update rate {UPDATE_RATE_HZ}Hz requirement: MET ({actual_rate:.1f}Hz achieved)") else: print(f" ✗ Update rate {UPDATE_RATE_HZ}Hz requirement: NOT MET ({actual_rate:.1f}Hz achieved)") requirements_met = False print() if requirements_met: print("✓ ALL REQUIREMENTS MET!") else: print("⚠ Some requirements not met (this is expected in simulation)") print() except KeyboardInterrupt: print("\nInterrupted by user") finally: # ===================================================================== # Step 7: Cleanup # ===================================================================== print("Step 7: Shutting down...") receiver.stop() broadcaster.stop() pose_tracker.stop() print(" ✓ All systems stopped") print() print("=" * 80) print("Example complete!") print("=" * 80) if __name__ == "__main__": main()