Merge pull request #205 from pranavreddy23/pathfinder

Pathfinder

Author: m-zain-khawaja

VisionPilot/Production_Releases/0.5/CMakeLists.txt

@@ -7,6 +7,7 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON)
 # Find dependencies
 find_package(OpenCV REQUIRED)
 find_package(Threads REQUIRED)
+find_package(Eigen3 REQUIRED)
 
 # ONNX Runtime - user must set ONNXRUNTIME_ROOT
 if(NOT DEFINED ENV{ONNXRUNTIME_ROOT})
@@ -35,6 +36,7 @@ include_directories(
     include
     ${OpenCV_INCLUDE_DIRS}
     ${ONNXRUNTIME_INCLUDE_DIR}
+    ${EIGEN3_INCLUDE_DIR}
 )
 
 # AutoSteer inference backend
@@ -80,6 +82,25 @@ target_link_libraries(camera_utils
     ${OpenCV_LIBS}
 )
 
+# Path planning module (polynomial fitting + Bayes filter)
+add_library(path_planning
+    src/path_planning/estimator.cpp
+    src/path_planning/poly_fit.cpp
+    src/path_planning/path_finder.cpp
+)
+target_link_libraries(path_planning
+    ${OpenCV_LIBS}
+    Eigen3::Eigen
+)
+
+# Steering control module
+add_library(steering_control
+    src/steering_control/steering_controller.cpp
+)
+target_link_libraries(steering_control
+    # No external dependencies, pure C++ math
+)
+
 # Rerun SDK (optional, for visualization/logging)
 option(ENABLE_RERUN "Enable Rerun logging and visualization" OFF)
 
@@ -152,6 +173,8 @@ set(AUTOSTEER_LIBS
     autosteer_lane_filtering
     autosteer_lane_tracking
     camera_utils
+    path_planning
+    steering_control
     ${OpenCV_LIBS}
     pthread
 )

VisionPilot/Production_Releases/0.5/include/lane_tracking/lane_tracking.hpp

@@ -14,6 +14,11 @@ struct BEVVisuals {
     std::vector<double> bev_left_coeffs;  // BEV egoleft coeffs
     std::vector<double> bev_right_coeffs; // BEV egoright coeffs
     std::vector<double> bev_center_coeffs;// BEV drivable corridor coeffs
+    
+    // BEV lane points in pixel space (for PathFinder)
+    std::vector<cv::Point2f> bev_left_pts;   // Left lane points in BEV pixels
+    std::vector<cv::Point2f> bev_right_pts;  // Right lane points in BEV pixels
+    
     double last_valid_width_pixels = 0.0; // Width bar
     bool valid = false;
 };

VisionPilot/Production_Releases/0.5/include/path_planning/estimator.hpp

@@ -0,0 +1,35 @@
+/**
+ * @file estimator.hpp
+ * @brief Bayes filter for temporal tracking of lane parameters
+ * 
+ * Ported from PATHFINDER ROS2 package (no ROS2 dependencies)
+ */
+
+#pragma once
+
+#include <vector>
+#include <array>
+#include <cmath>
+
+constexpr size_t STATE_DIM = 14;
+
+struct Gaussian
+{
+    double mean;
+    double variance;
+};
+
+class Estimator
+{
+private:
+    std::array<Gaussian, STATE_DIM> state;
+    std::vector<std::pair<size_t, size_t>> fusion_rules;
+
+public:
+    void initialize(const std::array<Gaussian, STATE_DIM> &init_state);
+    void predict(const std::array<Gaussian, STATE_DIM> &process);
+    void update(const std::array<Gaussian, STATE_DIM> &measurement);
+    void configureFusionGroups(const std::vector<std::pair<size_t, size_t>> &rules);
+    const std::array<Gaussian, STATE_DIM> &getState() const;
+};
+

VisionPilot/Production_Releases/0.5/include/path_planning/path_finder.hpp

@@ -0,0 +1,114 @@
+/**
+ * @file path_finder.hpp
+ * @brief Standalone PathFinder module (no ROS2)
+ * 
+ * Integrates polynomial fitting and Bayes filter for robust lane tracking
+ */
+
+#pragma once
+
+#include "path_planning/poly_fit.hpp"
+#include "path_planning/estimator.hpp"
+#include <opencv2/opencv.hpp>
+#include <vector>
+#include <array>
+#include <string>
+
+namespace autoware_pov::vision::path_planning {
+
+/**
+ * @brief Output of PathFinder module
+ */
+struct PathFinderOutput
+{
+    // Fused metrics from Bayes filter (temporally smoothed)
+    double cte;          // Cross-track error (meters)
+    double yaw_error;    // Yaw error (radians)
+    double curvature;    // Curvature (1/meters)
+    double lane_width;   // Corridor width (meters)
+    
+    // Confidence (variance from Bayes filter)
+    double cte_variance;
+    double yaw_variance;
+    double curv_variance;
+    double lane_width_variance;
+    
+    // Raw polynomial coefficients (for debugging/visualization)
+    std::array<double, 3> left_coeff;   // x = c0*y² + c1*y + c2
+    std::array<double, 3> right_coeff;  // x = c0*y² + c1*y + c2
+    std::array<double, 3> center_coeff; // Average of left and right
+    
+    // Individual curve metrics (before fusion)
+    double left_cte;
+    double left_yaw_error;
+    double left_curvature;
+    double right_cte;
+    double right_yaw_error;
+    double right_curvature;
+    
+    // Validity flags
+    bool left_valid;
+    bool right_valid;
+    bool fused_valid;
+};
+
+/**
+ * @brief PathFinder for lane tracking and trajectory estimation
+ * 
+ * Features:
+ * - Polynomial fitting to BEV lane points (in meters)
+ * - Temporal smoothing via Bayes filter
+ * - Robust fusion of left/right lanes
+ * 
+ * Note: Expects BEV points already in metric coordinates (meters)
+ */
+class PathFinder
+{
+public:
+    /**
+     * @brief Initialize PathFinder
+     * @param default_lane_width Default lane width in meters (default: 4.0)
+     */
+    explicit PathFinder(double default_lane_width = 4.0);
+    
+    /**
+     * @brief Update with new lane detections
+     * 
+     * @param left_pts_bev Left lane points in BEV meters (x=lateral, y=longitudinal)
+     * @param right_pts_bev Right lane points in BEV meters
+     * @return PathFinder output (fused metrics + individual curves)
+     */
+    PathFinderOutput update(
+        const std::vector<cv::Point2f>& left_pts_bev,
+        const std::vector<cv::Point2f>& right_pts_bev);
+    
+    /**
+     * @brief Get current tracked state
+     * @return Current Bayes filter state (all 14 variables)
+     */
+    const std::array<Gaussian, STATE_DIM>& getState() const;
+    
+    /**
+     * @brief Reset Bayes filter to initial state
+     */
+    void reset();
+
+private:
+    double default_lane_width_;
+    Estimator bayes_filter_;
+    
+    // Tuning parameters (from PATHFINDER)
+    const double PROC_SD = 0.5;          // Process noise
+    const double STD_M_CTE = 0.1;        // CTE measurement std (m)
+    const double STD_M_YAW = 0.01;       // Yaw measurement std (rad)
+    const double STD_M_CURV = 0.1;       // Curvature measurement std (1/m)
+    const double STD_M_WIDTH = 0.01;     // Width measurement std (m)
+    
+    /**
+     * @brief Initialize Bayes filter
+     */
+    void initializeBayesFilter();
+};
+
+} // namespace autoware_pov::vision::path_planning
+

VisionPilot/Production_Releases/0.5/include/path_planning/poly_fit.hpp

@@ -0,0 +1,56 @@
+/**
+ * @file poly_fit.hpp
+ * @brief Polynomial fitting and lane curve utilities
+ * 
+ * Extracted from PATHFINDER package's path_finder.hpp/cpp
+ */
+
+#pragma once
+
+#include <opencv2/opencv.hpp>
+#include <array>
+#include <vector>
+
+namespace autoware_pov::vision::path_planning {
+
+/**
+ * @brief Fitted quadratic curve: x = c0*y² + c1*y + c2
+ * 
+ * Vehicle-centric BEV coordinates:
+ * - x: lateral position (positive = right)
+ * - y: longitudinal position (positive = forward)
+ */
+struct FittedCurve
+{
+    std::array<double, 3> coeff; // [c0, c1, c2] for x = c0*y² + c1*y + c2
+    double cte;                  // Cross-track error at y=0 (meters)
+    double yaw_error;            // Yaw error at y=0 (radians)
+    double curvature;            // Curvature at y=0 (meters^-1)
+    
+    FittedCurve();
+    explicit FittedCurve(const std::array<double, 3> &coeff);
+};
+
+/**
+ * @brief Fit quadratic polynomial to BEV points
+ * 
+ * Fits x = c0*y² + c1*y + c2 using least squares
+ * 
+ * @param points BEV points in meters (x=lateral, y=longitudinal)
+ * @return Coefficients [c0, c1, c2], or NaN if insufficient points
+ */
+std::array<double, 3> fitQuadPoly(const std::vector<cv::Point2f> &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
+

VisionPilot/Production_Releases/0.5/include/steering_control/steering_controller.hpp

@@ -0,0 +1,60 @@
+/**
+ * @file steering_controller.hpp
+ * @brief Steering controller for path following
+ * 
+ * Combines Stanley controller (PI on CTE) with curvature feedforward
+ * Input: CTE, yaw_error, forward_velocity, curvature (from PathFinder)
+ * Output: Steering angle (radians)
+ */
+
+#pragma once
+
+#include <iostream>
+#include <cmath>
+
+namespace autoware_pov::vision::steering_control {
+
+/**
+ * @brief Default steering controller parameters
+ */
+
+namespace SteeringControllerDefaults {    
+    constexpr double K_P = 0.8;                // Proportional gain for yaw error
+    constexpr double K_I = 1.6;                // Integral gain for CTE (Stanley controller)
+    constexpr double K_D = 1.0;             // Derivative gain for yaw error
+    constexpr double K_S = 1.0;              //proportionality constant for curvature  
+};
+
+class SteeringController
+{
+public:
+    /**
+     * @brief Constructor
+     * @param K_p Proportional gain for yaw error
+     * @param K_i Integral gain for CTE (Stanley controller)
+     * @param K_d Derivative gain for yaw error
+     * @param K_S Proportionality constant for curvature feedforward
+     */
+    SteeringController(
+                       double K_p,
+                       double K_i,
+                       double K_d,
+                       double K_S);
+                      
+    
+    /**
+     * @brief Compute steering angle
+     * @param cte Cross-track error (meters)
+     * @param yaw_error Yaw error (radians)
+     * @param feed_forward_steering_estimate (angle in degrees)
+     * @return Steering angle (radians)
+     */
+    double computeSteering(double cte, double yaw_error, double feed_forward_steering_estimate);
+
+private:
+    double K_p, K_i, K_d, K_S;
+    double prev_yaw_error;
+};
+
+} // namespace autoware_pov::vision::steering_control
+

VisionPilot/Production_Releases/0.5/include/visualization/draw_lanes.hpp

@@ -77,6 +77,16 @@ void drawBEVVis(
   const BEVVisuals& bev_data
 );
 
+/**
+ * @brief Debug function to verify Pixel -> Meter conversion
+ * Draws the metric polynomials (projected back to pixels) on top of the BEV image.
+ */
+void drawMetricVerification(
+    cv::Mat& bev_image,
+    const std::vector<double>& left_metric_coeffs,
+    const std::vector<double>& right_metric_coeffs
+);
+
 }  // namespace autoware_pov::vision::autosteer
 
 #endif  // AUTOWARE_POV_VISION_AUTOSTEER_DRAW_LANES_HPP_