Skip to content

Modify steering visualization #207

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
m-zain-khawaja merged 4 commits into from
Dec 15, 2025
Merged

Modify steering visualization #207

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
2abe5d5
Modify steering visualization
atanasko Dec 14, 2025
e5f18d6
Merge branch 'autowarefoundation:main' into main
atanasko Dec 14, 2025
a9c1b62
Update PID controll
atanasko Dec 14, 2025
2960ff7
Add low pass steering controll filtering
atanasko Dec 14, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions VisionPilot/Production_Releases/0.5/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 3.16)
project(egolanes_pipeline LANGUAGES CXX)

Check warning on line 2 in VisionPilot/Production_Releases/0.5/CMakeLists.txt

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (egolanes)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
Expand All @@ -9,21 +9,21 @@
find_package(Threads REQUIRED)
find_package(Eigen3 REQUIRED)

# ONNX Runtime - user must set ONNXRUNTIME_ROOT

Check warning on line 12 in VisionPilot/Production_Releases/0.5/CMakeLists.txt

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (ONNXRUNTIME)
if(NOT DEFINED ENV{ONNXRUNTIME_ROOT})

Check warning on line 13 in VisionPilot/Production_Releases/0.5/CMakeLists.txt

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (ONNXRUNTIME)
message(FATAL_ERROR "Please set ONNXRUNTIME_ROOT environment variable to ONNX Runtime installation directory\n"

Check warning on line 14 in VisionPilot/Production_Releases/0.5/CMakeLists.txt

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (ONNXRUNTIME)
"Example: export ONNXRUNTIME_ROOT=/path/to/onnxruntime-linux-x64-gpu-1.22.0")

Check warning on line 15 in VisionPilot/Production_Releases/0.5/CMakeLists.txt

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (onnxruntime)

Check warning on line 15 in VisionPilot/Production_Releases/0.5/CMakeLists.txt

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (ONNXRUNTIME)
endif()

set(ONNXRUNTIME_ROOT $ENV{ONNXRUNTIME_ROOT})

Check warning on line 18 in VisionPilot/Production_Releases/0.5/CMakeLists.txt

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (ONNXRUNTIME)
set(ONNXRUNTIME_INCLUDE_DIR ${ONNXRUNTIME_ROOT}/include)
set(ONNXRUNTIME_LIB_DIR ${ONNXRUNTIME_ROOT}/lib)

# Find library (handle versioned .so files)
file(GLOB ONNXRUNTIME_LIBRARY "${ONNXRUNTIME_LIB_DIR}/libonnxruntime.so*")

Check warning on line 23 in VisionPilot/Production_Releases/0.5/CMakeLists.txt

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (libonnxruntime)
list(GET ONNXRUNTIME_LIBRARY 0 ONNXRUNTIME_LIBRARY)

if(NOT EXISTS ${ONNXRUNTIME_INCLUDE_DIR}/onnxruntime_cxx_api.h)

Check warning on line 26 in VisionPilot/Production_Releases/0.5/CMakeLists.txt

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (onnxruntime)
message(FATAL_ERROR "ONNX Runtime headers not found at: ${ONNXRUNTIME_INCLUDE_DIR}")
endif()

Expand All @@ -40,7 +40,7 @@
)

# EgoLanes inference backend
add_library(egolanes_inference

Check warning on line 43 in VisionPilot/Production_Releases/0.5/CMakeLists.txt

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (egolanes)
src/inference/onnxruntime_session.cpp
src/inference/onnxruntime_engine.cpp
src/inference/autosteer_engine.cpp
Expand Down Expand Up @@ -162,6 +162,9 @@
)

# Main executable
file(COPY ${CMAKE_SOURCE_DIR}/images
DESTINATION ${CMAKE_BINARY_DIR})

add_executable(egolanes_pipeline
main.cpp
)
Expand Down
50 changes: 34 additions & 16 deletions VisionPilot/Production_Releases/0.5/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
# VisionPilot 0.5 - EgoLanes Production Release
# VisionPilot 0.5 - AutoSteer Production Release

This release enables autonomous steering using the EgoLanes neural network to detect lane lines and navigate roads at a predetermined, desired speed.
This release enables autonomous steering using the EgoLanes and AutoSteer neural networks to detect lane lines determine
steering angle and navigate roads at a predetermined, desired speed.

This includes autonomous lane keeping with cruise control.

Expand All @@ -10,26 +11,22 @@ Multi-threaded lane detection inference system with ONNX Runtime backend.

### Quick Start

1. **Set ONNX Runtime path**:
[Download](https://github.com/microsoft/onnxruntime/releases) ONNX Runtime for the appropriate CUDA version and OS.

**Set ONNX Runtime path**

Unpack the ONNX runtime archive and set `ONNXRUNTIME_ROOT` to point to the directory as for example:

```bash
export ONNXRUNTIME_ROOT=/path/to/onnxruntime-linux-x64-gpu-1.22.0
```

2. **Build**:
```bash
mkdir -p build && cd build
cmake ..
make -j$(nproc)
cd ..
```
_Note_: For Jetson AGX download appropriate ONNX runetime from [Jetson Zoo](https://elinux.org/Jetson_Zoo#ONNX_Runtime).

3. **Configure and Run**:
```bash
# Edit run.sh to set paths and options
./run.sh
```
**Build**

### Directory Structure
[Download](https://github.com/autowarefoundation/autoware.privately-owned-vehicles.git) VisionPilot source code.
Navigate to `VisionPilot/Production_Releases/0.5` subdirectory which looks like:

```
0.5/
Expand All @@ -46,6 +43,27 @@ cd ..
└── run.sh # Runner script
```

and create `build` subdirectory:

```bash
mkdir -p build && cd build
```

build the code

```bash
cmake ../
make -j$(nproc)
cd ..
```

**Configure and Run**

```bash
# Edit run.sh to set paths and options
./run.sh
```

### Configuration (run.sh)

- `VIDEO_PATH`: Input video file
Expand Down
Binary file added VisionPilot/Production_Releases/0.5/images/wheel_green.png
View file
Open in desktop
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
3 changes: 3 additions & 0 deletions VisionPilot/Production_Releases/0.5/include/visualization/draw_lanes.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,6 +87,9 @@ void drawMetricVerification(
const std::vector<double>& right_metric_coeffs
);

cv::Mat rotateSteeringWheel(const cv::Mat& img, float steering_angle_deg);
void visualizeSteering(cv::Mat& img, float steering_angle, const cv::Mat& steeringWheelImage);

} // namespace autoware_pov::vision::egolanes

#endif // AUTOWARE_POV_VISION_EGOLANES_DRAW_LANES_HPP_
226 changes: 117 additions & 109 deletions VisionPilot/Production_Releases/0.5/main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -300,13 +300,13 @@ void inferenceThread(

auto t_inference_start = steady_clock::now();

// Crop tf.frame 420 pixels top
tf.frame = tf.frame(cv::Rect(
0,
420,
tf.frame.cols,
tf.frame.rows - 420
));
// // Crop tf.frame 420 pixels top
// tf.frame = tf.frame(cv::Rect(
// 0,
// 420,
// tf.frame.cols,
// tf.frame.rows - 420
// ));

image_buffer.push_back(tf.frame.clone()); // Store a copy of the cropped frame

Expand Down Expand Up @@ -477,7 +477,7 @@ 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 PID steering angle
result.steering_angle = steering_angle * 180.0 / M_PI; // 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());
Expand All @@ -501,8 +501,8 @@ void displayThread(
)
{
// Visualization setup
int window_width = 1600;
int window_height = 1080;
int window_width = 640;
int window_height = 320;
if (enable_viz) {
cv::namedWindow(
"EgoLanes Inference",
Expand Down Expand Up @@ -539,6 +539,9 @@ void displayThread(
std::cerr << "Error: could not open " << csv_log_path << " for writing" << std::endl;
}

std::string image_path = "images/wheel_green.png";
cv::Mat steeringWheelImg = cv::imread(image_path, cv::IMREAD_UNCHANGED);

while (running.load()) {
InferenceResult result;
if (!queue.try_pop(result)) {
Expand All @@ -560,113 +563,118 @@ void displayThread(
// - Polyfit lanes (final prod)
// - BEV vis
cv::Mat view_debug = result.frame.clone();
cv::Mat view_final = result.frame.clone();
cv::Mat view_bev(
640,
640,
CV_8UC3,
cv::Scalar(0,0,0)
);
// cv::Mat view_final = result.frame.clone();
// cv::Mat view_bev(
// 640,
// 640,
// CV_8UC3,
// cv::Scalar(0,0,0)
// );

cv::Mat rotatedSteeringWheelImg = rotateSteeringWheel(steeringWheelImg, result.steering_angle);
visualizeSteering(view_debug, result.steering_angle, rotatedSteeringWheelImg);

// std::cout<< "************ " << result.steering_angle << std::endl;

// 2. Draw 3 views
drawRawMasksInPlace(
view_debug,
result.lanes
);
drawPolyFitLanesInPlace(
view_final,
result.lanes
);
drawBEVVis(
view_bev,
result.frame,
result.metrics.bev_visuals
);

// Draw Metric Debug (projected back to pixels) - only if path is valid
if (result.path_output.fused_valid) {
std::vector<double> left_coeffs(result.path_output.left_coeff.begin(), result.path_output.left_coeff.end());
std::vector<double> right_coeffs(result.path_output.right_coeff.begin(), result.path_output.right_coeff.end());
autoware_pov::vision::egolanes::drawMetricVerification(
view_bev,
left_coeffs,
right_coeffs
);
}

// 3. View layout handling
// Layout:
// | [Debug] | [ BEV (640x640) ]
// | [Final] | [ Black Space ]

// Left col: debug (top) + final (bottom)
cv::Mat left_col;
cv::vconcat(
view_debug,
view_final,
left_col
);

float left_aspect = static_cast<float>(left_col.cols) / left_col.rows;
int target_left_w = static_cast<int>(window_height * left_aspect);
cv::resize(
left_col,
left_col,
cv::Size(target_left_w, window_height)
);

// Right col: BEV (stretched to match height)
// Black canvas matching left col height
cv::Mat right_col = cv::Mat::zeros(
window_height,
640,
view_bev.type()
);
// Prep BEV
cv::Rect top_roi(
0, 0,
view_bev.cols,
view_bev.rows
);
view_bev.copyTo(right_col(top_roi));

// Final stacked view
cv::Mat stacked_view;
cv::hconcat(
left_col,
right_col,
stacked_view
);

// Initialize video writer on first frame
if (save_video && !video_writer_initialized) {
// Use H.264 for better performance and smaller file size
// XVID is slower and creates larger files
int fourcc = cv::VideoWriter::fourcc('a', 'v', 'c', '1'); // H.264
video_writer.open(
output_video_path,
fourcc,
10.0,
stacked_view.size(),
true
);

if (video_writer.isOpened()) {
std::cout << "Video writer initialized (H.264): " << stacked_view.cols
<< "x" << stacked_view.rows << " @ 10 fps" << std::endl;
video_writer_initialized = true;
} else {
std::cerr << "Warning: Failed to initialize video writer" << std::endl;
}
}

// Write to video
if (save_video && video_writer_initialized && video_writer.isOpened()) {
video_writer.write(stacked_view);
}
// drawPolyFitLanesInPlace(
// view_final,
// result.lanes
// );
// drawBEVVis(
// view_bev,
// result.frame,
// result.metrics.bev_visuals
// );
//
// // Draw Metric Debug (projected back to pixels) - only if path is valid
// if (result.path_output.fused_valid) {
// std::vector<double> left_coeffs(result.path_output.left_coeff.begin(), result.path_output.left_coeff.end());
// std::vector<double> right_coeffs(result.path_output.right_coeff.begin(), result.path_output.right_coeff.end());
// autoware_pov::vision::egolanes::drawMetricVerification(
// view_bev,
// left_coeffs,
// right_coeffs
// );
// }
//
// // 3. View layout handling
// // Layout:
// // | [Debug] | [ BEV (640x640) ]
// // | [Final] | [ Black Space ]
//
// // Left col: debug (top) + final (bottom)
// cv::Mat left_col;
// cv::vconcat(
// view_debug,
// view_final,
// left_col
// );
//
// float left_aspect = static_cast<float>(left_col.cols) / left_col.rows;
// int target_left_w = static_cast<int>(window_height * left_aspect);
// cv::resize(
// left_col,
// left_col,
// cv::Size(target_left_w, window_height)
// );
//
// // Right col: BEV (stretched to match height)
// // Black canvas matching left col height
// cv::Mat right_col = cv::Mat::zeros(
// window_height,
// 640,
// view_bev.type()
// );
// // Prep BEV
// cv::Rect top_roi(
// 0, 0,
// view_bev.cols,
// view_bev.rows
// );
// view_bev.copyTo(right_col(top_roi));
//
// // Final stacked view
// cv::Mat stacked_view;
// cv::hconcat(
// left_col,
// right_col,
// stacked_view
// );
//
// // Initialize video writer on first frame
// if (save_video && !video_writer_initialized) {
// // Use H.264 for better performance and smaller file size
// // XVID is slower and creates larger files
// int fourcc = cv::VideoWriter::fourcc('a', 'v', 'c', '1'); // H.264
// video_writer.open(
// output_video_path,
// fourcc,
// 10.0,
// stacked_view.size(),
// true
// );
//
// if (video_writer.isOpened()) {
// std::cout << "Video writer initialized (H.264): " << stacked_view.cols
// << "x" << stacked_view.rows << " @ 10 fps" << std::endl;
// video_writer_initialized = true;
// } else {
// std::cerr << "Warning: Failed to initialize video writer" << std::endl;
// }
// }
//
// // Write to video
// if (save_video && video_writer_initialized && video_writer.isOpened()) {
// video_writer.write(stacked_view);
// }

// Display
cv::imshow("EgoLanes Inference", stacked_view);
cv::imshow("EgoLanes Inference", view_debug);

if (cv::waitKey(1) == 'q') {
running.store(false);
Expand Down
Loading
Loading