automotive-adas

name	automotive-adas
description	Automotive Adas expertise. Covers 7 topics: Adas Features Implementation, Autosar Adas Integration, Camera Processing Vision, Hd Maps Localization, Path Planning Control.
tags	["automotive","automotive-adas"]

Automotive Adas

Adas Features Implementation

ADAS Features Implementation

Overview

Concrete implementations of ADAS features: Adaptive Cruise Control (ACC), Lane Keep Assist (LKA), Automatic Emergency Braking (AEB), Blind Spot Detection (BSD), Park Assist, and Traffic Sign Recognition (TSR). Production-ready code for L0-L2+ systems.

Adaptive Cruise Control (ACC)

Full ACC Implementation

#include <Eigen/Dense>
#include <algorithm>
#include <cmath>

class AdaptiveCruiseControl {
public:
    struct ACCParams {
        double time_gap = 2.0;              // seconds (ISO 22179)
        double min_distance = 5.0;          // meters
        double max_acceleration = 2.0;      // m/s²
        double max_deceleration = -3.0;     // m/s²
        double comfort_decel = -2.0;        // m/s²
        double set_speed = 30.0;            // m/s (108 km/h)
        double speed_tolerance = 2.0;       // m/s
    };

    enum class ACCMode {
        OFF,
        STANDBY,
        ACTIVE_CRUISE,
        ACTIVE_FOLLOWING,
        EMERGENCY_BRAKE
    };

    AdaptiveCruiseControl(const ACCParams& params) : params_(params), mode_(ACCMode::STANDBY) {}

    struct ACCOutput {
        double acceleration;        // Commanded acceleration (m/s²)
        ACCMode mode;
        double target_speed;
        double target_distance;
        bool warning_issued;
    };

    ACCOutput compute(double ego_velocity, double ego_acceleration,
                     const std::vector<DetectedObject>& objects) {
        ACCOutput output;
        output.mode = mode_;
        output.warning_issued = false;

        // Find lead vehicle
        auto lead_vehicle = find_lead_vehicle(objects, ego_velocity);

        if (!lead_vehicle.has_value()) {
            // No lead vehicle - cruise control mode
            output.acceleration = cruise_control(ego_velocity);
            output.target_speed = params_.set_speed;
            output.target_distance = 0.0;
            mode_ = ACCMode::ACTIVE_CRUISE;
        } else {
            // Following mode
            double relative_velocity = ego_velocity - lead_vehicle->velocity;
            double distance = lead_vehicle->distance;
            double desired_distance = calculate_desired_distance(ego_velocity);

            // Calculate acceleration using Intelligent Driver Model (IDM)
            output.acceleration = intelligent_driver_model(
                ego_velocity, distance, relative_velocity, desired_distance
            );

            output.target_speed = lead_vehicle->velocity;
            output.target_distance = desired_distance;

            // Check for emergency
            double ttc = time_to_collision(distance, relative_velocity);
            if (ttc > 0 && ttc < 2.0 && relative_velocity > 0) {
                output.acceleration = params_.max_deceleration;
                output.warning_issued = true;
                mode_ = ACCMode::EMERGENCY_BRAKE;
            } else {
                mode_ = ACCMode::ACTIVE_FOLLOWING;
            }
        }

        // Clamp acceleration
        output.acceleration = std::clamp(output.acceleration,
                                        params_.max_deceleration,
                                        params_.max_acceleration);

        return output;
    }

private:
    ACCParams params_;
    ACCMode mode_;

    struct DetectedObject {
        double distance;   // meters (longitudinal)
        double velocity;   // m/s
        double lateral_offset;  // meters
        std::string object_class;
    };

    std::optional<DetectedObject> find_lead_vehicle(
        const std::vector<DetectedObject>& objects, double ego_velocity)
    {
        std::optional<DetectedObject> lead;
        double min_distance = std::numeric_limits<double>::max();

        for (const auto& obj : objects) {
            // Filter: only consider vehicles in same lane
            if (std::abs(obj.lateral_offset) > 1.5) continue;

            // Filter: only vehicles ahead
            if (obj.distance < 0) continue;

            // Find closest
            if (obj.distance < min_distance) {
                min_distance = obj.distance;
                lead = obj;
            }
        }

        return lead;
    }

    double cruise_control(double ego_velocity) {
        // Simple P controller to reach set speed
        double error = params_.set_speed - ego_velocity;
        double kp = 0.5;
        return std::clamp(kp * error, params_.max_deceleration, params_.max_acceleration);
    }

    double calculate_desired_distance(double ego_velocity) {
        // Time gap policy: d = d_min + v * T
        return params_.min_distance + ego_velocity * params_.time_gap;
    }

    double intelligent_driver_model(double velocity, double distance,
                                    double relative_velocity, double desired_distance) {
        // IDM parameters
        const double a_max = params_.max_acceleration;
        const double b_comfortable = -params_.comfort_decel;
        const double delta = 4.0;  // Acceleration exponent

        // Desired dynamical distance
        double v_approach_term = velocity * relative_velocity / (2 * std::sqrt(a_max * b_comfortable));
        double s_star = params_.min_distance + std::max(0.0, velocity * params_.time_gap + v_approach_term);

        // IDM acceleration
        double accel = a_max * (1.0 - std::pow(velocity / params_.set_speed, delta) -
                               std::pow(s_star / distance, 2.0));

        return accel;
    }

    double time_to_collision(double distance, double relative_velocity) {
        if (relative_velocity <= 0) return -1.0;  // No collision
        return distance / relative_velocity;
    }
};

AUTOSAR ACC Component

<!-- ACC_SWC.arxml -->
<AUTOSAR>
  <AR-PACKAGES>
    <AR-PACKAGE>
      <SHORT-NAME>ACC_Application</SHORT-NAME>
      <ELEMENTS>
        <APPLICATION-SW-COMPONENT-TYPE>
          <SHORT-NAME>ACC_SWC</SHORT-NAME>
          <PORTS>
            <R-PORT-PROTOTYPE>
              <SHORT-NAME>EgoSpeed</SHORT-NAME>
              <REQUIRED-INTERFACE-TREF>/Interfaces/SpeedInterface</REQUIRED-INTERFACE-TREF>
            </R-PORT-PROTOTYPE>
            <R-PORT-PROTOTYPE>
              <SHORT-NAME>RadarObjects</SHORT-NAME>
              <REQUIRED-INTERFACE-TREF>/Interfaces/ObjectListInterface</REQUIRED-INTERFACE-TREF>
            </R-PORT-PROTOTYPE>
            <P-PORT-PROTOTYPE>
              <SHORT-NAME>AccelRequest</SHORT-NAME>
              <PROVIDED-INTERFACE-TREF>/Interfaces/AccelerationInterface</PROVIDED-INTERFACE-TREF>
            </P-PORT-PROTOTYPE>
            <P-PORT-PROTOTYPE>
              <SHORT-NAME>ACCStatus</SHORT-NAME>
              <PROVIDED-INTERFACE-TREF>/Interfaces/ACCStatusInterface</PROVIDED-INTERFACE-TREF>
            </P-PORT-PROTOTYPE>
          </PORTS>
          <INTERNAL-BEHAVIORS>
            <SWC-INTERNAL-BEHAVIOR>
              <SHORT-NAME>ACC_InternalBehavior</SHORT-NAME>
              <RUNNABLES>
                <RUNNABLE-ENTITY>
                  <SHORT-NAME>ACC_MainFunction</SHORT-NAME>
                  <MINIMUM-START-INTERVAL>0.05</MINIMUM-START-INTERVAL>
                  <CAN-BE-INVOKED-CONCURRENTLY>false</CAN-BE-INVOKED-CONCURRENTLY>
                  <SYMBOL>ACC_MainFunction</SYMBOL>
                </RUNNABLE-ENTITY>
              </RUNNABLES>
            </SWC-INTERNAL-BEHAVIOR>
          </INTERNAL-BEHAVIORS>
        </APPLICATION-SW-COMPONENT-TYPE>
      </ELEMENTS>
    </AR-PACKAGE>
  </AR-PACKAGES>
</AUTOSAR>

Lane Keep Assist (LKA)

#include <Eigen/Dense>
#include <vector>
#include <cmath>

class LaneKeepAssist {
public:
    struct LKAParams {
        double lookahead_time = 2.0;        // seconds
        double kp_lateral = 0.5;            // Proportional gain
        double ki_lateral = 0.05;           // Integral gain
        double kd_lateral = 0.1;            // Derivative gain
        double max_steering_angle = 0.1;    // radians (~6 degrees)
        double lane_departure_threshold = 0.3;  // meters
    };

    enum class LKAState {
        OFF,
        STANDBY,
        ACTIVE,
        WARNING
    };

    LaneKeepAssist(const LKAParams& params) : params_(params), state_(LKAState::STANDBY) {
        integral_error_ = 0.0;
        previous_error_ = 0.0;
    }

    struct LaneMarkings {
        // Polynomial coefficients: lateral_offset = c0 + c1*x + c2*x^2 + c3*x^3
        Eigen::Vector4d left_lane;
        Eigen::Vector4d right_lane;
        bool left_detected;
        bool right_detected;
        double lane_width;
    };

    struct LKAOutput {
        double steering_torque;  // N⋅m
        LKAState state;
        bool warning_active;
        double lateral_offset;
    };

    LKAOutput compute(const LaneMarkings& lanes, double velocity, double dt) {
        LKAOutput output;
        output.state = state_;
        output.warning_active = false;

        // Check if lanes are detected
        if (!lanes.left_detected && !lanes.right_detected) {
            state_ = LKAState::STANDBY;
            output.steering_torque = 0.0;
            integral_error_ = 0.0;
            return output;
        }

        // Calculate lateral offset from lane center
        double lateral_offset = calculate_lateral_offset(lanes);
        output.lateral_offset = lateral_offset;

        // Calculate lateral error at lookahead distance
        double lookahead_distance = velocity * params_.lookahead_time;
        double lateral_error = calculate_lateral_error(lanes, lookahead_distance);

        // Check for lane departure
        if (std::abs(lateral_offset) > params_.lane_departure_threshold) {
            state_ = LKAState::WARNING;
            output.warning_active = true;
        } else {
            state_ = LKAState::ACTIVE;
        }

        // PID control for steering torque
        integral_error_ += lateral_error * dt;
        double derivative_error = (lateral_error - previous_error_) / dt;

        double steering_correction = params_.kp_lateral * lateral_error +
                                    params_.ki_lateral * integral_error_ +
                                    params_.kd_lateral * derivative_error;

        previous_error_ = lateral_error;

        // Convert to steering torque (simplified model)
        double steering_torque = steering_correction * 10.0;  // N⋅m

        // Limit torque
        const double max_torque = 3.0;  // N⋅m
        steering_torque = std::clamp(steering_torque, -max_torque, max_torque);

        output.steering_torque = steering_torque;

        return output;
    }

private:
    LKAParams params_;
    LKAState state_;
    double integral_error_;
    double previous_error_;

    double calculate_lateral_offset(const LaneMarkings& lanes) {
        // Calculate offset from lane center
        double left_offset = 0.0, right_offset = 0.0;
        int count = 0;

        if (lanes.left_detected) {
            left_offset = lanes.left_lane(0);  // c0 at x=0 (vehicle position)
            count++;
        }

        if (lanes.right_detected) {
            right_offset = lanes.right_lane(0);
            count++;
        }

        if (count == 2) {
            // Both lanes detected - offset from center
            return (left_offset + right_offset) / 2.0;
        } else if (lanes.left_detected) {
            // Only left lane - assume lane width
            return left_offset - lanes.lane_width / 2.0;
        } else {
            // Only right lane
            return right_offset + lanes.lane_width / 2.0;
        }
    }

    double calculate_lateral_error(const LaneMarkings& lanes, double lookahead_distance) {
        // Evaluate polynomial at lookahead distance
        double x = lookahead_distance;
        double left_lateral = 0.0, right_lateral = 0.0;
        int count = 0;

        if (lanes.left_detected) {
            left_lateral = lanes.left_lane(0) + lanes.left_lane(1) * x +
                          lanes.left_lane(2) * x * x + lanes.left_lane(3) * x * x * x;
            count++;
        }

        if (lanes.right_detected) {
            right_lateral = lanes.right_lane(0) + lanes.right_lane(1) * x +
                           lanes.right_lane(2) * x * x + lanes.right_lane(3) * x * x * x;
            count++;
        }

        if (count == 2) {
            return (left_lateral + right_lateral) / 2.0;
        } else if (lanes.left_detected) {
            return left_lateral - lanes.lane_width / 2.0;
        } else {
            return right_lateral + lanes.lane_width / 2.0;
        }
    }
};

Automatic Emergency Braking (AEB)

class AutomaticEmergencyBraking {
public:
    struct AEBParams {
        double warning_ttc = 2.5;       // Time to collision for warning (seconds)
        double brake_ttc = 1.5;         // TTC for partial braking
        double emergency_ttc = 0.8;     // TTC for full emergency braking
        double max_brake_pressure = 100.0;  // bar
        double partial_brake_pressure = 30.0;  // bar
    };

    enum class AEBState {
        MONITORING,
        WARNING,
        PARTIAL_BRAKE,
        EMERGENCY_BRAKE
    };

    AutomaticEmergencyBraking(const AEBParams& params) : params_(params) {}

    struct AEBOutput {
        double brake_pressure;  // bar
        AEBState state;
        bool warning_active;
        bool brake_active;
        double ttc;
    };

    AEBOutput compute(const DetectedObject& closest_object, double ego_velocity) {
        AEBOutput output;
        output.brake_pressure = 0.0;
        output.state = AEBState::MONITORING;
        output.warning_active = false;
        output.brake_active = false;
        output.ttc = -1.0;

        if (!closest_object.valid) {
            return output;
        }

        // Calculate time to collision
        double relative_velocity = ego_velocity - closest_object.velocity;
        double ttc = calculate_ttc(closest_object.distance, relative_velocity);
        output.ttc = ttc;

        if (ttc < 0) {
            return output;  // No collision imminent
        }

        // State machine
        if (ttc < params_.emergency_ttc) {
            // Emergency braking
            output.state = AEBState::EMERGENCY_BRAKE;
            output.brake_pressure = params_.max_brake_pressure;
            output.brake_active = true;
            output.warning_active = true;
        } else if (ttc < params_.brake_ttc) {
            // Partial braking
            output.state = AEBState::PARTIAL_BRAKE;
            output.brake_pressure = params_.partial_brake_pressure;
            output.brake_active = true;
            output.warning_active = true;
        } else if (ttc < params_.warning_ttc) {
            // Warning only
            output.state = AEBState::WARNING;
            output.warning_active = true;
        }

        return output;
    }

private:
    AEBParams params_;

    struct DetectedObject {
        double distance;
        double velocity;
        bool valid;
    };

    double calculate_ttc(double distance, double relative_velocity) {
        if (relative_velocity <= 0) return -1.0;
        return distance / relative_velocity;
    }
};

Blind Spot Detection (BSD)

class BlindSpotDetection {
public:
    struct BSDParams {
        double blind_spot_min_x = -1.0;     // meters (rear)
        double blind_spot_max_x = 1.0;      // meters (front)
        double blind_spot_min_y = 1.5;      // meters (lateral)
        double blind_spot_max_y = 3.5;      // meters (lateral)
        double warning_velocity_threshold = 2.0;  // m/s (approaching)
    };

    enum class BSDZone {
        NO_DETECTION,
        LEFT_BLIND_SPOT,
        RIGHT_BLIND_SPOT,
        BOTH_BLIND_SPOTS
    };

    BlindSpotDetection(const BSDParams& params) : params_(params) {}

    struct BSDOutput {
        BSDZone zone;
        bool left_warning;
        bool right_warning;
        bool left_approaching;
        bool right_approaching;
    };

    BSDOutput compute(const std::vector<DetectedObject>& objects) {
        BSDOutput output;
        output.zone = BSDZone::NO_DETECTION;
        output.left_warning = false;
        output.right_warning = false;
        output.left_approaching = false;
        output.right_approaching = false;

        for (const auto& obj : objects) {
            // Check if object is in blind spot region
            bool in_longitudinal_range = (obj.x >= params_.blind_spot_min_x &&
                                         obj.x <= params_.blind_spot_max_x);

            bool in_left_blind_spot = (in_longitudinal_range &&
                                      obj.y >= params_.blind_spot_min_y &&
                                      obj.y <= params_.blind_spot_max_y);

            bool in_right_blind_spot = (in_longitudinal_range &&
                                       obj.y >= -params_.blind_spot_max_y &&
                                       obj.y <= -params_.blind_spot_min_y);

            if (in_left_blind_spot) {
                output.left_warning = true;

                // Check if approaching
                if (obj.vx > params_.warning_velocity_threshold) {
                    output.left_approaching = true;
                }
            }

            if (in_right_blind_spot) {
                output.right_warning = true;

                if (obj.vx > params_.warning_velocity_threshold) {
                    output.right_approaching = true;
                }
            }
        }

        // Determine zone
        if (output.left_warning && output.right_warning) {
            output.zone = BSDZone::BOTH_BLIND_SPOTS;
        } else if (output.left_warning) {
            output.zone = BSDZone::LEFT_BLIND_SPOT;
        } else if (output.right_warning) {
            output.zone = BSDZone::RIGHT_BLIND_SPOT;
        }

        return output;
    }

private:
    BSDParams params_;

    struct DetectedObject {
        double x, y;      // Position relative to ego vehicle (meters)
        double vx, vy;    // Velocity relative to ego vehicle (m/s)
    };
};

Parking Assist

import numpy as np
from enum import Enum

class ParkingAssistant:
    """
    Automated parking system for parallel and perpendicular parking
    """

    class ParkingMode(Enum):
        SEARCH = 1
        PLANNING = 2
        EXECUTING = 3
        COMPLETED = 4

    def __init__(self, vehicle_length=4.5, vehicle_width=1.8, wheelbase=2.7):
        self.vehicle_length = vehicle_length
        self.vehicle_width = vehicle_width
        self.wheelbase = wheelbase
        self.mode = self.ParkingMode.SEARCH

        # Minimum parking space dimensions
        self.min_parallel_length = vehicle_length + 1.0  # meters
        self.min_perpendicular_width = vehicle_width + 0.6  # meters

    def detect_parking_space(self, ultrasonic_measurements):
        """
        Detect available parking spaces using ultrasonic sensors

        Args:
            ultrasonic_measurements: List of distances from 12 ultrasonic sensors

        Returns:
            parking_space: Dictionary with space dimensions and type
        """
        # Left side measurements (4 sensors)
        left_front = ultrasonic_measurements[0]
        left_mid_front = ultrasonic_measurements[1]
        left_mid_rear = ultrasonic_measurements[2]
        left_rear = ultrasonic_measurements[3]

        # Detect parallel parking space
        if (left_front > 2.0 and left_mid_front > 2.0 and
            left_mid_rear > 2.0 and left_rear > 2.0):

            # Measure space length
            space_length = self.measure_space_length(ultrasonic_measurements)

            if space_length >= self.min_parallel_length:
                return {
                    'type': 'parallel',
                    'length': space_length,
                    'valid': True
                }

        return {'valid': False}

    def plan_parallel_parking(self, space_length, space_lateral_offset):
        """
        Plan trajectory for parallel parking

        Returns:
            List of waypoints with (x, y, theta, steering_angle)
        """
        waypoints = []

        # Phase 1: Align with parking space
        waypoints.append({
            'x': 0.0,
            'y': 0.0,
            'theta': 0.0,
            'steering': 0.0,
            'velocity': 0.5  # m/s
        })

        # Phase 2: Reverse into space with maximum steering
        max_steering = 0.6  # radians
        reverse_distance = 3.0  # meters

        for i in range(10):
            progress = i / 10.0
            waypoints.append({
                'x': -progress * reverse_distance * np.cos(max_steering),
                'y': space_lateral_offset - progress * reverse_distance * np.sin(max_steering),
                'theta': -progress * max_steering,
                'steering': max_steering,
                'velocity': -0.3  # Reverse slowly
            })

        # Phase 3: Straighten wheels and center in space
        for i in range(5):
            progress = (i + 1) / 5.0
            waypoints.append({
                'x': waypoints[-1]['x'] - 0.3,
                'y': waypoints[-1]['y'],
                'theta': -(1.0 - progress * 0.5) * max_steering,
                'steering': -progress * max_steering,
                'velocity': -0.2
            })

        return waypoints

    def execute_parking(self, current_pose, target_waypoint, dt=0.1):
        """
        Execute parking maneuver using path following controller

        Args:
            current_pose: (x, y, theta) current vehicle pose
            target_waypoint: Dictionary with target pose and steering
            dt: Time step

        Returns:
            control_output: (steering_angle, velocity)
        """
        # Pure pursuit controller for waypoint following
        dx = target_waypoint['x'] - current_pose[0]
        dy = target_waypoint['y'] - current_pose[1]

        # Calculate steering angle
        lookahead_distance = 1.0  # meters
        alpha = np.arctan2(dy, dx) - current_pose[2]

        steering_angle = np.arctan2(2.0 * self.wheelbase * np.sin(alpha),
                                    lookahead_distance)

        # Limit steering angle
        steering_angle = np.clip(steering_angle, -0.6, 0.6)

        velocity = target_waypoint['velocity']

        return (steering_angle, velocity)

    def measure_space_length(self, ultrasonic_measurements):
        """Estimate parking space length from ultrasonic measurements"""
        # Simplified: integrate measurements along vehicle path
        # In production, use more sophisticated SLAM-based approach
        return 5.5  # meters (placeholder)

Traffic Sign Recognition (TSR)

import torch
import torchvision.transforms as transforms
from PIL import Image

class TrafficSignRecognition:
    """
    Traffic sign detection and classification
    """

    def __init__(self, model_path, device='cuda'):
        self.device = torch.device(device if torch.cuda.is_available() else 'cpu')

        # Load pretrained model (YOLO + classifier)
        self.detector = torch.hub.load('ultralytics/yolov5', 'custom',
                                      path=model_path, force_reload=False)

        # Sign classes (GTSDB/GTSRB dataset)
        self.sign_classes = {
            0: 'speed_limit_20',
            1: 'speed_limit_30',
            2: 'speed_limit_50',
            3: 'speed_limit_60',
            4: 'speed_limit_70',
            5: 'speed_limit_80',
            6: 'speed_limit_100',
            7: 'speed_limit_120',
            8: 'no_overtaking',
            9: 'no_overtaking_trucks',
            10: 'priority_road',
            11: 'yield',
            12: 'stop',
            13: 'no_entry',
            # ... (43 classes total)
        }

    def detect_and_classify(self, image):
        """
        Detect traffic signs and classify them

        Args:
            image: RGB image (H, W, 3)

        Returns:
            List of detected signs with class, confidence, bbox
        """
        # Run detection
        results = self.detector(image)

        detected_signs = []

        for *box, conf, cls in results.xyxy[0].cpu().numpy():
            sign_id = int(cls)

            if sign_id in self.sign_classes:
                sign = {
                    'class': self.sign_classes[sign_id],
                    'confidence': float(conf),
                    'bbox': box,  # [x1, y1, x2, y2]
                    'sign_id': sign_id
                }

                detected_signs.append(sign)

        return detected_signs

    def extract_speed_limit(self, detected_signs):
        """Extract current speed limit from detected signs"""
        speed_limits = []

        for sign in detected_signs:
            if 'speed_limit' in sign['class']:
                # Extract speed value from class name
                speed_value = int(sign['class'].split('_')[-1])
                speed_limits.append((speed_value, sign['confidence']))

        if speed_limits:
            # Return most confident speed limit
            return max(speed_limits, key=lambda x: x[1])[0]

        return None

HIL Testing Configuration

# Hardware-in-Loop test setup for ADAS features
import can

class ADASHILTester:
    """
    HIL test environment for ADAS features
    """

    def __init__(self, can_interface='can0'):
        self.bus = can.interface.Bus(channel=can_interface, bustype='socketcan')

    def simulate_radar_objects(self, objects):
        """Send simulated radar objects over CAN"""
        for obj in objects:
            # CAN message for radar object (example format)
            data = [
                int(obj['distance'] * 10) & 0xFF,
                (int(obj['distance'] * 10) >> 8) & 0xFF,
                int(obj['velocity'] * 10 + 128) & 0xFF,
                int(obj['lateral_offset'] * 10 + 128) & 0xFF,
                obj['object_id'] & 0xFF,
                0, 0, 0  # Reserved
            ]

            msg = can.Message(arbitration_id=0x500 + obj['object_id'],
                            data=data,
                            is_extended_id=False)

            self.bus.send(msg)

    def read_acc_output(self):
        """Read ACC output from ECU"""
        msg = self.bus.recv(timeout=1.0)

        if msg and msg.arbitration_id == 0x400:
            # Parse ACC command
            accel_request = (msg.data[0] | (msg.data[1] << 8)) / 100.0 - 10.0
            acc_active = bool(msg.data[2] & 0x01)

            return {
                'acceleration': accel_request,
                'active': acc_active
            }

        return None

    def test_acc_scenario(self, scenario_name, test_duration=10.0):
        """Run ACC test scenario"""
        print(f"Running ACC test: {scenario_name}")

        # Load scenario (lead vehicle trajectory)
        scenario = self.load_scenario(scenario_name)

        start_time = time.time()

        while (time.time() - start_time) < test_duration:
            # Simulate radar detections
            timestamp = time.time() - start_time
            objects = scenario.get_objects_at_time(timestamp)

            self.simulate_radar_objects(objects)

            # Read ACC response
            acc_output = self.read_acc_output()

            if acc_output:
                print(f"T={timestamp:.2f}s: ACC accel={acc_output['acceleration']:.2f} m/s²")

            time.sleep(0.05)  # 20 Hz

        print(f"Test {scenario_name} completed")

Performance Metrics

Feature	Latency	Accuracy	ASIL
ACC	< 100ms	TTC error < 5%	ASIL B
LKA	< 50ms	Lateral error < 10cm	ASIL B
AEB	< 50ms	False positive < 1%	ASIL D
BSD	< 100ms	Detection rate > 99%	ASIL A
Parking	< 200ms	Position error < 5cm	ASIL A
TSR	< 200ms	Recognition > 95%	QM

Standards

• ISO 22179: Full-speed ACC
• ISO 11270: Lane departure warning
• Euro NCAP: AEB testing protocols
• UN R79: Steering system requirements (LKA)

Related Skills

• sensor-fusion-perception.md
• camera-processing-vision.md
• path-planning-control.md

---

Autosar Adas Integration

AUTOSAR ADAS Integration

Overview

AUTOSAR Classic and Adaptive Platform for ADAS, RTE configuration, sensor abstraction, ara::com for distributed ADAS, resource partitioning, timing constraints, and ASIL-D compliance.

AUTOSAR Classic Platform for ADAS

Software Component Architecture

<!-- ADAS_ECU_Extract.arxml -->
<?xml version="1.0" encoding="UTF-8"?>
<AUTOSAR xmlns="http://autosar.org/schema/r4.0">
  <AR-PACKAGES>
    <!-- Application Layer -->
    <AR-PACKAGE>
      <SHORT-NAME>ADAS_Application</SHORT-NAME>
      <ELEMENTS>
        <!-- Sensor Fusion SWC -->
        <APPLICATION-SW-COMPONENT-TYPE>
          <SHORT-NAME>SensorFusion_SWC</SHORT-NAME>
          <PORTS>
            <!-- Input Ports -->
            <R-PORT-PROTOTYPE>
              <SHORT-NAME>CameraData</SHORT-NAME>
              <REQUIRED-INTERFACE-TREF DEST="SENDER-RECEIVER-INTERFACE">
                /Interfaces/CameraImageInterface
              </REQUIRED-INTERFACE-TREF>
            </R-PORT-PROTOTYPE>
            <R-PORT-PROTOTYPE>
              <SHORT-NAME>RadarData</SHORT-NAME>
              <REQUIRED-INTERFACE-TREF DEST="SENDER-RECEIVER-INTERFACE">
                /Interfaces/RadarObjectListInterface
              </REQUIRED-INTERFACE-TREF>
            </R-PORT-PROTOTYPE>
            <R-PORT-PROTOTYPE>
              <SHORT-NAME>LidarData</SHORT-NAME>
              <REQUIRED-INTERFACE-TREF DEST="SENDER-RECEIVER-INTERFACE">
                /Interfaces/LidarPointCloudInterface
              </REQUIRED-INTERFACE-TREF>
            </R-PORT-PROTOTYPE>

            <!-- Output Ports -->
            <P-PORT-PROTOTYPE>
              <SHORT-NAME>FusedObjectList</SHORT-NAME>
              <PROVIDED-INTERFACE-TREF DEST="SENDER-RECEIVER-INTERFACE">
                /Interfaces/FusedObjectListInterface
              </PROVIDED-INTERFACE-TREF>
            </P-PORT-PROTOTYPE>
          </PORTS>

          <!-- Internal Behavior -->
          <INTERNAL-BEHAVIORS>
            <SWC-INTERNAL-BEHAVIOR>
              <SHORT-NAME>SensorFusion_InternalBehavior</SHORT-NAME>

              <!-- Runnables -->
              <RUNNABLES>
                <RUNNABLE-ENTITY>
                  <SHORT-NAME>SensorFusion_Init</SHORT-NAME>
                  <MINIMUM-START-INTERVAL>0</MINIMUM-START-INTERVAL>
                  <CAN-BE-INVOKED-CONCURRENTLY>false</CAN-BE-INVOKED-CONCURRENTLY>
                  <SYMBOL>SensorFusion_Init</SYMBOL>
                </RUNNABLE-ENTITY>

                <RUNNABLE-ENTITY>
                  <SHORT-NAME>SensorFusion_MainFunction</SHORT-NAME>
                  <MINIMUM-START-INTERVAL>0.02</MINIMUM-START-INTERVAL>  <!-- 50 Hz -->
                  <CAN-BE-INVOKED-CONCURRENTLY>false</CAN-BE-INVOKED-CONCURRENTLY>
                  <SYMBOL>SensorFusion_MainFunction</SYMBOL>

                  <!-- Data Read Access -->
                  <DATA-RECEIVE-POINT-BY-ARGUMENTS>
                    <VARIABLE-ACCESS>
                      <SHORT-NAME>Read_CameraData</SHORT-NAME>
                      <ACCESSED-VARIABLE>
                        <AUTOSAR-VARIABLE-IREF>
                          <PORT-PROTOTYPE-REF DEST="R-PORT-PROTOTYPE">
                            /ADAS_Application/SensorFusion_SWC/CameraData
                          </PORT-PROTOTYPE-REF>
                          <TARGET-DATA-PROTOTYPE-REF>
                            /Interfaces/CameraImageInterface/ImageData
                          </TARGET-DATA-PROTOTYPE-REF>
                        </AUTOSAR-VARIABLE-IREF>
                      </ACCESSED-VARIABLE>
                    </VARIABLE-ACCESS>
                  </DATA-RECEIVE-POINT-BY-ARGUMENTS>

                  <!-- Data Write Access -->
                  <DATA-SEND-POINTS>
                    <VARIABLE-ACCESS>
                      <SHORT-NAME>Write_FusedObjects</SHORT-NAME>
                      <ACCESSED-VARIABLE>
                        <AUTOSAR-VARIABLE-IREF>
                          <PORT-PROTOTYPE-REF DEST="P-PORT-PROTOTYPE">
                            /ADAS_Application/SensorFusion_SWC/FusedObjectList
                          </PORT-PROTOTYPE-REF>
                          <TARGET-DATA-PROTOTYPE-REF>
                            /Interfaces/FusedObjectListInterface/Objects
                          </TARGET-DATA-PROTOTYPE-REF>
                        </AUTOSAR-VARIABLE-IREF>
                      </ACCESSED-VARIABLE>
                    </VARIABLE-ACCESS>
                  </DATA-SEND-POINTS>
                </RUNNABLE-ENTITY>
              </RUNNABLES>

              <!-- Events -->
              <EVENTS>
                <INIT-EVENT>
                  <SHORT-NAME>InitEvent</SHORT-NAME>
                  <START-ON-EVENT-REF DEST="RUNNABLE-ENTITY">
                    /ADAS_Application/SensorFusion_SWC/SensorFusion_InternalBehavior/SensorFusion_Init
                  </START-ON-EVENT-REF>
                </INIT-EVENT>

                <TIMING-EVENT>
                  <SHORT-NAME>MainFunction_TimingEvent</SHORT-NAME>
                  <START-ON-EVENT-REF DEST="RUNNABLE-ENTITY">
                    /ADAS_Application/SensorFusion_SWC/SensorFusion_InternalBehavior/SensorFusion_MainFunction
                  </START-ON-EVENT-REF>
                  <PERIOD>0.02</PERIOD>  <!-- 50 Hz -->
                </TIMING-EVENT>
              </EVENTS>

            </SWC-INTERNAL-BEHAVIOR>
          </INTERNAL-BEHAVIORS>
        </APPLICATION-SW-COMPONENT-TYPE>

        <!-- Path Planning SWC -->
        <APPLICATION-SW-COMPONENT-TYPE>
          <SHORT-NAME>PathPlanning_SWC</SHORT-NAME>
          <PORTS>
            <R-PORT-PROTOTYPE>
              <SHORT-NAME>FusedObjectList</SHORT-NAME>
              <REQUIRED-INTERFACE-TREF DEST="SENDER-RECEIVER-INTERFACE">
                /Interfaces/FusedObjectListInterface
              </REQUIRED-INTERFACE-TREF>
            </R-PORT-PROTOTYPE>
            <R-PORT-PROTOTYPE>
              <SHORT-NAME>VehicleState</SHORT-NAME>
              <REQUIRED-INTERFACE-TREF DEST="SENDER-RECEIVER-INTERFACE">
                /Interfaces/VehicleStateInterface
              </REQUIRED-INTERFACE-TREF>
            </R-PORT-PROTOTYPE>
            <P-PORT-PROTOTYPE>
              <SHORT-NAME>TrajectoryOutput</SHORT-NAME>
              <PROVIDED-INTERFACE-TREF DEST="SENDER-RECEIVER-INTERFACE">
                /Interfaces/TrajectoryInterface
              </PROVIDED-INTERFACE-TREF>
            </P-PORT-PROTOTYPE>
          </PORTS>
        </APPLICATION-SW-COMPONENT-TYPE>

      </ELEMENTS>
    </AR-PACKAGE>

    <!-- Interface Definitions -->
    <AR-PACKAGE>
      <SHORT-NAME>Interfaces</SHORT-NAME>
      <ELEMENTS>
        <SENDER-RECEIVER-INTERFACE>
          <SHORT-NAME>FusedObjectListInterface</SHORT-NAME>
          <IS-SERVICE>false</IS-SERVICE>
          <DATA-ELEMENTS>
            <VARIABLE-DATA-PROTOTYPE>
              <SHORT-NAME>Objects</SHORT-NAME>
              <TYPE-TREF DEST="IMPLEMENTATION-DATA-TYPE">
                /DataTypes/FusedObjectArray
              </TYPE-TREF>
            </VARIABLE-DATA-PROTOTYPE>
          </DATA-ELEMENTS>
        </SENDER-RECEIVER-INTERFACE>
      </ELEMENTS>
    </AR-PACKAGE>

    <!-- Data Type Definitions -->
    <AR-PACKAGE>
      <SHORT-NAME>DataTypes</SHORT-NAME>
      <ELEMENTS>
        <IMPLEMENTATION-DATA-TYPE>
          <SHORT-NAME>FusedObjectArray</SHORT-NAME>
          <CATEGORY>ARRAY</CATEGORY>
          <SUB-ELEMENTS>
            <IMPLEMENTATION-DATA-TYPE-ELEMENT>
              <SHORT-NAME>Object</SHORT-NAME>
              <CATEGORY>TYPE_REFERENCE</CATEGORY>
              <ARRAY-SIZE>50</ARRAY-SIZE>
              <ARRAY-SIZE-SEMANTICS>FIXED-SIZE</ARRAY-SIZE-SEMANTICS>
              <SW-DATA-DEF-PROPS>
                <SW-DATA-DEF-PROPS-VARIANTS>
                  <SW-DATA-DEF-PROPS-CONDITIONAL>
                    <IMPLEMENTATION-DATA-TYPE-REF DEST="IMPLEMENTATION-DATA-TYPE">
                      /DataTypes/FusedObject
                    </IMPLEMENTATION-DATA-TYPE-REF>
                  </SW-DATA-DEF-PROPS-CONDITIONAL>
                </SW-DATA-DEF-PROPS-VARIANTS>
              </SW-DATA-DEF-PROPS>
            </IMPLEMENTATION-DATA-TYPE-ELEMENT>
          </SUB-ELEMENTS>
        </IMPLEMENTATION-DATA-TYPE>

        <IMPLEMENTATION-DATA-TYPE>
          <SHORT-NAME>FusedObject</SHORT-NAME>
          <CATEGORY>STRUCTURE</CATEGORY>
          <SUB-ELEMENTS>
            <IMPLEMENTATION-DATA-TYPE-ELEMENT>
              <SHORT-NAME>id</SHORT-NAME>
              <CATEGORY>VALUE</CATEGORY>
              <SW-DATA-DEF-PROPS>
                <SW-DATA-DEF-PROPS-VARIANTS>
                  <SW-DATA-DEF-PROPS-CONDITIONAL>
                    <BASE-TYPE-REF DEST="SW-BASE-TYPE">/BaseTypes/uint32</BASE-TYPE-REF>
                  </SW-DATA-DEF-PROPS-CONDITIONAL>
                </SW-DATA-DEF-PROPS-VARIANTS>
              </SW-DATA-DEF-PROPS>
            </IMPLEMENTATION-DATA-TYPE-ELEMENT>
            <IMPLEMENTATION-DATA-TYPE-ELEMENT>
              <SHORT-NAME>position_x</SHORT-NAME>
              <CATEGORY>VALUE</CATEGORY>
              <SW-DATA-DEF-PROPS>
                <SW-DATA-DEF-PROPS-VARIANTS>
                  <SW-DATA-DEF-PROPS-CONDITIONAL>
                    <BASE-TYPE-REF DEST="SW-BASE-TYPE">/BaseTypes/float32</BASE-TYPE-REF>
                  </SW-DATA-DEF-PROPS-CONDITIONAL>
                </SW-DATA-DEF-PROPS-VARIANTS>
              </SW-DATA-DEF-PROPS>
            </IMPLEMENTATION-DATA-TYPE-ELEMENT>
            <IMPLEMENTATION-DATA-TYPE-ELEMENT>
              <SHORT-NAME>position_y</SHORT-NAME>
              <CATEGORY>VALUE</CATEGORY>
            </IMPLEMENTATION-DATA-TYPE-ELEMENT>
            <IMPLEMENTATION-DATA-TYPE-ELEMENT>
              <SHORT-NAME>velocity_x</SHORT-NAME>
              <CATEGORY>VALUE</CATEGORY>
            </IMPLEMENTATION-DATA-TYPE-ELEMENT>
            <IMPLEMENTATION-DATA-TYPE-ELEMENT>
              <SHORT-NAME>velocity_y</SHORT-NAME>
              <CATEGORY>VALUE</CATEGORY>
            </IMPLEMENTATION-DATA-TYPE-ELEMENT>
            <IMPLEMENTATION-DATA-TYPE-ELEMENT>
              <SHORT-NAME>object_class</SHORT-NAME>
              <CATEGORY>VALUE</CATEGORY>
              <SW-DATA-DEF-PROPS>
                <SW-DATA-DEF-PROPS-VARIANTS>
                  <SW-DATA-DEF-PROPS-CONDITIONAL>
                    <BASE-TYPE-REF DEST="SW-BASE-TYPE">/BaseTypes/uint8</BASE-TYPE-REF>
                  </SW-DATA-DEF-PROPS-CONDITIONAL>
                </SW-DATA-DEF-PROPS-VARIANTS>
              </SW-DATA-DEF-PROPS>
            </IMPLEMENTATION-DATA-TYPE-ELEMENT>
          </SUB-ELEMENTS>
        </IMPLEMENTATION-DATA-TYPE>
      </ELEMENTS>
    </AR-PACKAGE>
  </AR-PACKAGES>
</AUTOSAR>

RTE Generated Code

/* Rte_SensorFusion.h - Generated by RTE Generator */

#ifndef RTE_SENSORFUSION_H
#define RTE_SENSORFUSION_H

#include "Rte_Type.h"

/* API Functions */
FUNC(Std_ReturnType, RTE_CODE) Rte_Read_SensorFusion_CameraData_ImageData(
    P2VAR(CameraImage_Type, AUTOMATIC, RTE_APPL_DATA) data
);

FUNC(Std_ReturnType, RTE_CODE) Rte_Read_SensorFusion_RadarData_Objects(
    P2VAR(RadarObjectList_Type, AUTOMATIC, RTE_APPL_DATA) data
);

FUNC(Std_ReturnType, RTE_CODE) Rte_Write_SensorFusion_FusedObjectList_Objects(
    P2CONST(FusedObjectArray_Type, AUTOMATIC, RTE_APPL_CONST) data
);

/* Runnable Entity Prototypes */
FUNC(void, RTE_CODE) SensorFusion_Init(void);
FUNC(void, RTE_CODE) SensorFusion_MainFunction(void);

#endif /* RTE_SENSORFUSION_H */

Application Implementation

/* SensorFusion.c - Application Implementation */

#include "Rte_SensorFusion.h"
#include "SensorFusion_Internal.h"

static FusionState_Type fusionState;

void SensorFusion_Init(void) {
    /* Initialize fusion algorithms */
    InitKalmanFilters(&fusionState);
    InitDataAssociation(&fusionState);
}

void SensorFusion_MainFunction(void) {
    CameraImage_Type cameraData;
    RadarObjectList_Type radarData;
    LidarPointCloud_Type lidarData;
    FusedObjectArray_Type fusedObjects;

    /* Read sensor inputs via RTE */
    Std_ReturnType ret_camera = Rte_Read_SensorFusion_CameraData_ImageData(&cameraData);
    Std_ReturnType ret_radar = Rte_Read_SensorFusion_RadarData_Objects(&radarData);
    Std_ReturnType ret_lidar = Rte_Read_SensorFusion_LidarData_PointCloud(&lidarData);

    if ((ret_camera == RTE_E_OK) && (ret_radar == RTE_E_OK) && (ret_lidar == RTE_E_OK)) {
        /* Perform sensor fusion */
        ProcessCameraDetections(&cameraData, &fusionState);
        ProcessRadarDetections(&radarData, &fusionState);
        ProcessLidarDetections(&lidarData, &fusionState);

        /* Run fusion algorithm (EKF) */
        UpdateKalmanFilters(&fusionState);

        /* Data association */
        PerformDataAssociation(&fusionState);

        /* Generate fused object list */
        GenerateFusedObjectList(&fusionState, &fusedObjects);

        /* Write output via RTE */
        Rte_Write_SensorFusion_FusedObjectList_Objects(&fusedObjects);
    }
}

AUTOSAR Adaptive Platform for ADAS

Service-Oriented Architecture

// ara::com service interface definition
// SensorFusion.arxml service interface

#include <ara/com/types.h>
#include <ara/core/future.h>
#include <ara/core/result.h>

namespace adas {
namespace sensorfusion {

struct FusedObject {
    uint32_t id;
    float position_x;
    float position_y;
    float velocity_x;
    float velocity_y;
    uint8_t object_class;
    float confidence;
};

using FusedObjectList = std::vector<FusedObject>;

class SensorFusionServiceInterface {
public:
    virtual ~SensorFusionServiceInterface() = default;

    // Events (publisher-subscriber)
    virtual ara::com::EventPtr<FusedObjectList> GetFusedObjectListEvent() = 0;

    // Methods (request-response)
    virtual ara::core::Future<ara::core::Result<bool>> StartFusion() = 0;
    virtual ara::core::Future<ara::core::Result<bool>> StopFusion() = 0;

    // Fields (getter/setter with notification)
    virtual ara::com::FieldPtr<uint32_t> GetFusionStatusField() = 0;
};

}} // namespace adas::sensorfusion

Service Implementation (Provider)

// SensorFusionServiceImpl.cpp

#include "SensorFusionServiceInterface.h"
#include <ara/com/instance_identifier.h>
#include <ara/core/instance_specifier.h>

namespace adas {
namespace sensorfusion {

class SensorFusionServiceImpl : public SensorFusionServiceInterface {
public:
    SensorFusionServiceImpl() {
        // Initialize service skeleton
        ara::core::InstanceSpecifier instance("ADAS/SensorFusion/Instance1");
        skeleton_ = std::make_unique<SensorFusionSkeleton>(instance);

        // Offer service
        skeleton_->OfferService();
    }

    ~SensorFusionServiceImpl() {
        skeleton_->StopOfferService();
    }

    ara::com::EventPtr<FusedObjectList> GetFusedObjectListEvent() override {
        return skeleton_->fusedObjectList;
    }

    ara::core::Future<ara::core::Result<bool>> StartFusion() override {
        // Start fusion processing
        fusion_active_ = true;
        return ara::core::MakeReadyFuture<ara::core::Result<bool>>(true);
    }

    ara::core::Future<ara::core::Result<bool>> StopFusion() override {
        fusion_active_ = false;
        return ara::core::MakeReadyFuture<ara::core::Result<bool>>(true);
    }

    ara::com::FieldPtr<uint32_t> GetFusionStatusField() override {
        return skeleton_->fusionStatus;
    }

    // Main processing function
    void ProcessSensorData() {
        if (!fusion_active_) return;

        // Perform fusion
        FusedObjectList fusedObjects = PerformFusion();

        // Publish event
        skeleton_->fusedObjectList.Send(fusedObjects);

        // Update status field
        skeleton_->fusionStatus.Set(1);  // 1 = Active
    }

private:
    std::unique_ptr<SensorFusionSkeleton> skeleton_;
    bool fusion_active_ = false;

    FusedObjectList PerformFusion() {
        // Fusion logic here
        FusedObjectList objects;
        // ...
        return objects;
    }
};

}} // namespace adas::sensorfusion

Service Consumer (Proxy)

// PathPlanningApp.cpp - Consumer of SensorFusion service

#include "SensorFusionServiceInterface.h"
#include <ara/com/service_proxy_factory.h>
#include <ara/core/instance_specifier.h>

class PathPlanningApp {
public:
    PathPlanningApp() {
        // Find and connect to service
        ara::core::InstanceSpecifier instance("ADAS/SensorFusion/Instance1");

        auto handles = SensorFusionProxy::FindService(instance);

        if (!handles.empty()) {
            proxy_ = std::make_unique<SensorFusionProxy>(handles[0]);

            // Subscribe to fused object list
            proxy_->GetFusedObjectListEvent().Subscribe(1);  // Queue size = 1
            proxy_->GetFusedObjectListEvent().SetReceiveHandler(
                [this](const FusedObjectList& objects) {
                    HandleFusedObjects(objects);
                }
            );
        }
    }

    void HandleFusedObjects(const FusedObjectList& objects) {
        // Use fused objects for path planning
        PlanPath(objects);
    }

    void PlanPath(const FusedObjectList& objects) {
        // Path planning logic
    }

private:
    std::unique_ptr<SensorFusionProxy> proxy_;
};

Resource Partitioning & Timing

Timing Configuration

<!-- Timing_Config.arxml -->
<AUTOSAR>
  <AR-PACKAGES>
    <AR-PACKAGE>
      <SHORT-NAME>Timing</SHORT-NAME>
      <ELEMENTS>
        <!-- Task Configuration -->
        <OS-TASK>
          <SHORT-NAME>SensorFusion_Task</SHORT-NAME>
          <PRIORITY>10</PRIORITY>  <!-- High priority -->
          <SCHEDULE>FULL</SCHEDULE>
          <ACTIVATION>1</ACTIVATION>
          <AUTOSTART>true</AUTOSTART>
          <TIMING-PROTECTION>
            <EXECUTION-TIME>
              <VALUE>0.015</VALUE>  <!-- 15ms WCET -->
            </EXECUTION-TIME>
            <TIME-FRAME>
              <VALUE>0.020</VALUE>  <!-- 20ms period -->
            </TIME-FRAME>
          </TIMING-PROTECTION>
        </OS-TASK>

        <OS-TASK>
          <SHORT-NAME>PathPlanning_Task</SHORT-NAME>
          <PRIORITY>8</PRIORITY>
          <SCHEDULE>FULL</SCHEDULE>
          <TIMING-PROTECTION>
            <EXECUTION-TIME>
              <VALUE>0.045</VALUE>  <!-- 45ms WCET -->
            </EXECUTION-TIME>
            <TIME-FRAME>
              <VALUE>0.050</VALUE>  <!-- 50ms period -->
            </TIME-FRAME>
          </TIMING-PROTECTION>
        </OS-TASK>

        <!-- Alarm for periodic activation -->
        <OS-ALARM>
          <SHORT-NAME>SensorFusion_Alarm</SHORT-NAME>
          <COUNTER-REF DEST="OS-COUNTER">/Timing/SystemCounter</COUNTER-REF>
          <ALARM-ACTION>
            <OS-ALARM-ACTIVATE-TASK-ACTION>
              <TASK-REF DEST="OS-TASK">/Timing/SensorFusion_Task</TASK-REF>
            </OS-ALARM-ACTIVATE-TASK-ACTION>
          </ALARM-ACTION>
          <AUTOSTART-ALARM>
            <AUTOSTART-ALARM-REF DEST="AUTOSTART">/Timing/Autostart</AUTOSTART-ALARM-REF>
            <ALARM-TIME>20</ALARM-TIME>  <!-- 20ms -->
            <CYCLE-TIME>20</CYCLE-TIME>
          </AUTOSTART-ALARM>
        </OS-ALARM>
      </ELEMENTS>
    </AR-PACKAGE>
  </AR-PACKAGES>
</AUTOSAR>

Memory Protection

<!-- Memory_Partition.arxml -->
<AUTOSAR>
  <AR-PACKAGES>
    <AR-PACKAGE>
      <SHORT-NAME>MemoryPartitioning</SHORT-NAME>
      <ELEMENTS>
        <OS-APPLICATION>
          <SHORT-NAME>ADAS_Application</SHORT-NAME>
          <TRUSTED>true</TRUSTED>
          <MEMORY-SECTIONS>
            <OS-APPLICATION-MEMORY-SECTION>
              <SHORT-NAME>ADAS_RAM</SHORT-NAME>
              <SIZE>0x100000</SIZE>  <!-- 1MB -->
              <ALIGNMENT>4</ALIGNMENT>
              <BASE-ADDRESS>0x40000000</BASE-ADDRESS>
            </OS-APPLICATION-MEMORY-SECTION>
          </MEMORY-SECTIONS>
        </OS-APPLICATION>
      </ELEMENTS>
    </AR-PACKAGE>
  </AR-PACKAGES>
</AUTOSAR>

Safety (ASIL-D) Configuration

Safety Mechanisms

/* Safety Monitor for ADAS Functions */

#define ADAS_SAFETY_TIMEOUT_MS 100
#define ADAS_PLAUSIBILITY_THRESHOLD 0.5

typedef enum {
    SAFETY_STATE_NORMAL,
    SAFETY_STATE_DEGRADED,
    SAFETY_STATE_SAFE_STOP
} SafetyState_Type;

typedef struct {
    uint32_t timestamp_ms;
    boolean sensor_fusion_alive;
    boolean path_planning_alive;
    boolean control_alive;
    SafetyState_Type state;
} SafetyMonitor_Type;

void SafetyMonitor_Check(SafetyMonitor_Type* monitor) {
    uint32_t current_time = GetSystemTime_ms();

    /* Check aliveness */
    if ((current_time - monitor->timestamp_ms) > ADAS_SAFETY_TIMEOUT_MS) {
        monitor->state = SAFETY_STATE_SAFE_STOP;
        TriggerSafeMechanism();
    }

    /* Plausibility checks */
    if (!CheckSensorFusionPlausibility()) {
        monitor->state = SAFETY_STATE_DEGRADED;
    }
}

void SafetyMonitor_UpdateAlive(SafetyMonitor_Type* monitor, ADASComponent_Type component) {
    switch (component) {
        case COMPONENT_SENSOR_FUSION:
            monitor->sensor_fusion_alive = TRUE;
            break;
        case COMPONENT_PATH_PLANNING:
            monitor->path_planning_alive = TRUE;
            break;
        case COMPONENT_CONTROL:
            monitor->control_alive = TRUE;
            break;
    }

    monitor->timestamp_ms = GetSystemTime_ms();
}

Performance Requirements

Component	WCET	Period	Latency	ASIL
Sensor Fusion	15ms	20ms	< 50ms	ASIL D
Path Planning	45ms	50ms	< 100ms	ASIL D
Control	5ms	10ms	< 20ms	ASIL D
Diagnostics	20ms	100ms	N/A	ASIL B

Standards

• AUTOSAR Classic R4.x: Foundation for ADAS ECUs
• AUTOSAR Adaptive R19-11: High-performance computing platforms
• ISO 26262: ASIL D for safety-critical functions
• ISO 17356 (AUTOSAR): Integration standard

Related Skills

• sensor-fusion-perception.md
• adas-features-implementation.md
• path-planning-control.md

---

Camera Processing Vision

Camera Processing & Computer Vision for ADAS

Overview

Camera-based perception for ADAS including lane detection, object detection, semantic segmentation, depth estimation, and ISP tuning. Covers classical computer vision (Hough transform, edge detection) and deep learning approaches (YOLO, SSD, Faster R-CNN, semantic segmentation networks).

Camera Hardware Architecture

Typical ADAS Camera Setup

Multi-Camera System (ADAS L2-L5)
────────────────────────────────────────

Front View:
  - Wide FOV (120°): Pedestrian detection, lane keeping
  - Tele FOV (30°): Long-range object detection (up to 200m)
  - Fisheye (180°+): Parking assistance

Surround View (360°):
  - Front fisheye: 190° FOV
  - Rear fisheye: 190° FOV
  - Left/Right fisheye: 190° FOV each

Specifications:
  - Resolution: 1280x960 to 3840x2160 (1-8MP)
  - Frame Rate: 30-60 FPS
  - Dynamic Range: 100-120 dB HDR
  - Interface: MIPI CSI-2, GMSL2, FPD-Link III
  - ISP: Hardware accelerated (denoise, HDR, lens correction)

Lane Detection

Classical Approach: Hough Transform

import cv2
import numpy as np

class LaneDetector:
    """
    Classical lane detection using edge detection and Hough transform
    """

    def __init__(self):
        # Canny edge detection parameters
        self.canny_low = 50
        self.canny_high = 150

        # Hough transform parameters
        self.rho = 1              # Distance resolution (pixels)
        self.theta = np.pi/180    # Angular resolution (radians)
        self.threshold = 50       # Min intersections to detect line
        self.min_line_length = 100
        self.max_line_gap = 50

        # ROI vertices (trapezoid)
        self.roi_vertices = None

    def detect_lanes(self, image):
        """
        Detect lane lines in image

        Args:
            image: RGB image (H, W, 3)

        Returns:
            lane_image: Image with detected lanes drawn
            lane_lines: List of detected lane line parameters
        """
        # 1. Convert to grayscale
        gray = cv2.cvtColor(image, cv2.COLOR_RGB2GRAY)

        # 2. Apply Gaussian blur
        blur = cv2.GaussianBlur(gray, (5, 5), 0)

        # 3. Edge detection
        edges = cv2.Canny(blur, self.canny_low, self.canny_high)

        # 4. Apply ROI mask
        masked_edges = self.apply_roi(edges, image.shape)

        # 5. Hough line detection
        lines = cv2.HoughLinesP(
            masked_edges,
            self.rho,
            self.theta,
            self.threshold,
            minLineLength=self.min_line_length,
            maxLineGap=self.max_line_gap
        )

        # 6. Filter and cluster lines into left/right lanes
        left_lane, right_lane = self.separate_lanes(lines, image.shape)

        # 7. Fit polynomial to lane points
        lane_lines = self.fit_lane_polynomials(left_lane, right_lane)

        # 8. Draw lanes on image
        lane_image = self.draw_lanes(image, lane_lines)

        return lane_image, lane_lines

    def apply_roi(self, edges, image_shape):
        """Apply region of interest mask"""
        height, width = image_shape[:2]

        # Define trapezoid ROI
        vertices = np.array([[
            (width * 0.1, height),
            (width * 0.4, height * 0.6),
            (width * 0.6, height * 0.6),
            (width * 0.9, height)
        ]], dtype=np.int32)

        mask = np.zeros_like(edges)
        cv2.fillPoly(mask, vertices, 255)

        masked = cv2.bitwise_and(edges, mask)
        return masked

    def separate_lanes(self, lines, image_shape):
        """Separate left and right lane lines based on slope"""
        if lines is None:
            return [], []

        height, width = image_shape[:2]
        left_lines = []
        right_lines = []

        for line in lines:
            x1, y1, x2, y2 = line[0]

            # Calculate slope
            if x2 - x1 == 0:
                continue
            slope = (y2 - y1) / (x2 - x1)

            # Filter by slope
            if slope < -0.5:  # Left lane (negative slope)
                left_lines.append(line[0])
            elif slope > 0.5:  # Right lane (positive slope)
                right_lines.append(line[0])

        return left_lines, right_lines

    def fit_lane_polynomials(self, left_lines, right_lines):
        """Fit polynomial curves to lane line points"""
        def fit_poly(lines):
            if not lines:
                return None

            # Extract all points
            points = []
            for x1, y1, x2, y2 in lines:
                points.extend([(x1, y1), (x2, y2)])

            if len(points) < 2:
                return None

            # Convert to numpy arrays
            points = np.array(points)
            x = points[:, 0]
            y = points[:, 1]

            # Fit 2nd order polynomial
            poly_coeffs = np.polyfit(y, x, 2)
            return poly_coeffs

        left_poly = fit_poly(left_lines)
        right_poly = fit_poly(right_lines)

        return {"left": left_poly, "right": right_poly}

    def draw_lanes(self, image, lane_lines):
        """Draw detected lanes on image"""
        lane_image = np.copy(image)
        height = image.shape[0]

        # Y coordinates for drawing
        y_vals = np.linspace(height * 0.6, height, 100)

        # Draw left lane
        if lane_lines["left"] is not None:
            left_poly = lane_lines["left"]
            left_x = np.polyval(left_poly, y_vals).astype(int)
            left_points = np.array([np.column_stack((left_x, y_vals.astype(int)))])
            cv2.polylines(lane_image, left_points, False, (255, 0, 0), 5)

        # Draw right lane
        if lane_lines["right"] is not None:
            right_poly = lane_lines["right"]
            right_x = np.polyval(right_poly, y_vals).astype(int)
            right_points = np.array([np.column_stack((right_x, y_vals.astype(int)))])
            cv2.polylines(lane_image, right_points, False, (0, 0, 255), 5)

        return lane_image

Deep Learning Approach: Lane Segmentation

import torch
import torch.nn as nn
import torchvision.transforms as transforms

class LaneSegmentationNet(nn.Module):
    """
    U-Net style architecture for lane segmentation
    """

    def __init__(self, in_channels=3, num_classes=2):
        super(LaneSegmentationNet, self).__init__()

        # Encoder (downsampling)
        self.enc1 = self.conv_block(in_channels, 64)
        self.enc2 = self.conv_block(64, 128)
        self.enc3 = self.conv_block(128, 256)
        self.enc4 = self.conv_block(256, 512)

        # Bottleneck
        self.bottleneck = self.conv_block(512, 1024)

        # Decoder (upsampling)
        self.upconv4 = nn.ConvTranspose2d(1024, 512, 2, stride=2)
        self.dec4 = self.conv_block(1024, 512)

        self.upconv3 = nn.ConvTranspose2d(512, 256, 2, stride=2)
        self.dec3 = self.conv_block(512, 256)

        self.upconv2 = nn.ConvTranspose2d(256, 128, 2, stride=2)
        self.dec2 = self.conv_block(256, 128)

        self.upconv1 = nn.ConvTranspose2d(128, 64, 2, stride=2)
        self.dec1 = self.conv_block(128, 64)

        # Output
        self.out = nn.Conv2d(64, num_classes, 1)

        self.pool = nn.MaxPool2d(2, 2)

    def conv_block(self, in_ch, out_ch):
        return nn.Sequential(
            nn.Conv2d(in_ch, out_ch, 3, padding=1),
            nn.BatchNorm2d(out_ch),
            nn.ReLU(inplace=True),
            nn.Conv2d(out_ch, out_ch, 3, padding=1),
            nn.BatchNorm2d(out_ch),
            nn.ReLU(inplace=True)
        )

    def forward(self, x):
        # Encoder
        enc1 = self.enc1(x)
        enc2 = self.enc2(self.pool(enc1))
        enc3 = self.enc3(self.pool(enc2))
        enc4 = self.enc4(self.pool(enc3))

        # Bottleneck
        bottleneck = self.bottleneck(self.pool(enc4))

        # Decoder with skip connections
        dec4 = self.upconv4(bottleneck)
        dec4 = torch.cat([dec4, enc4], dim=1)
        dec4 = self.dec4(dec4)

        dec3 = self.upconv3(dec4)
        dec3 = torch.cat([dec3, enc3], dim=1)
        dec3 = self.dec3(dec3)

        dec2 = self.upconv2(dec3)
        dec2 = torch.cat([dec2, enc2], dim=1)
        dec2 = self.dec2(dec2)

        dec1 = self.upconv1(dec2)
        dec1 = torch.cat([dec1, enc1], dim=1)
        dec1 = self.dec1(dec1)

        return self.out(dec1)


class DeepLaneDetector:
    """
    Deep learning-based lane detection
    """

    def __init__(self, model_path, device='cuda'):
        self.device = torch.device(device if torch.cuda.is_available() else 'cpu')
        self.model = LaneSegmentationNet().to(self.device)
        self.model.load_state_dict(torch.load(model_path, map_location=self.device))
        self.model.eval()

        self.transform = transforms.Compose([
            transforms.ToTensor(),
            transforms.Normalize(mean=[0.485, 0.456, 0.406],
                               std=[0.229, 0.224, 0.225])
        ])

    def detect(self, image):
        """
        Detect lanes using deep learning

        Args:
            image: RGB image (H, W, 3) numpy array

        Returns:
            lane_mask: Binary mask (H, W) with lane pixels
            lane_lines: Fitted polynomial lane lines
        """
        # Preprocess
        input_tensor = self.transform(image).unsqueeze(0).to(self.device)

        # Inference
        with torch.no_grad():
            output = self.model(input_tensor)
            pred = torch.argmax(output, dim=1).squeeze().cpu().numpy()

        # Post-process: fit polynomials to predicted lane pixels
        lane_lines = self.fit_lanes_from_mask(pred)

        return pred, lane_lines

    def fit_lanes_from_mask(self, lane_mask):
        """Fit polynomial curves to segmented lane mask"""
        height, width = lane_mask.shape

        # Find lane pixels
        lane_pixels = np.where(lane_mask > 0)
        y_coords = lane_pixels[0]
        x_coords = lane_pixels[1]

        if len(y_coords) < 10:
            return {"left": None, "right": None}

        # Separate left and right lanes based on x position
        midpoint = width // 2
        left_mask = x_coords < midpoint
        right_mask = x_coords >= midpoint

        left_x = x_coords[left_mask]
        left_y = y_coords[left_mask]
        right_x = x_coords[right_mask]
        right_y = y_coords[right_mask]

        # Fit polynomials
        left_poly = np.polyfit(left_y, left_x, 2) if len(left_y) > 10 else None
        right_poly = np.polyfit(right_y, right_x, 2) if len(right_y) > 10 else None

        return {"left": left_poly, "right": right_poly}

Object Detection

YOLO v5 for Real-Time Object Detection

import torch
import cv2
import numpy as np

class YOLOv5Detector:
    """
    YOLO v5 object detector optimized for automotive applications
    """

    def __init__(self, model_path='yolov5s.pt', conf_thresh=0.5, iou_thresh=0.45):
        self.model = torch.hub.load('ultralytics/yolov5', 'custom',
                                    path=model_path, force_reload=False)
        self.model.conf = conf_thresh
        self.model.iou = iou_thresh

        # COCO class names relevant for ADAS
        self.classes_of_interest = [
            'person', 'bicycle', 'car', 'motorcycle', 'bus', 'truck',
            'traffic light', 'stop sign'
        ]

    def detect(self, image):
        """
        Detect objects in image

        Args:
            image: RGB image (H, W, 3)

        Returns:
            detections: List of Detection objects
        """
        # Run inference
        results = self.model(image)

        # Parse results
        detections = []
        for *box, conf, cls in results.xyxy[0].cpu().numpy():
            class_name = self.model.names[int(cls)]

            # Filter for ADAS-relevant objects
            if class_name in self.classes_of_interest:
                detection = {
                    'bbox': box,  # [x1, y1, x2, y2]
                    'confidence': conf,
                    'class': class_name,
                    'class_id': int(cls)
                }
                detections.append(detection)

        return detections

    def detect_with_tracking(self, image, prev_detections=None):
        """
        Detect and track objects using simple IOU matching
        """
        current_detections = self.detect(image)

        if prev_detections is None:
            # Initialize track IDs
            for i, det in enumerate(current_detections):
                det['track_id'] = i
            return current_detections

        # Match current detections with previous using IOU
        matched, unmatched_current, unmatched_prev = self.match_detections(
            current_detections, prev_detections
        )

        # Update track IDs
        next_track_id = max([d['track_id'] for d in prev_detections]) + 1

        for curr_idx, prev_idx in matched:
            current_detections[curr_idx]['track_id'] = \
                prev_detections[prev_idx]['track_id']

        for curr_idx in unmatched_current:
            current_detections[curr_idx]['track_id'] = next_track_id
            next_track_id += 1

        return current_detections

    def match_detections(self, current, previous, iou_thresh=0.3):
        """Match detections using IOU"""
        if not current or not previous:
            return [], list(range(len(current))), list(range(len(previous)))

        # Compute IOU matrix
        iou_matrix = np.zeros((len(current), len(previous)))
        for i, curr_det in enumerate(current):
            for j, prev_det in enumerate(previous):
                if curr_det['class'] == prev_det['class']:
                    iou_matrix[i, j] = self.compute_iou(
                        curr_det['bbox'], prev_det['bbox']
                    )

        # Hungarian algorithm for matching (greedy approximation)
        matched = []
        unmatched_current = list(range(len(current)))
        unmatched_prev = list(range(len(previous)))

        while iou_matrix.size > 0:
            max_iou = iou_matrix.max()
            if max_iou < iou_thresh:
                break

            curr_idx, prev_idx = np.unravel_index(iou_matrix.argmax(),
                                                  iou_matrix.shape)
            matched.append((curr_idx, prev_idx))

            unmatched_current.remove(curr_idx)
            unmatched_prev.remove(prev_idx)

            # Remove matched from matrix
            iou_matrix[curr_idx, :] = 0
            iou_matrix[:, prev_idx] = 0

        return matched, unmatched_current, unmatched_prev

    def compute_iou(self, box1, box2):
        """Compute IOU between two bounding boxes"""
        x1_min, y1_min, x1_max, y1_max = box1
        x2_min, y2_min, x2_max, y2_max = box2

        # Intersection area
        inter_xmin = max(x1_min, x2_min)
        inter_ymin = max(y1_min, y2_min)
        inter_xmax = min(x1_max, x2_max)
        inter_ymax = min(y1_max, y2_max)

        inter_area = max(0, inter_xmax - inter_xmin) * \
                    max(0, inter_ymax - inter_ymin)

        # Union area
        box1_area = (x1_max - x1_min) * (y1_max - y1_min)
        box2_area = (x2_max - x2_min) * (y2_max - y2_min)
        union_area = box1_area + box2_area - inter_area

        return inter_area / union_area if union_area > 0 else 0

Semantic Segmentation

import torch
import torch.nn as nn
from torchvision.models.segmentation import deeplabv3_resnet50

class SemanticSegmentor:
    """
    Semantic segmentation for ADAS scene understanding
    """

    def __init__(self, model_path=None, num_classes=19, device='cuda'):
        """
        Args:
            num_classes: Number of classes (Cityscapes: 19 classes)
                0: road, 1: sidewalk, 2: building, 3: wall, 4: fence,
                5: pole, 6: traffic light, 7: traffic sign, 8: vegetation,
                9: terrain, 10: sky, 11: person, 12: rider, 13: car,
                14: truck, 15: bus, 16: train, 17: motorcycle, 18: bicycle
        """
        self.device = torch.device(device if torch.cuda.is_available() else 'cpu')
        self.num_classes = num_classes

        # Load pretrained DeepLabV3
        self.model = deeplabv3_resnet50(pretrained=True)
        self.model.classifier[4] = nn.Conv2d(256, num_classes, kernel_size=1)

        if model_path:
            self.model.load_state_dict(torch.load(model_path,
                                                  map_location=self.device))

        self.model.to(self.device)
        self.model.eval()

        # Class color map (Cityscapes colors)
        self.color_map = self.create_cityscapes_colormap()

    def create_cityscapes_colormap(self):
        """Create color map for visualization"""
        colors = [
            [128, 64, 128],   # road
            [244, 35, 232],   # sidewalk
            [70, 70, 70],     # building
            [102, 102, 156],  # wall
            [190, 153, 153],  # fence
            [153, 153, 153],  # pole
            [250, 170, 30],   # traffic light
            [220, 220, 0],    # traffic sign
            [107, 142, 35],   # vegetation
            [152, 251, 152],  # terrain
            [70, 130, 180],   # sky
            [220, 20, 60],    # person
            [255, 0, 0],      # rider
            [0, 0, 142],      # car
            [0, 0, 70],       # truck
            [0, 60, 100],     # bus
            [0, 80, 100],     # train
            [0, 0, 230],      # motorcycle
            [119, 11, 32],    # bicycle
        ]
        return np.array(colors, dtype=np.uint8)

    def segment(self, image):
        """
        Perform semantic segmentation

        Args:
            image: RGB image (H, W, 3)

        Returns:
            segmentation_mask: (H, W) class labels
            colored_mask: (H, W, 3) RGB colored segmentation
        """
        # Preprocess
        input_tensor = self.preprocess(image)

        # Inference
        with torch.no_grad():
            output = self.model(input_tensor)['out']
            pred = torch.argmax(output, dim=1).squeeze().cpu().numpy()

        # Colorize for visualization
        colored_mask = self.color_map[pred]

        return pred, colored_mask

    def preprocess(self, image):
        """Preprocess image for model input"""
        # Normalize using ImageNet statistics
        mean = np.array([0.485, 0.456, 0.406])
        std = np.array([0.229, 0.224, 0.225])

        # Convert to tensor and normalize
        img = image.astype(np.float32) / 255.0
        img = (img - mean) / std
        img = torch.from_numpy(img).permute(2, 0, 1).unsqueeze(0)

        return img.to(self.device)

    def get_drivable_area(self, segmentation_mask):
        """Extract drivable area from segmentation"""
        # Drivable classes: road (0), sidewalk (1)
        drivable_mask = np.isin(segmentation_mask, [0, 1])
        return drivable_mask.astype(np.uint8)

Depth Estimation

Stereo Depth Estimation

#include <opencv2/opencv.hpp>
#include <opencv2/calib3d.hpp>

class StereoDepthEstimator {
public:
    StereoDepthEstimator(const cv::Mat& K_left, const cv::Mat& K_right,
                        const cv::Mat& R, const cv::Mat& T, float baseline)
        : K_left_(K_left), K_right_(K_right), R_(R), T_(T), baseline_(baseline)
    {
        // Create stereo matcher
        stereo_ = cv::StereoSGBM::create(
            0,                      // minDisparity
            16 * 10,                // numDisparities (must be divisible by 16)
            5,                      // blockSize
            8 * 5 * 5,              // P1
            32 * 5 * 5,             // P2
            1,                      // disp12MaxDiff
            0,                      // preFilterCap
            10,                     // uniquenessRatio
            100,                    // speckleWindowSize
            32,                     // speckleRange
            cv::StereoSGBM::MODE_SGBM_3WAY
        );

        // Compute rectification maps
        cv::Size img_size(1280, 720);  // Adjust to your camera
        cv::stereoRectify(K_left_, cv::Mat(), K_right_, cv::Mat(),
                         img_size, R_, T_, R1_, R2_, P1_, P2_, Q_);

        cv::initUndistortRectifyMap(K_left_, cv::Mat(), R1_, P1_,
                                   img_size, CV_32FC1, map_left_x_, map_left_y_);
        cv::initUndistortRectifyMap(K_right_, cv::Mat(), R2_, P2_,
                                   img_size, CV_32FC1, map_right_x_, map_right_y_);
    }

    cv::Mat compute_disparity(const cv::Mat& left_img, const cv::Mat& right_img) {
        // Rectify images
        cv::Mat left_rect, right_rect;
        cv::remap(left_img, left_rect, map_left_x_, map_left_y_, cv::INTER_LINEAR);
        cv::remap(right_img, right_rect, map_right_x_, map_right_y_, cv::INTER_LINEAR);

        // Convert to grayscale
        cv::Mat left_gray, right_gray;
        cv::cvtColor(left_rect, left_gray, cv::COLOR_BGR2GRAY);
        cv::cvtColor(right_rect, right_gray, cv::COLOR_BGR2GRAY);

        // Compute disparity
        cv::Mat disparity;
        stereo_->compute(left_gray, right_gray, disparity);

        // Convert to float and normalize
        disparity.convertTo(disparity, CV_32F, 1.0 / 16.0);

        return disparity;
    }

    cv::Mat disparity_to_depth(const cv::Mat& disparity) {
        // depth = (focal_length * baseline) / disparity
        cv::Mat depth;
        float focal_length = K_left_.at<double>(0, 0);

        depth = (focal_length * baseline_) / disparity;

        // Clip unrealistic depths
        cv::threshold(depth, depth, 0.1, 100.0, cv::THRESH_TOZERO);
        cv::threshold(depth, depth, 100.0, 100.0, cv::THRESH_TRUNC);

        return depth;
    }

    cv::Mat compute_point_cloud(const cv::Mat& disparity) {
        // Reproject to 3D using Q matrix
        cv::Mat points_3d;
        cv::reprojectImageTo3D(disparity, points_3d, Q_, true);

        return points_3d;
    }

private:
    cv::Mat K_left_, K_right_;  // Intrinsic matrices
    cv::Mat R_, T_;              // Extrinsic: rotation and translation
    cv::Mat R1_, R2_, P1_, P2_, Q_;  // Rectification matrices
    cv::Mat map_left_x_, map_left_y_, map_right_x_, map_right_y_;
    float baseline_;
    cv::Ptr<cv::StereoSGBM> stereo_;
};

Monocular Depth Estimation (Deep Learning)

import torch
import torch.nn as nn

class MonoDepthNet(nn.Module):
    """
    Monocular depth estimation network based on encoder-decoder architecture
    """

    def __init__(self, encoder='resnet50'):
        super(MonoDepthNet, self).__init__()

        # Encoder (pretrained ResNet)
        if encoder == 'resnet50':
            resnet = torch.hub.load('pytorch/vision:v0.10.0', 'resnet50',
                                   pretrained=True)
            self.encoder = nn.ModuleList([
                nn.Sequential(resnet.conv1, resnet.bn1, resnet.relu, resnet.maxpool),
                resnet.layer1,
                resnet.layer2,
                resnet.layer3,
                resnet.layer4
            ])
            encoder_channels = [64, 256, 512, 1024, 2048]

        # Decoder
        self.decoder = nn.ModuleList([
            self.upconv_block(2048, 1024),
            self.upconv_block(1024 + 1024, 512),
            self.upconv_block(512 + 512, 256),
            self.upconv_block(256 + 256, 128),
            self.upconv_block(128 + 64, 64)
        ])

        # Output layer
        self.output_conv = nn.Sequential(
            nn.Conv2d(64, 32, 3, padding=1),
            nn.ReLU(inplace=True),
            nn.Conv2d(32, 1, 1),
            nn.Sigmoid()  # Output in range [0, 1]
        )

    def upconv_block(self, in_channels, out_channels):
        return nn.Sequential(
            nn.ConvTranspose2d(in_channels, out_channels, 3, stride=2, padding=1,
                             output_padding=1),
            nn.BatchNorm2d(out_channels),
            nn.ReLU(inplace=True),
            nn.Conv2d(out_channels, out_channels, 3, padding=1),
            nn.BatchNorm2d(out_channels),
            nn.ReLU(inplace=True)
        )

    def forward(self, x):
        # Encoder with skip connections
        skip_connections = []
        for layer in self.encoder:
            x = layer(x)
            skip_connections.append(x)

        # Decoder
        x = skip_connections[-1]
        for i, decoder_layer in enumerate(self.decoder):
            x = decoder_layer(x)
            if i < len(skip_connections) - 1:
                # Concatenate with skip connection
                x = torch.cat([x, skip_connections[-(i + 2)]], dim=1)

        # Output depth map
        depth = self.output_conv(x)

        return depth

ISP Tuning

HDR and Low-Light Enhancement

#include <opencv2/opencv.hpp>

class ISPProcessor {
public:
    cv::Mat process_hdr(const std::vector<cv::Mat>& exposures) {
        // Multi-exposure HDR fusion
        cv::Ptr<cv::MergeDebevec> merge = cv::createMergeDebevec();

        std::vector<float> exposure_times;
        for (size_t i = 0; i < exposures.size(); ++i) {
            exposure_times.push_back(std::pow(2.0f, i - 1));  // -1, 0, +1 EV
        }

        cv::Mat hdr;
        merge->process(exposures, hdr, exposure_times, cv::Mat());

        // Tone mapping
        cv::Ptr<cv::TonemapDrago> tonemap = cv::createTonemapDrago(2.2f);
        cv::Mat ldr;
        tonemap->process(hdr, ldr);

        // Convert to 8-bit
        ldr = ldr * 255.0;
        ldr.convertTo(ldr, CV_8UC3);

        return ldr;
    }

    cv::Mat enhance_low_light(const cv::Mat& image) {
        // CLAHE (Contrast Limited Adaptive Histogram Equalization)
        cv::Mat lab;
        cv::cvtColor(image, lab, cv::COLOR_BGR2Lab);

        std::vector<cv::Mat> lab_planes;
        cv::split(lab, lab_planes);

        // Apply CLAHE to L channel
        cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
        clahe->apply(lab_planes[0], lab_planes[0]);

        cv::merge(lab_planes, lab);
        cv::Mat enhanced;
        cv::cvtColor(lab, enhanced, cv::COLOR_Lab2BGR);

        return enhanced;
    }

    cv::Mat denoise(const cv::Mat& image) {
        // Fast non-local means denoising
        cv::Mat denoised;
        cv::fastNlMeansDenoisingColored(image, denoised, 10, 10, 7, 21);
        return denoised;
    }
};

Performance Optimization

TensorRT Optimization for NVIDIA Platforms

import tensorrt as trt
import pycuda.driver as cuda
import numpy as np

class TensorRTInference:
    """
    Optimize and run deep learning models with TensorRT
    """

    def __init__(self, onnx_path, fp16_mode=True):
        self.logger = trt.Logger(trt.Logger.WARNING)
        self.engine = self.build_engine(onnx_path, fp16_mode)
        self.context = self.engine.create_execution_context()

        # Allocate buffers
        self.inputs, self.outputs, self.bindings = self.allocate_buffers()

    def build_engine(self, onnx_path, fp16_mode):
        """Build TensorRT engine from ONNX model"""
        builder = trt.Builder(self.logger)
        network = builder.create_network(
            1 << int(trt.NetworkDefinitionCreationFlag.EXPLICIT_BATCH)
        )
        parser = trt.OnnxParser(network, self.logger)

        # Parse ONNX
        with open(onnx_path, 'rb') as model:
            parser.parse(model.read())

        # Builder config
        config = builder.create_builder_config()
        config.max_workspace_size = 1 << 30  # 1GB

        if fp16_mode:
            config.set_flag(trt.BuilderFlag.FP16)

        # Build engine
        engine = builder.build_engine(network, config)
        return engine

    def allocate_buffers(self):
        inputs = []
        outputs = []
        bindings = []

        for binding in self.engine:
            size = trt.volume(self.engine.get_binding_shape(binding))
            dtype = trt.nptype(self.engine.get_binding_dtype(binding))

            # Allocate host and device buffers
            host_mem = cuda.pagelocked_empty(size, dtype)
            device_mem = cuda.mem_alloc(host_mem.nbytes)

            bindings.append(int(device_mem))

            if self.engine.binding_is_input(binding):
                inputs.append({'host': host_mem, 'device': device_mem})
            else:
                outputs.append({'host': host_mem, 'device': device_mem})

        return inputs, outputs, bindings

    def infer(self, input_data):
        """Run inference"""
        # Copy input to device
        np.copyto(self.inputs[0]['host'], input_data.ravel())
        cuda.memcpy_htod(self.inputs[0]['device'], self.inputs[0]['host'])

        # Run inference
        self.context.execute_v2(bindings=self.bindings)

        # Copy output to host
        cuda.memcpy_dtoh(self.outputs[0]['host'], self.outputs[0]['device'])

        return self.outputs[0]['host']

Standards & Safety

• ISO 26262: ASIL B-D depending on function (lane keeping: ASIL B, AEB: ASIL D)
• ISO 21448 (SOTIF): Validation for lighting conditions, occlusions
• MISRA C++: Code quality for production deployment

Performance Targets

• Lane Detection: 30 FPS @ 1280x720
• Object Detection: 30 FPS @ 1920x1080 (YOLOv5s on GPU)
• Semantic Segmentation: 20 FPS @ 1280x720 (DeepLabV3)
• Latency: < 50ms camera-to-decision

Related Skills

• sensor-fusion-perception.md
• radar-lidar-processing.md
• adas-features-implementation.md

---

Hd Maps Localization

HD Maps & Localization for ADAS

Overview