diff --git a/VisionPilot/Production_Releases/0.5/CMakeLists.txt b/VisionPilot/Production_Releases/0.5/CMakeLists.txt index 8ae21c0da..932680190 100644 --- a/VisionPilot/Production_Releases/0.5/CMakeLists.txt +++ b/VisionPilot/Production_Releases/0.5/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.16) -project(autosteer_pipeline LANGUAGES CXX) +project(egolanes_pipeline LANGUAGES CXX) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) @@ -39,38 +39,39 @@ include_directories( ${EIGEN3_INCLUDE_DIR} ) -# AutoSteer inference backend -add_library(autosteer_inference +# EgoLanes inference backend +add_library(egolanes_inference src/inference/onnxruntime_session.cpp src/inference/onnxruntime_engine.cpp + src/inference/autosteer_engine.cpp ) -target_compile_definitions(autosteer_inference PRIVATE LOG_TYPE=0) # Use non-ROS logging -target_link_libraries(autosteer_inference +target_compile_definitions(egolanes_inference PRIVATE LOG_TYPE=0) # Use non-ROS logging +target_link_libraries(egolanes_inference ${OpenCV_LIBS} ${ONNXRUNTIME_LIBRARY} ) # Add RPATH so executable can find libonnxruntime.so at runtime -set_target_properties(autosteer_inference PROPERTIES +set_target_properties(egolanes_inference PROPERTIES BUILD_RPATH ${ONNXRUNTIME_LIB_DIR} INSTALL_RPATH ${ONNXRUNTIME_LIB_DIR} ) # Visualization module -add_library(autosteer_visualization +add_library(egolanes_visualization src/visualization/draw_lanes.cpp ) -target_compile_definitions(autosteer_visualization PRIVATE LOG_TYPE=0) # Use non-ROS logging -target_link_libraries(autosteer_visualization - autosteer_inference +target_compile_definitions(egolanes_visualization PRIVATE LOG_TYPE=0) # Use non-ROS logging +target_link_libraries(egolanes_visualization + egolanes_inference ${OpenCV_LIBS} ) # Lane filtering module -add_library(autosteer_lane_filtering +add_library(egolanes_lane_filtering src/lane_filtering/lane_filter.cpp ) -target_link_libraries(autosteer_lane_filtering - autosteer_inference +target_link_libraries(egolanes_lane_filtering + egolanes_inference ${OpenCV_LIBS} ) @@ -135,8 +136,8 @@ if(ENABLE_RERUN) src/rerun/rerun_logger.cpp ) target_compile_definitions(rerun_logger PRIVATE ENABLE_RERUN) - target_link_libraries(rerun_logger - autosteer_inference +target_link_libraries(rerun_logger + egolanes_inference ${OpenCV_LIBS} rerun_sdk ) @@ -152,26 +153,26 @@ if(NOT RERUN_ENABLED) endif() # Lane tracking module -add_library(autosteer_lane_tracking +add_library(egolanes_lane_tracking src/lane_tracking/lane_tracking.cpp ) -target_link_libraries(autosteer_lane_tracking - autosteer_inference +target_link_libraries(egolanes_lane_tracking + egolanes_inference ${OpenCV_LIBS} ) # Main executable -add_executable(autosteer_pipeline +add_executable(egolanes_pipeline main.cpp ) -target_compile_definitions(autosteer_pipeline PRIVATE LOG_TYPE=0) # Use non-ROS logging +target_compile_definitions(egolanes_pipeline PRIVATE LOG_TYPE=0) # Use non-ROS logging # Link libraries -set(AUTOSTEER_LIBS - autosteer_inference - autosteer_visualization - autosteer_lane_filtering - autosteer_lane_tracking +set(EGOLANES_LIBS + egolanes_inference + egolanes_visualization + egolanes_lane_filtering + egolanes_lane_tracking camera_utils path_planning steering_control @@ -180,14 +181,21 @@ set(AUTOSTEER_LIBS ) if(ENABLE_RERUN AND RERUN_ENABLED) - target_compile_definitions(autosteer_pipeline PRIVATE ENABLE_RERUN) - list(APPEND AUTOSTEER_LIBS rerun_logger) + target_compile_definitions(egolanes_pipeline PRIVATE ENABLE_RERUN) + list(APPEND EGOLANES_LIBS rerun_logger) endif() -target_link_libraries(autosteer_pipeline ${AUTOSTEER_LIBS}) +target_link_libraries(egolanes_pipeline ${EGOLANES_LIBS}) + +# Test executable for AutoSteer +add_executable(test_autosteer + test/test_autosteer.cpp +) +target_compile_definitions(test_autosteer PRIVATE LOG_TYPE=0) +target_link_libraries(test_autosteer ${EGOLANES_LIBS}) # Installation -install(TARGETS autosteer_pipeline +install(TARGETS egolanes_pipeline RUNTIME DESTINATION bin ) diff --git a/VisionPilot/Production_Releases/0.5/README.md b/VisionPilot/Production_Releases/0.5/README.md index d601446c8..96716855c 100644 --- a/VisionPilot/Production_Releases/0.5/README.md +++ b/VisionPilot/Production_Releases/0.5/README.md @@ -1,4 +1,4 @@ -# VisionPilot 0.5 - AutoSteer Production Release +# VisionPilot 0.5 - EgoLanes Production Release This release enables autonomous steering using the EgoLanes neural network to detect lane lines and navigate roads at a predetermined, desired speed. diff --git a/VisionPilot/Production_Releases/0.5/include/inference/autosteer_engine.hpp b/VisionPilot/Production_Releases/0.5/include/inference/autosteer_engine.hpp new file mode 100644 index 000000000..b3eb2cb33 --- /dev/null +++ b/VisionPilot/Production_Releases/0.5/include/inference/autosteer_engine.hpp @@ -0,0 +1,104 @@ +#ifndef AUTOWARE_POV_VISION_EGOLANES_AUTOSTEER_ENGINE_HPP_ +#define AUTOWARE_POV_VISION_EGOLANES_AUTOSTEER_ENGINE_HPP_ + +#include +#include +#include +#include + +namespace autoware_pov::vision::egolanes +{ + +/** + * @brief AutoSteer ONNX Runtime Inference Engine + * + * Takes concatenated EgoLanes raw outputs from two consecutive frames + * and predicts steering angle. + * + * Input: [1, 6, 80, 160] - concatenated (t-1, t) EgoLanes outputs + * Output: Steering angle (degrees) + * + * Supports CPU and TensorRT execution providers. + */ +class AutoSteerOnnxEngine +{ +public: + /** + * @brief Constructor + * + * @param model_path Path to ONNX model (.onnx file) + * @param provider Execution provider: "cpu" or "tensorrt" + * @param precision Precision mode: "fp32" or "fp16" (TensorRT only) + * @param device_id GPU device ID (TensorRT only, default: 0) + * @param cache_dir TensorRT engine cache directory (default: ./trt_cache) + */ + AutoSteerOnnxEngine( + const std::string& model_path, + const std::string& provider = "cpu", + const std::string& precision = "fp32", + int device_id = 0, + const std::string& cache_dir = "./trt_cache" + ); + + ~AutoSteerOnnxEngine(); + + /** + * @brief Run AutoSteer inference + * + * @param concat_input Concatenated tensor [1, 6, 80, 160] + * = [t-1 EgoLanes output, t EgoLanes output] + * @return Steering angle in degrees (-30 to +30) + */ + float inference(const std::vector& concat_input); + + /** + * @brief Get model input dimensions + */ + int getInputChannels() const { return model_input_channels_; } + int getInputHeight() const { return model_input_height_; } + int getInputWidth() const { return model_input_width_; } + + /** + * @brief Get raw output logits for debugging + * @return Vector of 61 logit values (one per steering class) + */ + std::vector getRawOutputLogits() const; + +private: + /** + * @brief Run ONNX Runtime inference + */ + bool doInference(const std::vector& input_buffer); + + /** + * @brief Post-process raw output to steering angle + * + * Applies argmax and converts to degrees: argmax(logits) - 30 + */ + float postProcess(); + + // ONNX Runtime components + std::unique_ptr session_; + std::unique_ptr memory_info_; + + // Input/Output tensor names (storage + pointers) + std::string input_name_storage_; + std::string output_name_storage_; // Kept for backward compatibility (points to second output) + std::vector output_names_storage_; // Stores both output names + std::vector input_names_; + std::vector output_names_; + + // Model dimensions + int model_input_channels_; // 6 (two 3-channel EgoLanes outputs) + int model_input_height_; // 80 + int model_input_width_; // 160 + int model_output_classes_; // 61 (steering classes: -30 to +30) + + // Output buffer + std::vector output_tensors_; +}; + +} // namespace autoware_pov::vision::egolanes + +#endif // AUTOWARE_POV_VISION_EGOLANES_AUTOSTEER_ENGINE_HPP_ + diff --git a/VisionPilot/Production_Releases/0.5/include/inference/onnxruntime_engine.hpp b/VisionPilot/Production_Releases/0.5/include/inference/onnxruntime_engine.hpp index 53936ea72..bc9a3fed3 100644 --- a/VisionPilot/Production_Releases/0.5/include/inference/onnxruntime_engine.hpp +++ b/VisionPilot/Production_Releases/0.5/include/inference/onnxruntime_engine.hpp @@ -1,5 +1,5 @@ -#ifndef AUTOWARE_POV_VISION_AUTOSTEER_ONNXRUNTIME_ENGINE_HPP_ -#define AUTOWARE_POV_VISION_AUTOSTEER_ONNXRUNTIME_ENGINE_HPP_ +#ifndef AUTOWARE_POV_VISION_EGOLANES_ONNXRUNTIME_ENGINE_HPP_ +#define AUTOWARE_POV_VISION_EGOLANES_ONNXRUNTIME_ENGINE_HPP_ #include #include @@ -8,7 +8,7 @@ #include #include -namespace autoware_pov::vision::autosteer +namespace autoware_pov::vision::egolanes { /** @@ -48,7 +48,7 @@ struct LaneSegmentation }; /** - * @brief AutoSteer ONNX Runtime Inference Engine + * @brief EgoLanes ONNX Runtime Inference Engine * * Supports multiple execution providers: * - CPU: Default CPU execution @@ -59,7 +59,7 @@ struct LaneSegmentation * 2. Model inference via ONNX Runtime * 3. Post-processing (thresholding, channel extraction) */ -class AutoSteerOnnxEngine +class EgoLanesOnnxEngine { public: /** @@ -71,7 +71,7 @@ class AutoSteerOnnxEngine * @param device_id GPU device ID (TensorRT only, default: 0) * @param cache_dir TensorRT engine cache directory (default: ./trt_cache) */ - AutoSteerOnnxEngine( + EgoLanesOnnxEngine( const std::string& model_path, const std::string& provider = "cpu", const std::string& precision = "fp32", @@ -79,7 +79,7 @@ class AutoSteerOnnxEngine const std::string& cache_dir = "./trt_cache" ); - ~AutoSteerOnnxEngine(); + ~EgoLanesOnnxEngine(); /** * @brief Run complete inference pipeline @@ -105,7 +105,7 @@ class AutoSteerOnnxEngine */ std::vector getTensorShape() const; - // Model input dimensions (320x640 for AutoSteer) + // Model input dimensions (320x640 for EgoLanes) int getInputWidth() const { return model_input_width_; } int getInputHeight() const { return model_input_height_; } @@ -115,11 +115,11 @@ class AutoSteerOnnxEngine private: /** - * @brief Preprocess image for AutoSteer + * @brief Preprocess image for EgoLanes * * Resizes to 320x640, converts to RGB, normalizes with ImageNet stats, converts to CHW */ - void preprocessAutoSteer(const cv::Mat& input_image, float* buffer); + void preprocessEgoLanes(const cv::Mat& input_image, float* buffer); /** * @brief Run ONNX Runtime inference @@ -159,7 +159,7 @@ class AutoSteerOnnxEngine static constexpr std::array STD = {0.229f, 0.224f, 0.225f}; }; -} // namespace autoware_pov::vision::autosteer +} // namespace autoware_pov::vision::egolanes -#endif // AUTOWARE_POV_VISION_AUTOSTEER_ONNXRUNTIME_ENGINE_HPP_ +#endif // AUTOWARE_POV_VISION_EGOLANES_ONNXRUNTIME_ENGINE_HPP_ diff --git a/VisionPilot/Production_Releases/0.5/include/inference/onnxruntime_session.hpp b/VisionPilot/Production_Releases/0.5/include/inference/onnxruntime_session.hpp index a5e8cd42d..1f88d7a3d 100644 --- a/VisionPilot/Production_Releases/0.5/include/inference/onnxruntime_session.hpp +++ b/VisionPilot/Production_Releases/0.5/include/inference/onnxruntime_session.hpp @@ -1,11 +1,11 @@ -#ifndef AUTOWARE_POV_VISION_AUTOSTEER_ONNXRUNTIME_SESSION_HPP_ -#define AUTOWARE_POV_VISION_AUTOSTEER_ONNXRUNTIME_SESSION_HPP_ +#ifndef AUTOWARE_POV_VISION_EGOLANES_ONNXRUNTIME_SESSION_HPP_ +#define AUTOWARE_POV_VISION_EGOLANES_ONNXRUNTIME_SESSION_HPP_ #include #include #include -namespace autoware_pov::vision::autosteer +namespace autoware_pov::vision::egolanes { /** @@ -26,6 +26,8 @@ class OnnxRuntimeSessionFactory * @param precision "fp32" or "fp16" (only for TensorRT) * @param device_id GPU device ID (only for TensorRT) * @param cache_dir Directory for TensorRT engine cache (optional) + * @param workspace_size_gb TensorRT workspace size in GB (default: 1GB) + * @param cache_prefix Prefix for TensorRT engine cache files (default: "egolanes_") * @return Unique pointer to created session */ static std::unique_ptr createSession( @@ -33,7 +35,9 @@ class OnnxRuntimeSessionFactory const std::string& provider, const std::string& precision = "fp16", int device_id = 0, - const std::string& cache_dir = "./trt_cache" + const std::string& cache_dir = "./trt_cache", + double workspace_size_gb = 1.0, + const std::string& cache_prefix = "egolanes_" ); private: @@ -51,7 +55,7 @@ class OnnxRuntimeSessionFactory * Enables: * - Engine caching for fast startup * - FP16/FP32 precision - * - Optimal workspace size + * - Configurable workspace size * - Timing cache */ static std::unique_ptr createTensorRTSession( @@ -59,13 +63,15 @@ class OnnxRuntimeSessionFactory const std::string& model_path, const std::string& precision, int device_id, - const std::string& cache_dir + const std::string& cache_dir, + double workspace_size_gb, + const std::string& cache_prefix ); }; -} // namespace autoware_pov::vision::autosteer +} // namespace autoware_pov::vision::egolanes -#endif // AUTOWARE_POV_VISION_AUTOSTEER_ONNXRUNTIME_SESSION_HPP_ +#endif // AUTOWARE_POV_VISION_EGOLANES_ONNXRUNTIME_SESSION_HPP_ diff --git a/VisionPilot/Production_Releases/0.5/include/lane_filtering/lane_filter.hpp b/VisionPilot/Production_Releases/0.5/include/lane_filtering/lane_filter.hpp index 3d80da24a..2ddfac7ca 100644 --- a/VisionPilot/Production_Releases/0.5/include/lane_filtering/lane_filter.hpp +++ b/VisionPilot/Production_Releases/0.5/include/lane_filtering/lane_filter.hpp @@ -3,7 +3,7 @@ #include #include -namespace autoware_pov::vision::autosteer +namespace autoware_pov::vision::egolanes { /** @@ -103,4 +103,4 @@ class LaneFilter { float smoothing_factor; }; -} // namespace autoware_pov::vision::autosteer \ No newline at end of file +} // namespace autoware_pov::vision::egolanes \ No newline at end of file diff --git a/VisionPilot/Production_Releases/0.5/include/lane_tracking/lane_tracking.hpp b/VisionPilot/Production_Releases/0.5/include/lane_tracking/lane_tracking.hpp index d1b52082e..b91a3b0eb 100644 --- a/VisionPilot/Production_Releases/0.5/include/lane_tracking/lane_tracking.hpp +++ b/VisionPilot/Production_Releases/0.5/include/lane_tracking/lane_tracking.hpp @@ -1,11 +1,11 @@ -#ifndef AUTOWARE_POV_VISION_AUTOSTEER_LANE_TRACKING_HPP_ -#define AUTOWARE_POV_VISION_AUTOSTEER_LANE_TRACKING_HPP_ +#ifndef AUTOWARE_POV_VISION_EGOLANES_LANE_TRACKING_HPP_ +#define AUTOWARE_POV_VISION_EGOLANES_LANE_TRACKING_HPP_ #include "inference/onnxruntime_engine.hpp" #include #include -namespace autoware_pov::vision::autosteer +namespace autoware_pov::vision::egolanes { // BEV-specific visualization data @@ -128,6 +128,6 @@ class LaneTracker { }; -} // namespace autoware_pov::vision::autosteer +} // namespace autoware_pov::vision::egolanes -#endif // AUTOWARE_POV_VISION_AUTOSTEER_LANE_TRACKING_HPP_ \ No newline at end of file +#endif // AUTOWARE_POV_VISION_EGOLANES_LANE_TRACKING_HPP_ \ No newline at end of file diff --git a/VisionPilot/Production_Releases/0.5/include/path_planning/path_finder.hpp b/VisionPilot/Production_Releases/0.5/include/path_planning/path_finder.hpp index 283802ba0..b0f790ed4 100644 --- a/VisionPilot/Production_Releases/0.5/include/path_planning/path_finder.hpp +++ b/VisionPilot/Production_Releases/0.5/include/path_planning/path_finder.hpp @@ -13,6 +13,7 @@ #include #include #include +#include namespace autoware_pov::vision::path_planning { @@ -76,11 +77,13 @@ class PathFinder * * @param left_pts_bev Left lane points in BEV meters (x=lateral, y=longitudinal) * @param right_pts_bev Right lane points in BEV meters + * @param autosteer_steering_deg Optional AutoSteer steering angle in degrees (replaces computed curvature) * @return PathFinder output (fused metrics + individual curves) */ PathFinderOutput update( const std::vector& left_pts_bev, - const std::vector& right_pts_bev); + const std::vector& right_pts_bev, + double autosteer_steering_rad = std::numeric_limits::quiet_NaN()); /** * @brief Get current tracked state diff --git a/VisionPilot/Production_Releases/0.5/include/path_planning/poly_fit.hpp b/VisionPilot/Production_Releases/0.5/include/path_planning/poly_fit.hpp index 304540015..5efb76458 100644 --- a/VisionPilot/Production_Releases/0.5/include/path_planning/poly_fit.hpp +++ b/VisionPilot/Production_Releases/0.5/include/path_planning/poly_fit.hpp @@ -41,16 +41,5 @@ struct FittedCurve */ std::array fitQuadPoly(const std::vector &points); -/** - * @brief Calculate center lane from left and right lanes - * - * Averages the polynomial coefficients - * - * @param left_lane Left lane curve - * @param right_lane Right lane curve - * @return Center lane curve - */ -FittedCurve calculateEgoPath(const FittedCurve &left_lane, const FittedCurve &right_lane); - } // namespace autoware_pov::vision::path_planning diff --git a/VisionPilot/Production_Releases/0.5/include/rerun/rerun_logger.hpp b/VisionPilot/Production_Releases/0.5/include/rerun/rerun_logger.hpp index 6ef3983e6..fdb7c5f77 100644 --- a/VisionPilot/Production_Releases/0.5/include/rerun/rerun_logger.hpp +++ b/VisionPilot/Production_Releases/0.5/include/rerun/rerun_logger.hpp @@ -33,7 +33,7 @@ class RerunLogger { * @param spawn_viewer If true, spawn viewer; if false, save to file * @param save_path Path to .rrd file (used if !spawn_viewer) */ - RerunLogger(const std::string& app_id = "AutoSteer", + RerunLogger(const std::string& app_id = "EgoLanes", bool spawn_viewer = true, const std::string& save_path = ""); @@ -50,8 +50,8 @@ class RerunLogger { void logInference( int frame_number, const cv::Mat& input_frame, - const autoware_pov::vision::autosteer::LaneSegmentation& raw_lanes, - const autoware_pov::vision::autosteer::LaneSegmentation& filtered_lanes, + const autoware_pov::vision::egolanes::LaneSegmentation& raw_lanes, + const autoware_pov::vision::egolanes::LaneSegmentation& filtered_lanes, long inference_time_us); /** diff --git a/VisionPilot/Production_Releases/0.5/include/visualization/draw_lanes.hpp b/VisionPilot/Production_Releases/0.5/include/visualization/draw_lanes.hpp index 6abbdfca0..4f1740504 100644 --- a/VisionPilot/Production_Releases/0.5/include/visualization/draw_lanes.hpp +++ b/VisionPilot/Production_Releases/0.5/include/visualization/draw_lanes.hpp @@ -1,11 +1,11 @@ -#ifndef AUTOWARE_POV_VISION_AUTOSTEER_DRAW_LANES_HPP_ -#define AUTOWARE_POV_VISION_AUTOSTEER_DRAW_LANES_HPP_ +#ifndef AUTOWARE_POV_VISION_EGOLANES_DRAW_LANES_HPP_ +#define AUTOWARE_POV_VISION_EGOLANES_DRAW_LANES_HPP_ #include "../inference/onnxruntime_engine.hpp" #include "../lane_tracking/lane_tracking.hpp" #include -namespace autoware_pov::vision::autosteer +namespace autoware_pov::vision::egolanes { /** @@ -87,6 +87,6 @@ void drawMetricVerification( const std::vector& right_metric_coeffs ); -} // namespace autoware_pov::vision::autosteer +} // namespace autoware_pov::vision::egolanes -#endif // AUTOWARE_POV_VISION_AUTOSTEER_DRAW_LANES_HPP_ \ No newline at end of file +#endif // AUTOWARE_POV_VISION_EGOLANES_DRAW_LANES_HPP_ \ No newline at end of file diff --git a/VisionPilot/Production_Releases/0.5/main.cpp b/VisionPilot/Production_Releases/0.5/main.cpp index 29741ff15..4941763ae 100644 --- a/VisionPilot/Production_Releases/0.5/main.cpp +++ b/VisionPilot/Production_Releases/0.5/main.cpp @@ -1,6 +1,6 @@ /** * @file main.cpp - * @brief Multi-threaded AutoSteer lane detection inference pipeline + * @brief Multi-threaded EgoLanes lane detection inference pipeline * * Architecture: * - Capture Thread: Reads frames from video source or camera @@ -9,6 +9,7 @@ */ #include "inference/onnxruntime_engine.hpp" +#include "inference/autosteer_engine.hpp" #include "visualization/draw_lanes.hpp" #include "lane_filtering/lane_filter.hpp" #include "lane_tracking/lane_tracking.hpp" @@ -30,11 +31,12 @@ #include #include #include + #include #ifndef M_PI #define M_PI 3.14159265358979323846 #endif -using namespace autoware_pov::vision::autosteer; +using namespace autoware_pov::vision::egolanes; using namespace autoware_pov::vision::camera; using namespace autoware_pov::vision::path_planning; using namespace autoware_pov::vision::steering_control; @@ -116,8 +118,10 @@ struct InferenceResult { int frame_number; steady_clock::time_point capture_time; steady_clock::time_point inference_time; - double steering_angle = 0.0; // Steering angle from controller (radians) + double steering_angle = 0.0; // Steering angle from PID controller (radians) PathFinderOutput path_output; // Added for metric debug + float autosteer_angle = 0.0f; // Steering angle from AutoSteer (degrees) + bool autosteer_valid = false; // Whether AutoSteer ran (skips first frame) }; // Performance metrics @@ -255,14 +259,15 @@ void captureThread( * @brief Inference thread - runs lane detection model */ void inferenceThread( - AutoSteerOnnxEngine& engine, + EgoLanesOnnxEngine& engine, ThreadSafeQueue& input_queue, ThreadSafeQueue& output_queue, PerformanceMetrics& metrics, std::atomic& running, float threshold, PathFinder* path_finder = nullptr, - SteeringController* steering_controller = nullptr + SteeringController* steering_controller = nullptr, + AutoSteerOnnxEngine* autosteer_engine = nullptr #ifdef ENABLE_RERUN , autoware_pov::vision::rerun_integration::RerunLogger* rerun_logger = nullptr #endif @@ -274,8 +279,19 @@ void inferenceThread( // Init lane tracker LaneTracker lane_tracker; - //Buffer for last 2 frames' - boost::circular_buffer image_buffer(2); + // Buffer for last 2 frames (for temporal models) + boost::circular_buffer image_buffer(2); + + // AutoSteer: Circular buffer for raw EgoLanes tensors [1, 3, 80, 160] + // Stores copies of last 2 frames for temporal inference + const int EGOLANES_TENSOR_SIZE = 3 * 80 * 160; // 38,400 floats per frame + boost::circular_buffer> egolanes_tensor_buffer(2); + + // Pre-allocated concatenation buffer for AutoSteer input [1, 6, 80, 160] + std::vector autosteer_input_buffer; + if (autosteer_engine != nullptr) { + autosteer_input_buffer.resize(6 * 80 * 160); // 76,800 floats + } while (running.load()) { @@ -297,10 +313,36 @@ void inferenceThread( // Run Ego Lanes inference LaneSegmentation raw_lanes = engine.inference(tf.frame, threshold); - // if( image_buffer.full()) { - // // Do inference on AutoSteer with 2 frames (current + previous) - // continue; - // } + // ======================================== + // AUTOSTEER INTEGRATION + // ======================================== + float autosteer_steering = 0.0f; + + if (autosteer_engine != nullptr) { + // 1. Copy raw EgoLanes tensor [1, 3, 80, 160] for temporal buffer + const float* raw_tensor = engine.getRawTensorData(); + std::vector current_tensor(EGOLANES_TENSOR_SIZE); + std::memcpy(current_tensor.data(), raw_tensor, EGOLANES_TENSOR_SIZE * sizeof(float)); + + // 2. Store in circular buffer (auto-deletes oldest when full) + egolanes_tensor_buffer.push_back(std::move(current_tensor)); + + // 3. Run AutoSteer only when buffer is full (skip first frame) + if (egolanes_tensor_buffer.full()) { + // Concatenate t-1 and t into pre-allocated buffer + std::memcpy(autosteer_input_buffer.data(), + egolanes_tensor_buffer[0].data(), // t-1 + EGOLANES_TENSOR_SIZE * sizeof(float)); + + std::memcpy(autosteer_input_buffer.data() + EGOLANES_TENSOR_SIZE, + egolanes_tensor_buffer[1].data(), // t + EGOLANES_TENSOR_SIZE * sizeof(float)); + + // Run AutoSteer inference + autosteer_steering = autosteer_engine->inference(autosteer_input_buffer); + } + } + // ======================================== // Post-processing with lane filter LaneSegmentation filtered_lanes = lane_filter.update(raw_lanes); @@ -324,7 +366,7 @@ void inferenceThread( // ======================================== // PATHFINDER (Polynomial Fitting + Bayes Filter) + STEERING CONTROL // ======================================== - double steering_angle = 0.0; // Initialize steering angle + double steering_angle = 0.0; // Initialize steering angle (PID controller output) PathFinderOutput path_output; // Declare at higher scope for result storage path_output.fused_valid = false; // Initialize as invalid @@ -340,7 +382,13 @@ void inferenceThread( std::vector right_bev_meters = transformPixelsToMeters(right_bev_pixels); // 3. Update PathFinder (polynomial fit + Bayes filter in metric space) - path_output = path_finder->update(left_bev_meters, right_bev_meters); + // Pass AutoSteer steering angle if available (replaces computed curvature) + // Convert AutoSteer from degrees to radians for controller + // AutoSteer outputs steering angle in degrees, controller expects radians + double autosteer_input_rad = (autosteer_engine != nullptr && egolanes_tensor_buffer.full()) + ? (autosteer_steering * M_PI / 180.0) // Convert degrees to radians + : std::numeric_limits::quiet_NaN(); + path_output = path_finder->update(left_bev_meters, right_bev_meters, autosteer_input_rad); // 4. Compute steering angle (if controller available) if (steering_controller != nullptr && path_output.fused_valid) { @@ -353,26 +401,40 @@ void inferenceThread( // 5. Print output (cross-track error, yaw error, curvature, lane width + variances + steering) if (path_output.fused_valid) { - std::cout << "[PathFinder Frame " << tf.frame_number << "] " + std::cout << "[Frame " << tf.frame_number << "] " << "CTE: " << std::fixed << std::setprecision(3) << path_output.cte << " m " << "(var: " << path_output.cte_variance << "), " << "Yaw: " << path_output.yaw_error << " rad " << "(var: " << path_output.yaw_variance << "), " - << "Curvature: " << path_output.curvature << " 1/m " + << "Curv: " << path_output.curvature << " 1/m " << "(var: " << path_output.curv_variance << "), " << "Width: " << path_output.lane_width << " m " << "(var: " << path_output.lane_width_variance << ")"; + // PID Steering output (if controller is enabled) if (steering_controller != nullptr) { - std::cout << ", Steering: " << std::setprecision(4) << steering_angle << " rad " - << "(" << (steering_angle * 180.0 / M_PI) << " deg)"; + double pid_deg = steering_angle * 180.0 / M_PI; + std::cout << " | PID: " << std::setprecision(2) << pid_deg << " deg"; + } + + // AutoSteer output (if enabled and valid) + if (autosteer_engine != nullptr && egolanes_tensor_buffer.full()) { + std::cout << " | AutoSteer: " << std::setprecision(2) << autosteer_steering << " deg"; + + // Show difference if both are available + if (steering_controller != nullptr) { + double pid_deg = steering_angle * 180.0 / M_PI; + double diff = autosteer_steering - pid_deg; + std::cout << " (Δ: " << std::setprecision(2) << diff << " deg)"; + } } - std::cout << std::endl; - // Print polynomial coefficients (for control interface) - std::cout << " Center polynomial: c0=" << path_output.center_coeff[0] - << ", c1=" << path_output.center_coeff[1] - << ", c2=" << path_output.center_coeff[2] << std::endl; + std::cout << std::endl; + } else if (autosteer_engine != nullptr && egolanes_tensor_buffer.full()) { + // If PathFinder is not valid but AutoSteer is running, still log AutoSteer + std::cout << "[Frame " << tf.frame_number << "] " + << "AutoSteer: " << std::fixed << std::setprecision(2) << autosteer_steering << " deg " + << "(PathFinder: invalid)" << std::endl; } } // ======================================== @@ -386,12 +448,12 @@ void inferenceThread( cv::resize(tf.frame, frame_small, cv::Size(), 0.5, 0.5, cv::INTER_AREA); // Deep clone lane masks (synchronous, safe for zero-copy borrow in logger) - autoware_pov::vision::autosteer::LaneSegmentation raw_clone; + autoware_pov::vision::egolanes::LaneSegmentation raw_clone; raw_clone.ego_left = raw_lanes.ego_left.clone(); raw_clone.ego_right = raw_lanes.ego_right.clone(); raw_clone.other_lanes = raw_lanes.other_lanes.clone(); - autoware_pov::vision::autosteer::LaneSegmentation filtered_clone; + autoware_pov::vision::egolanes::LaneSegmentation filtered_clone; filtered_clone.ego_left = filtered_lanes.ego_left.clone(); filtered_clone.ego_right = filtered_lanes.ego_right.clone(); filtered_clone.other_lanes = filtered_lanes.other_lanes.clone(); @@ -415,8 +477,10 @@ void inferenceThread( result.frame_number = tf.frame_number; result.capture_time = tf.timestamp; result.inference_time = t_inference_end; - result.steering_angle = steering_angle; // Store computed steering angle + result.steering_angle = steering_angle; // Store PID steering angle result.path_output = path_output; // Store for viz + result.autosteer_angle = autosteer_steering; // Store AutoSteer angle + result.autosteer_valid = (autosteer_engine != nullptr && egolanes_tensor_buffer.full()); output_queue.push(result); } @@ -441,11 +505,11 @@ void displayThread( int window_height = 1080; if (enable_viz) { cv::namedWindow( - "AutoSteer Inference", + "EgoLanes Inference", cv::WINDOW_NORMAL ); cv::resizeWindow( - "AutoSteer Inference", + "EgoLanes Inference", window_width, window_height ); @@ -459,7 +523,7 @@ void displayThread( std::cout << "Video saving enabled. Output: " << output_video_path << std::endl; } - // CSV logger for curve params metrics + // CSV logger for all steering outputs (PathFinder + PID + AutoSteer) std::ofstream csv_file; csv_file.open(csv_log_path); if (csv_file.is_open()) { @@ -467,12 +531,13 @@ void displayThread( csv_file << "frame_id,timestamp_ms," << "orig_lane_offset,orig_yaw_offset,orig_curvature," << "pathfinder_cte,pathfinder_yaw_error,pathfinder_curvature," - << "steering_angle_rad,steering_angle_deg\n"; + << "pid_steering_rad,pid_steering_deg," + << "autosteer_angle_deg,autosteer_valid\n"; std::cout << "CSV logging enabled: " << csv_log_path << std::endl; } else { std::cerr << "Error: could not open " << csv_log_path << " for writing" << std::endl; - } + } while (running.load()) { InferenceResult result; @@ -485,9 +550,6 @@ void displayThread( int count = metrics.frame_count.fetch_add(1) + 1; - // Console output: frame info - std::cout << "[Frame " << result.frame_number << "] Processed" << std::endl; - // Visualization if (enable_viz) { // drawLanesInPlace(result.frame, result.lanes, 2); @@ -525,7 +587,7 @@ void displayThread( if (result.path_output.fused_valid) { std::vector left_coeffs(result.path_output.left_coeff.begin(), result.path_output.left_coeff.end()); std::vector right_coeffs(result.path_output.right_coeff.begin(), result.path_output.right_coeff.end()); - autoware_pov::vision::autosteer::drawMetricVerification( + autoware_pov::vision::egolanes::drawMetricVerification( view_bev, left_coeffs, right_coeffs @@ -604,7 +666,7 @@ void displayThread( } // Display - cv::imshow("AutoSteer Inference", stacked_view); + cv::imshow("EgoLanes Inference", stacked_view); if (cv::waitKey(1) == 'q') { running.store(false); @@ -612,11 +674,9 @@ void displayThread( } } - // CSV logging for curve params (use PathFinder filtered outputs, not raw metrics) - if ( - csv_file.is_open() && - result.path_output.fused_valid // Use PathFinder validity, not lanes.path_valid - ) { + // CSV logging: Log all frames to ensure PID and AutoSteer are captured + // Use 0.0 for invalid PathFinder values (can be filtered in post-processing) + if (csv_file.is_open()) { // Timestamp calc, from captured time auto ms_since_epoch = duration_cast( result.capture_time.time_since_epoch() @@ -628,14 +688,17 @@ void displayThread( << result.metrics.orig_lane_offset << "," << result.metrics.orig_yaw_offset << "," << result.metrics.orig_curvature << "," - // PathFinder filtered metrics (CORRECT - these match console output) - << result.path_output.cte << "," - << result.path_output.yaw_error << "," - << result.path_output.curvature << "," - // Steering angle + // PathFinder filtered metrics (NaN or 0.0 when invalid) + << (result.path_output.fused_valid ? result.path_output.cte : 0.0) << "," + << (result.path_output.fused_valid ? result.path_output.yaw_error : 0.0) << "," + << (result.path_output.fused_valid ? result.path_output.curvature : 0.0) << "," + // PID Controller steering angle (always log if controller exists) << std::fixed << std::setprecision(6) << result.steering_angle << "," - << (result.steering_angle * 180.0 / M_PI) << "\n"; - } + << (result.steering_angle * 180.0 / M_PI) << "," + // AutoSteer steering angle (degrees) and validity + << result.autosteer_angle << "," + << (result.autosteer_valid ? 1 : 0) << "\n"; + } auto t_display_end = steady_clock::now(); @@ -686,7 +749,7 @@ void displayThread( if (csv_file.is_open()) { csv_file.close(); std::cout << "CSV log saved." << std::endl; - } + } } int main(int argc, char** argv) @@ -708,7 +771,7 @@ int main(int argc, char** argv) std::cerr << " output_video: (optional) Output video path (default: output.avi)\n\n"; std::cerr << "Rerun Logging (optional):\n"; std::cerr << " --rerun : Enable Rerun live viewer\n"; - std::cerr << " --rerun-save [path] : Save to .rrd file (default: autosteer.rrd)\n\n"; + std::cerr << " --rerun-save [path] : Save to .rrd file (default: egolanes.rrd)\n\n"; std::cerr << "PathFinder (optional):\n"; std::cerr << " --path-planner : Enable PathFinder (Bayes filter + polynomial fitting)\n"; std::cerr << " --pathfinder : (alias)\n\n"; @@ -722,7 +785,9 @@ int main(int argc, char** argv) << SteeringControllerDefaults::K_I << ")\n"; std::cerr << " --Kd [val] : Derivative gain (default: " << SteeringControllerDefaults::K_D << ")\n"; - std::cerr << " --csv-log [path] : CSV log file path (default: ./assets/curve_params_metrics.csv)\n"; + std::cerr << " --csv-log [path] : CSV log file path (default: ./assets/curve_params_metrics.csv)\n\n"; + std::cerr << "AutoSteer (optional - temporal steering prediction):\n"; + std::cerr << " --autosteer [model] : Enable AutoSteer with model path\n"; std::cerr << "Examples:\n"; std::cerr << " # Camera with live Rerun viewer:\n"; std::cerr << " " << argv[0] << " camera model.onnx tensorrt fp16 --rerun\n\n"; @@ -803,6 +868,8 @@ int main(int argc, char** argv) std::string rerun_save_path = ""; bool enable_path_planner = false; bool enable_steering_control = false; + bool enable_autosteer = false; + std::string autosteer_model_path = ""; // Steering controller parameters (defaults from steering_controller.hpp) double K_p = SteeringControllerDefaults::K_P; @@ -821,12 +888,15 @@ int main(int argc, char** argv) if (i + 1 < argc && argv[i + 1][0] != '-') { rerun_save_path = argv[++i]; } else { - rerun_save_path = "autosteer.rrd"; + rerun_save_path = "egolanes.rrd"; } } else if (arg == "--path-planner" || arg == "--pathfinder") { enable_path_planner = true; } else if (arg == "--steering-control") { enable_steering_control = true; + } else if (arg == "--autosteer" && i + 1 < argc) { + enable_autosteer = true; + autosteer_model_path = argv[++i]; } else if (arg == "--Ks" && i + 1 < argc) { K_S = std::atof(argv[++i]); } else if (arg == "--Kp" && i + 1 < argc) { @@ -860,7 +930,7 @@ int main(int argc, char** argv) std::cout << "\nNote: TensorRT engine build may take 20-30 seconds on first run..." << std::endl; } - AutoSteerOnnxEngine engine(model_path, provider, precision, device_id, cache_dir); + EgoLanesOnnxEngine engine(model_path, provider, precision, device_id, cache_dir); std::cout << "Backend initialized!\n" << std::endl; // Warm-up inference (builds TensorRT engine on first run) @@ -890,7 +960,7 @@ int main(int argc, char** argv) std::unique_ptr rerun_logger; if (enable_rerun) { rerun_logger = std::make_unique( - "AutoSteer", spawn_rerun_viewer, rerun_save_path); + "EgoLanes", spawn_rerun_viewer, rerun_save_path); } #endif @@ -909,6 +979,46 @@ int main(int argc, char** argv) steering_controller = std::make_unique(K_p, K_i, K_d, K_S); std::cout << "Steering Controller initialized" << std::endl; } + + // Initialize AutoSteer (optional - temporal steering prediction) + std::unique_ptr autosteer_engine; + if (enable_autosteer) { + std::cout << "\nLoading AutoSteer model: " << autosteer_model_path << std::endl; + std::cout << "Provider: " << provider << " | Precision: " << precision << std::endl; + + if (provider == "tensorrt") { + std::cout << "Device ID: " << device_id << " | Cache dir: " << cache_dir << std::endl; + std::cout << "\nNote: TensorRT engine build may take 20-30 seconds on first run..." << std::endl; + } + + autosteer_engine = std::make_unique( + autosteer_model_path, provider, precision, device_id, cache_dir); + + // Warm-up AutoSteer inference (builds TensorRT engine on first run) + if (provider == "tensorrt") { + std::cout << "Running AutoSteer warm-up inference to build TensorRT engine..." << std::endl; + std::cout << "This may take 20-60 seconds on first run. Please wait...\n" << std::endl; + + auto warmup_start = std::chrono::steady_clock::now(); + + // Create dummy concatenated input [1, 6, 80, 160] + std::vector dummy_input(6 * 80 * 160, 0.5f); + float warmup_result = autosteer_engine->inference(dummy_input); + + auto warmup_end = std::chrono::steady_clock::now(); + double warmup_time = std::chrono::duration_cast( + warmup_end - warmup_start).count() / 1000.0; + + std::cout << "AutoSteer warm-up complete! (took " << std::fixed << std::setprecision(1) + << warmup_time << "s)" << std::endl; + std::cout << "TensorRT engine is now cached and ready.\n" << std::endl; + } + + std::cout << "AutoSteer initialized (temporal steering prediction)" << std::endl; + std::cout << " - Input: [1, 6, 80, 160] (concatenated EgoLanes t-1, t)" << std::endl; + std::cout << " - Output: Steering angle (degrees, -30 to +30)" << std::endl; + std::cout << " - Note: First frame will be skipped (requires temporal buffer)\n" << std::endl; + } // Thread-safe queues with bounded size (prevents memory overflow) ThreadSafeQueue capture_queue(5); // Max 5 frames waiting for inference @@ -937,6 +1047,9 @@ int main(int argc, char** argv) if (steering_controller) { std::cout << "Steering Control: ENABLED" << std::endl; } + if (autosteer_engine) { + std::cout << "AutoSteer: ENABLED (temporal steering prediction)" << std::endl; + } if (measure_latency) { std::cout << "Latency measurement: ENABLED (metrics every 30 frames)" << std::endl; } @@ -959,13 +1072,15 @@ int main(int argc, char** argv) std::ref(metrics), std::ref(running), threshold, path_finder.get(), steering_controller.get(), + autosteer_engine.get(), rerun_logger.get()); #else std::thread t_inference(inferenceThread, std::ref(engine), std::ref(capture_queue), std::ref(display_queue), std::ref(metrics), std::ref(running), threshold, path_finder.get(), - steering_controller.get()); + steering_controller.get(), + autosteer_engine.get()); #endif std::thread t_display(displayThread, std::ref(display_queue), std::ref(metrics), std::ref(running), enable_viz, save_video, output_video_path, csv_log_path); diff --git a/VisionPilot/Production_Releases/0.5/run_final.sh b/VisionPilot/Production_Releases/0.5/run_final.sh new file mode 100644 index 000000000..96a932257 --- /dev/null +++ b/VisionPilot/Production_Releases/0.5/run_final.sh @@ -0,0 +1,160 @@ +#!/bin/bash +# EgoLanes Production Pipeline Runner + +set -euo pipefail + +# ============================================================================ +# Configuration +# ============================================================================ + +# Environment (set via environment variables or defaults) +export ONNXRUNTIME_ROOT="${ONNXRUNTIME_ROOT:-}" +export RERUN_SDK_ROOT="${RERUN_SDK_ROOT:-}" +export LD_LIBRARY_PATH="${ONNXRUNTIME_ROOT}/lib:${LD_LIBRARY_PATH:-}" +export GST_DEBUG="${GST_DEBUG:-1}" + +# Pipeline Mode +MODE="${MODE:-video}" # "camera" or "video" + +# Model Configuration +MODEL_PATH="${MODEL_PATH:-models/Egolanes_fp32.onnx}" +VIDEO_PATH="${VIDEO_PATH:-}" +PROVIDER="${PROVIDER:-tensorrt}" # "cpu" or "tensorrt" +PRECISION="${PRECISION:-fp16}" # "fp32" or "fp16" +DEVICE_ID="${DEVICE_ID:-0}" +CACHE_DIR="${CACHE_DIR:-models/trt_cache}" + +# Steering Controller +KP="${KP:-0.33}" +KI="${KI:-0.01}" +KD="${KD:--0.40}" +KS="${KS:--0.3}" +CSV_LOG_PATH="${CSV_LOG_PATH:-metrics_kp${KP}_ki${KI}_kd${KD}_ks${KS}.csv}" + +# AutoSteer (optional) +ENABLE_AUTOSTEER="${ENABLE_AUTOSTEER:-false}" +AUTOSTEER_MODEL_PATH="${AUTOSTEER_MODEL_PATH:-models/AutoSteer_FP32.onnx}" + +# Visualization +THRESHOLD="${THRESHOLD:-0.0}" +MEASURE_LATENCY="${MEASURE_LATENCY:-true}" +ENABLE_VIZ="${ENABLE_VIZ:-false}" +SAVE_VIDEO="${SAVE_VIDEO:-false}" +OUTPUT_VIDEO="${OUTPUT_VIDEO:-output_egolanes_${PRECISION}_${PROVIDER}.mp4}" + +# Rerun (optional) +ENABLE_RERUN="${ENABLE_RERUN:-false}" +RERUN_SPAWN="${RERUN_SPAWN:-false}" +RERUN_SAVE="${RERUN_SAVE:-false}" +RERUN_PATH="${RERUN_PATH:-egolanes.rrd}" + +# ============================================================================ +# Validation +# ============================================================================ + +validate_mode() { + if [[ "$MODE" != "camera" && "$MODE" != "video" ]]; then + echo "Error: MODE must be 'camera' or 'video'" >&2 + exit 1 + fi +} + +validate_files() { + if [[ ! -f "$MODEL_PATH" ]]; then + echo "Error: Model not found: $MODEL_PATH" >&2 + exit 1 + fi + + if [[ "$MODE" == "video" ]]; then + if [[ -z "$VIDEO_PATH" ]]; then + echo "Error: VIDEO_PATH required for video mode" >&2 + exit 1 + fi + if [[ ! -f "$VIDEO_PATH" ]]; then + echo "Error: Video not found: $VIDEO_PATH" >&2 + exit 1 + fi + fi + + if [[ "$ENABLE_AUTOSTEER" == "true" ]]; then + if [[ ! -f "$AUTOSTEER_MODEL_PATH" ]]; then + echo "Error: AutoSteer model not found: $AUTOSTEER_MODEL_PATH" >&2 + exit 1 + fi + fi +} + +# ============================================================================ +# Display Configuration +# ============================================================================ + +print_config() { + echo "━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━" + echo " EgoLanes Pipeline" + echo "━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━" + echo " Mode: $MODE" + [[ "$MODE" == "video" ]] && echo " Video: $VIDEO_PATH" + echo " Model: $MODEL_PATH" + echo " Provider: $PROVIDER | $PRECISION" + [[ "$PROVIDER" == "tensorrt" ]] && echo " Device: $DEVICE_ID | Cache: $CACHE_DIR" + echo " ────────────────────────────────────────────────────────────────────" + echo " Steering: Kp=$KP Ki=$KI Kd=$KD Ks=$KS" + echo " Log: $CSV_LOG_PATH" + [[ "$ENABLE_AUTOSTEER" == "true" ]] && echo " AutoSteer: $AUTOSTEER_MODEL_PATH" + echo " ────────────────────────────────────────────────────────────────────" + echo " Threshold: $THRESHOLD" + echo " Visualization: $ENABLE_VIZ | Save: $SAVE_VIDEO" + [[ "$SAVE_VIDEO" == "true" ]] && echo " Output: $OUTPUT_VIDEO" + [[ "$ENABLE_RERUN" == "true" ]] && echo " Rerun: Spawn=$RERUN_SPAWN | Save=$RERUN_SAVE" + [[ "$ENABLE_RERUN" == "true" && "$RERUN_SAVE" == "true" ]] && echo " Rerun Path: $RERUN_PATH" + echo "━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━" + echo "" +} + +# ============================================================================ +# Build Command +# ============================================================================ + +build_command() { + local cmd="./build/egolanes_pipeline $MODE" + + [[ "$MODE" == "video" ]] && cmd="$cmd \"$VIDEO_PATH\"" + + cmd="$cmd \"$MODEL_PATH\" $PROVIDER $PRECISION $DEVICE_ID \"$CACHE_DIR\"" + cmd="$cmd $THRESHOLD $MEASURE_LATENCY $ENABLE_VIZ $SAVE_VIDEO \"$OUTPUT_VIDEO\"" + cmd="$cmd --steering-control --Kp $KP --Ki $KI --Kd $KD --Ks $KS" + cmd="$cmd --csv-log \"$CSV_LOG_PATH\"" + + [[ "$ENABLE_AUTOSTEER" == "true" ]] && cmd="$cmd --autosteer \"$AUTOSTEER_MODEL_PATH\"" + + if [[ "$ENABLE_RERUN" == "true" ]]; then + [[ "$RERUN_SPAWN" == "true" ]] && cmd="$cmd --rerun" + [[ "$RERUN_SAVE" == "true" ]] && cmd="$cmd --rerun-save \"$RERUN_PATH\"" + fi + + echo "$cmd" +} + +# ============================================================================ +# Main +# ============================================================================ + +main() { + validate_mode + validate_files + print_config + + local cmd=$(build_command) + + if [[ "$ENABLE_VIZ" == "true" ]]; then + echo "Press 'q' to quit" + else + echo "Running in headless mode" + fi + echo "" + + eval "$cmd" +} + +main "$@" + diff --git a/VisionPilot/Production_Releases/0.5/src/inference/autosteer_engine.cpp b/VisionPilot/Production_Releases/0.5/src/inference/autosteer_engine.cpp new file mode 100644 index 000000000..6aba40f9d --- /dev/null +++ b/VisionPilot/Production_Releases/0.5/src/inference/autosteer_engine.cpp @@ -0,0 +1,221 @@ +#include "inference/autosteer_engine.hpp" +#include "inference/onnxruntime_session.hpp" +#include +#include +#include +#include +#include + +// Simple logging macros (standalone version) +#define LOG_INFO(...) printf("[INFO] "); printf(__VA_ARGS__); printf("\n") +#define LOG_ERROR(...) printf("[ERROR] "); printf(__VA_ARGS__); printf("\n") + +namespace autoware_pov::vision::egolanes +{ + +AutoSteerOnnxEngine::AutoSteerOnnxEngine( + const std::string& model_path, + const std::string& provider, + const std::string& precision, + int device_id, + const std::string& cache_dir) +{ + // Create session using factory (reuses same logic as EgoLanes) + // Use 1GB workspace for AutoSteer with separate cache prefix + session_ = OnnxRuntimeSessionFactory::createSession( + model_path, provider, precision, device_id, cache_dir, 1.0, "autosteer_" + ); + + // Create memory info for CPU tensors + memory_info_ = std::make_unique( + Ort::MemoryInfo::CreateCpu(OrtArenaAllocator, OrtMemTypeDefault) + ); + + // Get input/output names and store them permanently + Ort::AllocatorWithDefaultOptions allocator; + + // Input (typically "input") + auto input_name_allocated = session_->GetInputNameAllocated(0, allocator); + input_name_storage_ = std::string(input_name_allocated.get()); + input_names_.push_back(input_name_storage_.c_str()); + + // Outputs: AutoSteer has 2 outputs (prev_steering, current_steering) + // Reserve space first to avoid reallocation + output_names_storage_.reserve(2); + + // Get and store both output names + auto output_name_0_allocated = session_->GetOutputNameAllocated(0, allocator); + output_names_storage_.emplace_back(output_name_0_allocated.get()); + + auto output_name_1_allocated = session_->GetOutputNameAllocated(1, allocator); + output_names_storage_.emplace_back(output_name_1_allocated.get()); + + // Now get the c_str() pointers (after strings are stored and vector won't reallocate) + output_names_.clear(); + output_names_.reserve(2); + output_names_.push_back(output_names_storage_[0].c_str()); + output_names_.push_back(output_names_storage_[1].c_str()); + + // Keep the old output_name_storage_ for backward compatibility (points to second output) + output_name_storage_ = output_names_storage_[1]; + + LOG_INFO("[autosteer_engine] Output 0 name: '%s'", output_names_storage_[0].c_str()); + LOG_INFO("[autosteer_engine] Output 1 name: '%s' (USED)", output_names_storage_[1].c_str()); + + // Verify output names are not empty + if (output_names_storage_[0].empty() || output_names_storage_[1].empty()) { + LOG_ERROR("[autosteer_engine] ERROR: Output names are empty! Output 0: '%s', Output 1: '%s'", + output_names_storage_[0].c_str(), output_names_storage_[1].c_str()); + throw std::runtime_error("AutoSteer output names are empty"); + } + + // Get input shape: [1, 6, 80, 160] + auto input_shape = session_->GetInputTypeInfo(0).GetTensorTypeAndShapeInfo().GetShape(); + model_input_channels_ = static_cast(input_shape[1]); // 6 + model_input_height_ = static_cast(input_shape[2]); // 80 + model_input_width_ = static_cast(input_shape[3]); // 160 + + // Get output shapes: model has 2 outputs [1, 61] each (or [61] in some exports) + auto output_shape_0 = session_->GetOutputTypeInfo(0).GetTensorTypeAndShapeInfo().GetShape(); + auto output_shape_1 = session_->GetOutputTypeInfo(1).GetTensorTypeAndShapeInfo().GetShape(); + + // Handle both [1, 61] and [61] output shapes + if (output_shape_1.size() == 2) { + model_output_classes_ = static_cast(output_shape_1[1]); // [1, 61] -> use index 1 + } else if (output_shape_1.size() == 1) { + model_output_classes_ = static_cast(output_shape_1[0]); // [61] -> use index 0 + } else { + LOG_ERROR("[autosteer_engine] Unexpected output shape dimensions: %zu", output_shape_1.size()); + model_output_classes_ = 61; // Fallback to expected value + } + + LOG_INFO("[autosteer_engine] AutoSteer engine initialized successfully"); + LOG_INFO("[autosteer_engine] - Input: [1, %d, %d, %d]", + model_input_channels_, model_input_height_, model_input_width_); + LOG_INFO("[autosteer_engine] - Output classes: %d (steering range: -30 to +30 deg)", model_output_classes_); + LOG_INFO("[autosteer_engine] - Using output index 1 (current frame prediction)"); +} + +AutoSteerOnnxEngine::~AutoSteerOnnxEngine() +{ + // Smart pointers handle cleanup automatically +} + +bool AutoSteerOnnxEngine::doInference(const std::vector& input_buffer) +{ + // Validate input size + size_t expected_size = model_input_channels_ * model_input_height_ * model_input_width_; + if (input_buffer.size() != expected_size) { + LOG_ERROR("[autosteer_engine] Invalid input size: %zu (expected %zu)", + input_buffer.size(), expected_size); + return false; + } + + // Create input tensor from pre-concatenated buffer + std::vector input_shape = {1, model_input_channels_, model_input_height_, model_input_width_}; + auto input_tensor = Ort::Value::CreateTensor( + *memory_info_, + const_cast(input_buffer.data()), // ONNX Runtime requires non-const + input_buffer.size(), + input_shape.data(), + input_shape.size() + ); + + // Run inference (ONNX Runtime allocates output automatically) + // Note: AutoSteer model has 2 outputs (prev, current), we need both + try { + // Verify output names are valid before calling Run + if (output_names_.size() < 2) { + LOG_ERROR("[autosteer_engine] Not enough output names: %zu (expected 2)", output_names_.size()); + return false; + } + + if (output_names_[0] == nullptr || output_names_[1] == nullptr) { + LOG_ERROR("[autosteer_engine] Output name pointers are null!"); + return false; + } + + if (strlen(output_names_[0]) == 0 || strlen(output_names_[1]) == 0) { + LOG_ERROR("[autosteer_engine] Output names are empty! Output 0: '%s', Output 1: '%s'", + output_names_[0] ? output_names_[0] : "null", + output_names_[1] ? output_names_[1] : "null"); + return false; + } + + output_tensors_ = session_->Run( + Ort::RunOptions{nullptr}, + input_names_.data(), + &input_tensor, + 1, + output_names_.data(), + 2 // Request BOTH outputs + ); + } catch (const Ort::Exception& e) { + LOG_ERROR("[autosteer_engine] Inference failed: %s", e.what()); + return false; + } + + return true; +} + +float AutoSteerOnnxEngine::postProcess() +{ + if (output_tensors_.size() < 2) { + LOG_ERROR("[autosteer_engine] Expected 2 output tensors, got %zu", output_tensors_.size()); + if (output_tensors_.empty()) { + return 0.0f; + } + } + + // AutoSteer model returns TWO outputs: (prev_frame_prediction, current_frame_prediction) + // We use the SECOND output (index 1) for current frame steering + int output_index = (output_tensors_.size() >= 2) ? 1 : 0; + const float* raw_output = output_tensors_[output_index].GetTensorData(); + + // Find argmax (class with highest logit value) + int max_class = 0; + float max_value = raw_output[0]; + + for (int i = 1; i < model_output_classes_; ++i) { + if (raw_output[i] > max_value) { + max_value = raw_output[i]; + max_class = i; + } + } + + // Convert class to steering angle: argmax - 30 + // Classes: 0-60 → Angles: -30 to +30 degrees + float steering_angle = static_cast(max_class - 30); + + return steering_angle; +} + +float AutoSteerOnnxEngine::inference(const std::vector& concat_input) +{ + // Run inference + if (!doInference(concat_input)) { + LOG_ERROR("[autosteer_engine] Inference failed"); + return 0.0f; + } + + // Post-process and return steering angle + return postProcess(); +} + +std::vector AutoSteerOnnxEngine::getRawOutputLogits() const +{ + if (output_tensors_.size() < 2) { + LOG_ERROR("[autosteer_engine] Expected 2 output tensors, got %zu", output_tensors_.size()); + if (output_tensors_.empty()) { + return std::vector(); + } + } + + // Return the SECOND output (index 1), which is the current frame prediction + int output_index = (output_tensors_.size() >= 2) ? 1 : 0; + const float* raw_output = output_tensors_[output_index].GetTensorData(); + return std::vector(raw_output, raw_output + model_output_classes_); +} + +} // namespace autoware_pov::vision::egolanes + diff --git a/VisionPilot/Production_Releases/0.5/src/inference/onnxruntime_engine.cpp b/VisionPilot/Production_Releases/0.5/src/inference/onnxruntime_engine.cpp index a313981f4..898104faf 100644 --- a/VisionPilot/Production_Releases/0.5/src/inference/onnxruntime_engine.cpp +++ b/VisionPilot/Production_Releases/0.5/src/inference/onnxruntime_engine.cpp @@ -7,10 +7,10 @@ #define LOG_INFO(...) printf("[INFO] "); printf(__VA_ARGS__); printf("\n") #define LOG_ERROR(...) printf("[ERROR] "); printf(__VA_ARGS__); printf("\n") -namespace autoware_pov::vision::autosteer +namespace autoware_pov::vision::egolanes { -AutoSteerOnnxEngine::AutoSteerOnnxEngine( +EgoLanesOnnxEngine::EgoLanesOnnxEngine( const std::string& model_path, const std::string& provider, const std::string& precision, @@ -18,8 +18,9 @@ AutoSteerOnnxEngine::AutoSteerOnnxEngine( const std::string& cache_dir) { // Create session using factory (all provider logic is there) + // Use 1GB workspace for EgoLanes session_ = OnnxRuntimeSessionFactory::createSession( - model_path, provider, precision, device_id, cache_dir + model_path, provider, precision, device_id, cache_dir, 1.0, "egolanes_" ); // Create memory info for CPU tensors @@ -58,17 +59,17 @@ AutoSteerOnnxEngine::AutoSteerOnnxEngine( size_t input_size = model_input_width_ * model_input_height_ * 3; input_buffer_.resize(input_size); - LOG_INFO("[onnxrt_engine] AutoSteer engine initialized successfully"); + LOG_INFO("[onnxrt_engine] EgoLanes engine initialized successfully"); LOG_INFO("[onnxrt_engine] - Input: %dx%d", model_input_width_, model_input_height_); LOG_INFO("[onnxrt_engine] - Output: %dx%d (3 channels)", model_output_width_, model_output_height_); } -AutoSteerOnnxEngine::~AutoSteerOnnxEngine() +EgoLanesOnnxEngine::~EgoLanesOnnxEngine() { // Smart pointers handle cleanup automatically } -void AutoSteerOnnxEngine::preprocessAutoSteer(const cv::Mat& input_image, float* buffer) +void EgoLanesOnnxEngine::preprocessEgoLanes(const cv::Mat& input_image, float* buffer) { // Step 1: Resize to model input size (320x640) cv::Mat resized; @@ -100,10 +101,10 @@ void AutoSteerOnnxEngine::preprocessAutoSteer(const cv::Mat& input_image, float* } } -bool AutoSteerOnnxEngine::doInference(const cv::Mat& input_image) +bool EgoLanesOnnxEngine::doInference(const cv::Mat& input_image) { // Preprocess image - preprocessAutoSteer(input_image, input_buffer_.data()); + preprocessEgoLanes(input_image, input_buffer_.data()); // Create input tensor std::vector input_shape = {1, 3, model_input_height_, model_input_width_}; @@ -133,7 +134,7 @@ bool AutoSteerOnnxEngine::doInference(const cv::Mat& input_image) return true; } -LaneSegmentation AutoSteerOnnxEngine::inference( +LaneSegmentation EgoLanesOnnxEngine::inference( const cv::Mat& input_image, float threshold) { @@ -147,7 +148,7 @@ LaneSegmentation AutoSteerOnnxEngine::inference( return postProcess(threshold); } -LaneSegmentation AutoSteerOnnxEngine::postProcess(float threshold) +LaneSegmentation EgoLanesOnnxEngine::postProcess(float threshold) { LaneSegmentation result; result.height = model_output_height_; @@ -190,7 +191,7 @@ LaneSegmentation AutoSteerOnnxEngine::postProcess(float threshold) return result; } -const float* AutoSteerOnnxEngine::getRawTensorData() const +const float* EgoLanesOnnxEngine::getRawTensorData() const { if (output_tensors_.empty()) { throw std::runtime_error("Inference has not been run yet. Call inference() first."); @@ -198,7 +199,7 @@ const float* AutoSteerOnnxEngine::getRawTensorData() const return output_tensors_[0].GetTensorData(); } -std::vector AutoSteerOnnxEngine::getTensorShape() const +std::vector EgoLanesOnnxEngine::getTensorShape() const { if (output_tensors_.empty()) { throw std::runtime_error("Inference has not been run yet. Call inference() first."); @@ -206,5 +207,5 @@ std::vector AutoSteerOnnxEngine::getTensorShape() const return output_tensors_[0].GetTensorTypeAndShapeInfo().GetShape(); } -} // namespace autoware_pov::vision::autosteer +} // namespace autoware_pov::vision::egolanes diff --git a/VisionPilot/Production_Releases/0.5/src/inference/onnxruntime_session.cpp b/VisionPilot/Production_Releases/0.5/src/inference/onnxruntime_session.cpp index fe6956e07..a4b06398b 100644 --- a/VisionPilot/Production_Releases/0.5/src/inference/onnxruntime_session.cpp +++ b/VisionPilot/Production_Releases/0.5/src/inference/onnxruntime_session.cpp @@ -7,7 +7,7 @@ #define LOG_INFO(...) printf("[INFO] "); printf(__VA_ARGS__); printf("\n") #define LOG_ERROR(...) printf("[ERROR] "); printf(__VA_ARGS__); printf("\n") -namespace autoware_pov::vision::autosteer +namespace autoware_pov::vision::egolanes { std::unique_ptr OnnxRuntimeSessionFactory::createSession( @@ -15,10 +15,12 @@ std::unique_ptr OnnxRuntimeSessionFactory::createSession( const std::string& provider, const std::string& precision, int device_id, - const std::string& cache_dir) + const std::string& cache_dir, + double workspace_size_gb, + const std::string& cache_prefix) { // Create ONNX Runtime environment (thread-safe singleton pattern) - static Ort::Env env(ORT_LOGGING_LEVEL_WARNING, "AutoSteerOnnxRuntime"); + static Ort::Env env(ORT_LOGGING_LEVEL_WARNING, "EgoLanesOnnxRuntime"); if (provider == "cpu") { LOG_INFO("[onnxrt] Creating CPU session for model: %s", model_path.c_str()); @@ -27,7 +29,7 @@ std::unique_ptr OnnxRuntimeSessionFactory::createSession( else if (provider == "tensorrt") { LOG_INFO("[onnxrt] Creating TensorRT session (%s) for model: %s", precision.c_str(), model_path.c_str()); - return createTensorRTSession(env, model_path, precision, device_id, cache_dir); + return createTensorRTSession(env, model_path, precision, device_id, cache_dir, workspace_size_gb, cache_prefix); } else { throw std::runtime_error("Unsupported provider: " + provider + @@ -56,7 +58,9 @@ std::unique_ptr OnnxRuntimeSessionFactory::createTensorRTSession( const std::string& model_path, const std::string& precision, int device_id, - const std::string& cache_dir) + const std::string& cache_dir, + double workspace_size_gb, + const std::string& cache_prefix) { Ort::SessionOptions session_options; @@ -72,12 +76,16 @@ std::unique_ptr OnnxRuntimeSessionFactory::createTensorRTSession( Ort::ThrowOnError(api.CreateTensorRTProviderOptions(&tensorrt_options)); // Create unique cache prefix for fp32 vs fp16 - std::string cache_prefix = "autosteer_" + precision + "_"; + std::string full_cache_prefix = cache_prefix + precision + "_"; + + // Calculate workspace size in bytes + size_t workspace_size_bytes = static_cast(workspace_size_gb * 1024.0 * 1024.0 * 1024.0); + std::string workspace_size_str = std::to_string(workspace_size_bytes); // Prepare option keys and values std::vector option_keys = { "device_id", // GPU device ID - "trt_max_workspace_size", // 2GB workspace + "trt_max_workspace_size", // Configurable workspace size "trt_fp16_enable", // FP16 precision "trt_engine_cache_enable", // Enable engine caching "trt_engine_cache_path", // Cache directory @@ -94,11 +102,11 @@ std::unique_ptr OnnxRuntimeSessionFactory::createTensorRTSession( std::vector option_values = { device_id_str.c_str(), // GPU device - "2147483648", // 2GB workspace + workspace_size_str.c_str(), // Configurable workspace size (in bytes) fp16_flag.c_str(), // FP16 enable/disable "1", // Enable engine cache cache_dir.c_str(), // Cache path - cache_prefix.c_str(), // Unique prefix (fp16/fp32) + full_cache_prefix.c_str(), // Unique prefix (fp16/fp32) "1", // Enable timing cache cache_dir.c_str(), // Timing cache path "5", // Max optimization level @@ -130,10 +138,11 @@ std::unique_ptr OnnxRuntimeSessionFactory::createTensorRTSession( LOG_INFO("[onnxrt] TensorRT session created successfully"); LOG_INFO("[onnxrt] - Precision: %s", precision.c_str()); LOG_INFO("[onnxrt] - Device: %d", device_id); - LOG_INFO("[onnxrt] - Cache: %s/%s*.engine", cache_dir.c_str(), cache_prefix.c_str()); + LOG_INFO("[onnxrt] - Workspace: %.1f GB", workspace_size_gb); + LOG_INFO("[onnxrt] - Cache: %s/%s*.engine", cache_dir.c_str(), full_cache_prefix.c_str()); return session; } -} // namespace autoware_pov::vision::autosteer +} // namespace autoware_pov::vision::egolanes diff --git a/VisionPilot/Production_Releases/0.5/src/lane_filtering/lane_filter.cpp b/VisionPilot/Production_Releases/0.5/src/lane_filtering/lane_filter.cpp index cb6655076..e8d907990 100644 --- a/VisionPilot/Production_Releases/0.5/src/lane_filtering/lane_filter.cpp +++ b/VisionPilot/Production_Releases/0.5/src/lane_filtering/lane_filter.cpp @@ -4,7 +4,7 @@ #include #include -namespace autoware_pov::vision::autosteer +namespace autoware_pov::vision::egolanes { LaneFilter::LaneFilter(float smoothing_factor) @@ -593,4 +593,4 @@ std::vector LaneFilter::slidingWindowSearch( return lane_points; } -} // namespace autoware_pov::vision::autosteer \ No newline at end of file +} // namespace autoware_pov::vision::egolanes \ No newline at end of file diff --git a/VisionPilot/Production_Releases/0.5/src/lane_tracking/lane_tracking.cpp b/VisionPilot/Production_Releases/0.5/src/lane_tracking/lane_tracking.cpp index 7110d1438..5f747ae55 100644 --- a/VisionPilot/Production_Releases/0.5/src/lane_tracking/lane_tracking.cpp +++ b/VisionPilot/Production_Releases/0.5/src/lane_tracking/lane_tracking.cpp @@ -13,7 +13,7 @@ #include -namespace autoware_pov::vision::autosteer +namespace autoware_pov::vision::egolanes { LaneTracker::LaneTracker() { @@ -450,4 +450,4 @@ double LaneTracker::calcCurvature( return std::abs(d2x_dy2) / denom; } -} // namespace autoware_pov::vision::autosteer \ No newline at end of file +} // namespace autoware_pov::vision::egolanes \ No newline at end of file diff --git a/VisionPilot/Production_Releases/0.5/src/path_planning/path_finder.cpp b/VisionPilot/Production_Releases/0.5/src/path_planning/path_finder.cpp index 09dfa47c7..42b069e89 100644 --- a/VisionPilot/Production_Releases/0.5/src/path_planning/path_finder.cpp +++ b/VisionPilot/Production_Releases/0.5/src/path_planning/path_finder.cpp @@ -47,7 +47,8 @@ void PathFinder::initializeBayesFilter() PathFinderOutput PathFinder::update( const std::vector& left_pts_bev, - const std::vector& right_pts_bev) + const std::vector& right_pts_bev, + double autosteer_steering_rad) { PathFinderOutput output; output.left_valid = false; @@ -84,10 +85,13 @@ PathFinderOutput PathFinder::update( // Store individual metrics output.left_cte = left_curve.cte; output.left_yaw_error = left_curve.yaw_error; - output.left_curvature = left_curve.curvature; // Pass in AutoSteer units + output.left_curvature = left_curve.curvature; output.right_cte = right_curve.cte; output.right_yaw_error = right_curve.yaw_error; - output.right_curvature = right_curve.curvature; // Pass in AutoSteer units + output.right_curvature = right_curve.curvature; + + // Use AutoSteer steering angle (in radians) instead of computed curvature if provided + double steering_value = autosteer_steering_rad ; // 4. Create measurement (adapted from cb_drivCorr) std::array measurement; @@ -123,12 +127,12 @@ PathFinderOutput PathFinder::update( // [1,5,9] = left lane (offset CTE to lane center) measurement[1].mean = left_curve.cte + width / 2.0; measurement[5].mean = left_curve.yaw_error; - measurement[9].mean = left_curve.curvature; + measurement[9].mean = steering_value ; // [2,6,10] = right lane (offset CTE to lane center) measurement[2].mean = right_curve.cte - width / 2.0; measurement[6].mean = right_curve.yaw_error; - measurement[10].mean = right_curve.curvature; + measurement[10].mean = steering_value ; // [3,7,11] = fused (computed by Bayes filter) measurement[3].mean = std::numeric_limits::quiet_NaN(); @@ -160,7 +164,8 @@ PathFinderOutput PathFinder::update( output.cte = state[3].mean; output.yaw_error = state[7].mean; - output.curvature = state[11].mean; + // Use AutoSteer steering angle (radians) if provided, otherwise use fused curvature from Bayes filter + output.curvature = autosteer_steering_rad ; output.lane_width = state[12].mean; output.cte_variance = state[3].variance; @@ -172,22 +177,6 @@ PathFinderOutput PathFinder::update( !std::isnan(output.yaw_error) && !std::isnan(output.curvature); - // 7. Calculate center polynomial coefficients (average left/right) - if (output.left_valid && output.right_valid) { - FittedCurve center = calculateEgoPath(left_curve, right_curve); - output.center_coeff = center.coeff; - } else if (output.left_valid) { - output.center_coeff = left_coeff; - } else if (output.right_valid) { - output.center_coeff = right_coeff; - } else { - output.center_coeff = { - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN() - }; - } - return output; } diff --git a/VisionPilot/Production_Releases/0.5/src/path_planning/poly_fit.cpp b/VisionPilot/Production_Releases/0.5/src/path_planning/poly_fit.cpp index 5786feb3a..272e9c117 100644 --- a/VisionPilot/Production_Releases/0.5/src/path_planning/poly_fit.cpp +++ b/VisionPilot/Production_Releases/0.5/src/path_planning/poly_fit.cpp @@ -28,8 +28,9 @@ FittedCurve::FittedCurve(const std::array &coeff) : coeff(coeff) cte = -coeff[2]; // Lateral offset yaw_error = -std::atan2(coeff[1], 1.0); // Heading angle - // Curvature formula: κ = 2*c0 / (1 + c1²)^(3/2) - curvature = 2 * coeff[0] / std::pow(1 - (coeff[1] * coeff[1]), 1.5); + // Curvature is no longer computed here - we use AutoSteer steering angle instead + // Keep curvature field for backward compatibility with Bayes filter (will use AutoSteer when available) + curvature = std::numeric_limits::quiet_NaN(); } std::array fitQuadPoly(const std::vector &points) @@ -73,15 +74,5 @@ std::array fitQuadPoly(const std::vector &points) return coeffs; } -FittedCurve calculateEgoPath(const FittedCurve &left_lane, const FittedCurve &right_lane) -{ - // Average the polynomial coefficients - return FittedCurve({ - (left_lane.coeff[0] + right_lane.coeff[0]) / 2.0, - (left_lane.coeff[1] + right_lane.coeff[1]) / 2.0, - (left_lane.coeff[2] + right_lane.coeff[2]) / 2.0 - }); -} - } // namespace autoware_pov::vision::path_planning diff --git a/VisionPilot/Production_Releases/0.5/src/rerun/rerun_logger.cpp b/VisionPilot/Production_Releases/0.5/src/rerun/rerun_logger.cpp index 9235884cb..096e05456 100644 --- a/VisionPilot/Production_Releases/0.5/src/rerun/rerun_logger.cpp +++ b/VisionPilot/Production_Releases/0.5/src/rerun/rerun_logger.cpp @@ -82,8 +82,8 @@ RerunLogger::~RerunLogger() = default; void RerunLogger::logInference( int frame_number, const cv::Mat& input_frame, - const autoware_pov::vision::autosteer::LaneSegmentation& raw_lanes, - const autoware_pov::vision::autosteer::LaneSegmentation& filtered_lanes, + const autoware_pov::vision::egolanes::LaneSegmentation& raw_lanes, + const autoware_pov::vision::egolanes::LaneSegmentation& filtered_lanes, long inference_time_us) { #ifdef ENABLE_RERUN diff --git a/VisionPilot/Production_Releases/0.5/src/visualization/draw_lanes.cpp b/VisionPilot/Production_Releases/0.5/src/visualization/draw_lanes.cpp index c96caf588..eea5bb67c 100644 --- a/VisionPilot/Production_Releases/0.5/src/visualization/draw_lanes.cpp +++ b/VisionPilot/Production_Releases/0.5/src/visualization/draw_lanes.cpp @@ -2,7 +2,7 @@ #include #include -namespace autoware_pov::vision::autosteer +namespace autoware_pov::vision::egolanes { cv::Mat drawLanes( @@ -849,4 +849,4 @@ void drawMetricVerification( } } -} // namespace autoware_pov::vision::autosteer +} // namespace autoware_pov::vision::egolanes diff --git a/VisionPilot/Production_Releases/0.5/test/test_autosteer.cpp b/VisionPilot/Production_Releases/0.5/test/test_autosteer.cpp new file mode 100644 index 000000000..a3ccc534f --- /dev/null +++ b/VisionPilot/Production_Releases/0.5/test/test_autosteer.cpp @@ -0,0 +1,480 @@ +/** + * @file test_autosteer.cpp + * @brief Minimal test script for AutoSteer inference with visualization + * + * Tests AutoSteer model on video input and displays steering angle predictions + */ + +#include "../include/inference/onnxruntime_engine.hpp" +#include "../include/inference/autosteer_engine.hpp" +#include +#include +#include +#include +#include +#include +#include + +using namespace autoware_pov::vision::egolanes; + +// Debug helper functions +void printTensorStats(const std::vector& tensor, const std::string& name, int frame_num) { + if (tensor.empty()) { + std::cout << "[DEBUG Frame " << frame_num << "] " << name << ": EMPTY" << std::endl; + return; + } + + float min_val = *std::min_element(tensor.begin(), tensor.end()); + float max_val = *std::max_element(tensor.begin(), tensor.end()); + float sum = std::accumulate(tensor.begin(), tensor.end(), 0.0f); + float mean = sum / tensor.size(); + + float variance = 0.0f; + for (float val : tensor) { + variance += (val - mean) * (val - mean); + } + float std_dev = std::sqrt(variance / tensor.size()); + + std::cout << "[DEBUG Frame " << frame_num << "] " << name << " Stats:" << std::endl; + std::cout << " Size: " << tensor.size() << std::endl; + std::cout << " Min: " << std::fixed << std::setprecision(6) << min_val << std::endl; + std::cout << " Max: " << std::fixed << std::setprecision(6) << max_val << std::endl; + std::cout << " Mean: " << std::fixed << std::setprecision(6) << mean << std::endl; + std::cout << " Std: " << std::fixed << std::setprecision(6) << std_dev << std::endl; + std::cout << " First 10 values: "; + for (size_t i = 0; i < std::min(10UL, tensor.size()); ++i) { + std::cout << std::fixed << std::setprecision(4) << tensor[i] << " "; + } + std::cout << std::endl; +} + +void compareTensors(const std::vector& t1, const std::vector& t2, const std::string& name) { + if (t1.size() != t2.size()) { + std::cout << "[DEBUG] " << name << " comparison: SIZES DIFFER (" + << t1.size() << " vs " << t2.size() << ")" << std::endl; + return; + } + + float max_diff = 0.0f; + float sum_diff = 0.0f; + int num_different = 0; + + for (size_t i = 0; i < t1.size(); ++i) { + float diff = std::abs(t1[i] - t2[i]); + if (diff > 1e-6f) { + num_different++; + } + max_diff = std::max(max_diff, diff); + sum_diff += diff; + } + + float mean_diff = sum_diff / t1.size(); + + std::cout << "[DEBUG] " << name << " Comparison:" << std::endl; + std::cout << " Max difference: " << std::fixed << std::setprecision(6) << max_diff << std::endl; + std::cout << " Mean difference: " << std::fixed << std::setprecision(6) << mean_diff << std::endl; + std::cout << " Different elements: " << num_different << " / " << t1.size() + << " (" << (100.0f * num_different / t1.size()) << "%)" << std::endl; + + if (num_different == 0) { + std::cout << " WARNING: Tensors are IDENTICAL!" << std::endl; + } +} + +int main(int argc, char** argv) { + if (argc < 4) { + std::cerr << "Usage: " << argv[0] << " [cache_dir]\n"; + std::cerr << "\nExample:\n"; + std::cerr << " " << argv[0] << " video.mp4 egolanes.onnx autosteer.onnx ./trt_cache\n"; + return 1; + } + + std::string video_path = argv[1]; + std::string egolanes_model = argv[2]; + std::string autosteer_model = argv[3]; + std::string cache_dir = (argc >= 5) ? argv[4] : "./trt_cache"; + + // Default: TensorRT FP16, but allow override + std::string provider = (argc >= 6) ? argv[5] : "tensorrt"; + std::string precision = "fp16"; + int device_id = 0; + + std::cout << "========================================\n"; + std::cout << "AutoSteer Test Script\n"; + std::cout << "========================================\n"; + std::cout << "Video: " << video_path << "\n"; + std::cout << "EgoLanes Model: " << egolanes_model << "\n"; + std::cout << "AutoSteer Model: " << autosteer_model << "\n"; + std::cout << "Provider: " << provider << " | Precision: " << precision << "\n"; + std::cout << "Cache: " << cache_dir << "\n"; + std::cout << "========================================\n\n"; + + // Open video + cv::VideoCapture cap(video_path); + if (!cap.isOpened()) { + std::cerr << "Error: Could not open video: " << video_path << std::endl; + return 1; + } + + int frame_width = static_cast(cap.get(cv::CAP_PROP_FRAME_WIDTH)); + int frame_height = static_cast(cap.get(cv::CAP_PROP_FRAME_HEIGHT)); + double fps = cap.get(cv::CAP_PROP_FPS); + int total_frames = static_cast(cap.get(cv::CAP_PROP_FRAME_COUNT)); + + std::cout << "Video: " << frame_width << "x" << frame_height + << " @ " << fps << " fps, " << total_frames << " frames\n\n"; + + // Initialize EgoLanes engine + std::cout << "Loading EgoLanes model..." << std::endl; + EgoLanesOnnxEngine egolanes_engine(egolanes_model, provider, precision, device_id, cache_dir); + std::cout << "EgoLanes initialized!\n" << std::endl; + + // Initialize AutoSteer engine + std::cout << "Loading AutoSteer model..." << std::endl; + AutoSteerOnnxEngine autosteer_engine(autosteer_model, provider, precision, device_id, cache_dir); + std::cout << "AutoSteer initialized!\n" << std::endl; + + // Temporal buffer for AutoSteer + const int EGOLANES_TENSOR_SIZE = 3 * 80 * 160; // 38,400 floats + std::vector> tensor_buffer; + tensor_buffer.reserve(2); + std::vector autosteer_input_buffer(6 * 80 * 160); // Pre-allocated + + // Debug: Verify tensor size + std::cout << "[DEBUG] Expected EgoLanes tensor size: " << EGOLANES_TENSOR_SIZE << " floats" << std::endl; + std::cout << "[DEBUG] Expected AutoSteer input size: " << (6 * 80 * 160) << " floats" << std::endl; + + // Get actual tensor shape from EgoLanes (also serves as warm-up) + cv::Mat dummy_frame(720, 1280, CV_8UC3, cv::Scalar(128, 128, 128)); + if (dummy_frame.rows > 420) { + dummy_frame = dummy_frame(cv::Rect(0, 420, dummy_frame.cols, dummy_frame.rows - 420)); + } + egolanes_engine.inference(dummy_frame, 0.0f); + + // Warm-up AutoSteer + if (provider == "tensorrt") { + std::cout << "Warming up AutoSteer engine (building TensorRT engine)..." << std::endl; + std::vector dummy_input(6 * 80 * 160, 0.5f); + autosteer_engine.inference(dummy_input); + std::cout << "Warm-up complete!\n" << std::endl; + } + auto tensor_shape = egolanes_engine.getTensorShape(); + std::cout << "[DEBUG] Actual EgoLanes output shape: ["; + for (size_t i = 0; i < tensor_shape.size(); ++i) { + std::cout << tensor_shape[i]; + if (i < tensor_shape.size() - 1) std::cout << ", "; + } + std::cout << "]" << std::endl; + + int actual_tensor_size = 1; + for (size_t i = 1; i < tensor_shape.size(); ++i) { + actual_tensor_size *= static_cast(tensor_shape[i]); + } + std::cout << "[DEBUG] Actual tensor size (excluding batch): " << actual_tensor_size << " floats" << std::endl; + + if (actual_tensor_size != EGOLANES_TENSOR_SIZE) { + std::cerr << "[ERROR] Tensor size mismatch! Expected " << EGOLANES_TENSOR_SIZE + << ", got " << actual_tensor_size << std::endl; + std::cerr << "[ERROR] This will cause incorrect data copying!" << std::endl; + } + std::cout << std::endl; + + // OpenCV window + cv::namedWindow("AutoSteer Test", cv::WINDOW_NORMAL); + cv::resizeWindow("AutoSteer Test", 1280, 720); + + int frame_number = 0; + cv::Mat frame; + std::vector prev_tensor; // For comparison + + std::cout << "Processing video... (Press 'q' to quit, 's' to step frame-by-frame)" << std::endl; + std::cout << "Debug mode: ON (will print detailed statistics)\n" << std::endl; + + while (cap.read(frame) && !frame.empty()) { + // Crop top 420 pixels (same as main pipeline) + if (frame.rows > 420) { + frame = frame(cv::Rect(0, 420, frame.cols, frame.rows - 420)); + } + + // Run EgoLanes inference + LaneSegmentation lanes = egolanes_engine.inference(frame, 0.0f); + + // Get raw tensor for AutoSteer + const float* raw_tensor = egolanes_engine.getRawTensorData(); + if (raw_tensor == nullptr) { + std::cerr << "[ERROR Frame " << frame_number << "] Raw tensor pointer is NULL!" << std::endl; + continue; + } + + // Verify tensor shape + auto tensor_shape = egolanes_engine.getTensorShape(); + int actual_size = 1; + for (size_t i = 1; i < tensor_shape.size(); ++i) { + actual_size *= static_cast(tensor_shape[i]); + } + + // Use actual size instead of hardcoded size + int copy_size = std::min(EGOLANES_TENSOR_SIZE, actual_size); + std::vector current_tensor(copy_size); + std::memcpy(current_tensor.data(), raw_tensor, copy_size * sizeof(float)); + + // Debug: Print tensor statistics + if (frame_number < 3 || frame_number % 30 == 0) { + printTensorStats(current_tensor, "EgoLanes Raw Tensor", frame_number); + } + + // Debug: Compare with previous frame + if (!prev_tensor.empty() && prev_tensor.size() == current_tensor.size()) { + if (frame_number < 3 || frame_number % 30 == 0) { + compareTensors(prev_tensor, current_tensor, "EgoLanes Frame-to-Frame"); + } + } + prev_tensor = current_tensor; + + // Store in buffer + tensor_buffer.push_back(current_tensor); + + // Run AutoSteer when we have 2 frames + float steering_angle = 0.0f; + bool autosteer_valid = false; + + if (tensor_buffer.size() >= 2) { + // Keep only last 2 frames + if (tensor_buffer.size() > 2) { + tensor_buffer.erase(tensor_buffer.begin()); + } + + // Debug: Check buffer sizes + if (frame_number < 3 || frame_number % 30 == 0) { + std::cout << "[DEBUG Frame " << frame_number << "] Buffer status:" << std::endl; + std::cout << " Buffer size: " << tensor_buffer.size() << std::endl; + std::cout << " t-1 tensor size: " << tensor_buffer[0].size() << std::endl; + std::cout << " t tensor size: " << tensor_buffer[1].size() << std::endl; + printTensorStats(tensor_buffer[0], "Buffer t-1", frame_number); + printTensorStats(tensor_buffer[1], "Buffer t", frame_number); + } + + // Concatenate t-1 and t + int t1_size = static_cast(tensor_buffer[0].size()); + int t_size = static_cast(tensor_buffer[1].size()); + int total_size = t1_size + t_size; + + if (total_size > static_cast(autosteer_input_buffer.size())) { + std::cerr << "[ERROR Frame " << frame_number << "] Buffer overflow! " + << "Need " << total_size << " floats, have " + << autosteer_input_buffer.size() << std::endl; + continue; + } + + std::memcpy(autosteer_input_buffer.data(), + tensor_buffer[0].data(), // t-1 + t1_size * sizeof(float)); + + std::memcpy(autosteer_input_buffer.data() + t1_size, + tensor_buffer[1].data(), // t + t_size * sizeof(float)); + + // Debug: Print concatenated input statistics + std::vector autosteer_input_vec(autosteer_input_buffer.begin(), + autosteer_input_buffer.begin() + total_size); + if (frame_number < 3 || frame_number % 30 == 0) { + printTensorStats(autosteer_input_vec, "AutoSteer Input (concatenated)", frame_number); + } + + // Run AutoSteer + std::vector autosteer_input_for_inference(autosteer_input_buffer.begin(), + autosteer_input_buffer.begin() + total_size); + steering_angle = autosteer_engine.inference(autosteer_input_for_inference); + autosteer_valid = true; + + // Debug: ALWAYS print logits for first few frames to diagnose the issue + if (frame_number < 5 || frame_number % 30 == 0) { + std::vector logits = autosteer_engine.getRawOutputLogits(); + if (!logits.empty()) { + std::cout << "[DEBUG Frame " << frame_number << "] AutoSteer Output Logits:" << std::endl; + std::cout << " Size: " << logits.size() << " (expected 61)" << std::endl; + + float min_logit = *std::min_element(logits.begin(), logits.end()); + float max_logit = *std::max_element(logits.begin(), logits.end()); + float sum_logit = std::accumulate(logits.begin(), logits.end(), 0.0f); + float mean_logit = sum_logit / logits.size(); + + std::cout << " Min: " << std::fixed << std::setprecision(6) << min_logit << std::endl; + std::cout << " Max: " << std::fixed << std::setprecision(6) << max_logit << std::endl; + std::cout << " Mean: " << std::fixed << std::setprecision(6) << mean_logit << std::endl; + std::cout << " ALL 61 logits: "; + for (size_t i = 0; i < logits.size(); ++i) { + std::cout << std::fixed << std::setprecision(3) << logits[i]; + if (i < logits.size() - 1) std::cout << " "; + } + std::cout << std::endl; + + // Find argmax + int argmax_idx = 0; + float argmax_val = logits[0]; + for (size_t i = 1; i < logits.size(); ++i) { + if (logits[i] > argmax_val) { + argmax_val = logits[i]; + argmax_idx = static_cast(i); + } + } + + std::cout << " Argmax index: " << argmax_idx << " (steering = " + << (argmax_idx - 30) << " deg)" << std::endl; + std::cout << " Argmax value: " << std::fixed << std::setprecision(6) + << argmax_val << std::endl; + + // Show top 5 classes by probability + std::vector> logit_pairs; + for (size_t i = 0; i < logits.size(); ++i) { + logit_pairs.push_back({logits[i], static_cast(i)}); + } + std::sort(logit_pairs.rbegin(), logit_pairs.rend()); // Sort descending + + std::cout << " Top 5 classes:" << std::endl; + for (int i = 0; i < 5 && i < static_cast(logit_pairs.size()); ++i) { + int class_idx = logit_pairs[i].second; + float value = logit_pairs[i].first; + std::cout << " Class " << class_idx << " (steering=" << (class_idx - 30) + << " deg): " << std::fixed << std::setprecision(6) << value << std::endl; + } + + // Show first 10 and last 10 logits + std::cout << " First 10 logits: "; + for (size_t i = 0; i < std::min(10UL, logits.size()); ++i) { + std::cout << std::fixed << std::setprecision(3) << logits[i] << " "; + } + std::cout << std::endl; + + std::cout << " Last 10 logits: "; + for (size_t i = std::max(0UL, logits.size() - 10); i < logits.size(); ++i) { + std::cout << std::fixed << std::setprecision(3) << logits[i] << " "; + } + std::cout << std::endl; + + // Check if all logits are the same (would indicate a problem) + bool all_same = true; + for (size_t i = 1; i < logits.size(); ++i) { + if (std::abs(logits[i] - logits[0]) > 1e-6f) { + all_same = false; + break; + } + } + if (all_same) { + std::cout << " [WARNING] All logits are IDENTICAL! Model not responding to input." << std::endl; + } + } + } + + std::cout << "[Frame " << frame_number << "] " + << "Steering: " << std::fixed << std::setprecision(2) + << steering_angle << " deg" << std::endl; + + // Debug: Track steering angles + static std::vector steering_history; + steering_history.push_back(steering_angle); + if (steering_history.size() > 10) { + steering_history.erase(steering_history.begin()); + } + + if (frame_number < 3 || frame_number % 30 == 0) { + std::cout << "[DEBUG Frame " << frame_number << "] Recent steering angles: "; + for (float s : steering_history) { + std::cout << std::fixed << std::setprecision(2) << s << " "; + } + std::cout << std::endl; + + // Check if steering is constant + if (steering_history.size() >= 3) { + bool all_same = true; + for (size_t i = 1; i < steering_history.size(); ++i) { + if (std::abs(steering_history[i] - steering_history[0]) > 0.01f) { + all_same = false; + break; + } + } + if (all_same) { + std::cout << "[WARNING Frame " << frame_number + << "] Steering angle is CONSTANT! All recent values are: " + << steering_history[0] << std::endl; + } + } + } + } else { + std::cout << "[Frame " << frame_number << "] " + << "Skipped (waiting for temporal buffer, size=" + << tensor_buffer.size() << ")" << std::endl; + } + + // Visualize + cv::Mat vis_frame = frame.clone(); + + // Draw steering angle text + std::stringstream ss; + ss << "Frame: " << frame_number; + if (autosteer_valid) { + ss << " | Steering: " << std::fixed << std::setprecision(2) << steering_angle << " deg"; + } else { + ss << " | Waiting for buffer..."; + } + + // Draw text with background for visibility + int baseline = 0; + cv::Size text_size = cv::getTextSize(ss.str(), cv::FONT_HERSHEY_SIMPLEX, 1.0, 2, &baseline); + cv::rectangle(vis_frame, + cv::Point(10, 10), + cv::Point(20 + text_size.width, 50 + text_size.height), + cv::Scalar(0, 0, 0), -1); + cv::putText(vis_frame, ss.str(), cv::Point(15, 40), + cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(0, 255, 0), 2); + + // Draw steering angle indicator (simple bar) + if (autosteer_valid) { + int center_x = vis_frame.cols / 2; + int bar_width = 400; + int bar_height = 30; + int bar_y = vis_frame.rows - 60; + + // Background bar + cv::rectangle(vis_frame, + cv::Point(center_x - bar_width/2, bar_y), + cv::Point(center_x + bar_width/2, bar_y + bar_height), + cv::Scalar(100, 100, 100), -1); + + // Steering indicator (-30 to +30 degrees mapped to bar width) + float normalized = steering_angle / 30.0f; // -1.0 to +1.0 + normalized = std::max(-1.0f, std::min(1.0f, normalized)); // Clamp + int indicator_x = center_x + static_cast(normalized * bar_width / 2); + + // Color: green for center, red for extremes + cv::Scalar color = (std::abs(normalized) < 0.3) ? + cv::Scalar(0, 255, 0) : cv::Scalar(0, 0, 255); + + cv::circle(vis_frame, cv::Point(indicator_x, bar_y + bar_height/2), 15, color, -1); + cv::line(vis_frame, + cv::Point(center_x, bar_y - 5), + cv::Point(center_x, bar_y + bar_height + 5), + cv::Scalar(255, 255, 255), 2); + } + + cv::imshow("AutoSteer Test", vis_frame); + + int key = cv::waitKey(1) & 0xFF; + if (key == 'q') { + std::cout << "\nQuitting..." << std::endl; + break; + } else if (key == 's') { + // Step mode: wait for key press + cv::waitKey(0); + } + + frame_number++; + } + + cap.release(); + cv::destroyAllWindows(); + + std::cout << "\nProcessed " << frame_number << " frames." << std::endl; + return 0; +} +