Make learning work

.devcontainer/Dockerfile

@@ -2,6 +2,7 @@
 FROM ros:noetic-ros-core AS base
 
 ARG TZ=Europe/Zurich
+ARG VGLUSERS_GID=1004
 ENV DEBIAN_FRONTEND=noninteractive
 ENV PATH="${PATH}:/opt/hpcx/ompi/bin"
 
@@ -15,25 +16,36 @@ ENV LANG="en_US.UTF-8" \
 
 # Add user "asl" with sudo rights
 RUN groupadd -r asl && \
-    useradd --create-home --gid asl --groups dialout,plugdev --shell /bin/bash asl && \
+    groupadd -g ${VGLUSERS_GID} vglusers && \
+    useradd --create-home --gid asl --groups dialout,plugdev,vglusers --shell /bin/bash asl && \
     mkdir -p /etc/sudoers.d && \
     echo 'asl ALL=NOPASSWD: ALL' > /etc/sudoers.d/asl
 
-# CUDA: Install
+# CUDA: Install (NOTE: libcublas 12.6 is needed for trtexec)
 RUN wget https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/cuda-keyring_1.1-1_all.deb && \
     sudo dpkg -i cuda-keyring_1.1-1_all.deb && \
     sudo apt update && \
-    sudo apt -y install cuda-toolkit-12-6 cudnn9-cuda-12
+    sudo apt -y install cuda-toolkit-11-8 cudnn9-cuda-11 libcublas-12-6
 
 # CUDA: Add PATH and LD_LIBRARY_PATH to .bash_aliases
-RUN echo 'export PATH=$PATH:/usr/local/cuda/bin' >> /home/asl/.bash_aliases && \
-    echo 'export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/cuda/lib64' >> /home/asl/.bash_aliases
+RUN echo 'export PATH=$PATH:/usr/local/cuda-11.8/bin' >> /home/asl/.bash_aliases && \
+    echo 'export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/cuda-11.8/lib64' >> /home/asl/.bash_aliases
 
-# TensorRT: Install from .deb file
-COPY resources/nv-tensorrt-repo-ubuntu2004-cuda11.6-trt8.4.3.1-ga-20220813_1-1_amd64.deb /tmp/
-RUN sudo dpkg -i /tmp/resources/nv-tensorrt-repo-ubuntu2004-cuda11.6-trt8.4.3.1-ga-20220813_1-1_amd64.deb || sudo apt-get install -f -y
-# RUN sudo cp /var/nv-tensorrt-repo-ubuntu2004-cuda11.6-trt8.4.3.1-ga-20220813/*-keyring.gpg /usr/share/keyrings/ && sudo apt update && sudo apt install -y tensorrt
-RUN sudo apt update && sudo apt install -y tensorrt
+# TensorRT: Install from .deb file: Seems we run 8.5.2.2 (which is bundled with 8.5.3)
+COPY .devcontainer/nv-tensorrt-local-repo-ubuntu2004-8.5.3-cuda-11.8_1.0-1_amd64.deb /tmp/tensorrt.deb
+RUN sudo dpkg -i /tmp/tensorrt.deb
+RUN sudo cp /var/nv-tensorrt-local-repo-ubuntu2004-8.5.3-cuda-11.8/nv-tensorrt-local-3EFA7C6A-keyring.gpg /usr/share/keyrings/
+RUN sudo apt update && sudo apt install -y tensorrt=8.5.2.2-1+cuda11.8 \
+                                           libnvinfer8=8.5.2-1+cuda11.8 \
+                                           libnvinfer-plugin8=8.5.2-1+cuda11.8 \
+                                           libnvparsers8=8.5.2-1+cuda11.8 \
+                                           libnvonnxparsers8=8.5.2-1+cuda11.8 \
+                                           libnvinfer-bin=8.5.2-1+cuda11.8 \
+                                           libnvinfer-dev=8.5.2-1+cuda11.8 \
+                                           libnvinfer-plugin-dev=8.5.2-1+cuda11.8 \
+                                           libnvparsers-dev=8.5.2-1+cuda11.8 \
+                                           libnvonnxparsers-dev=8.5.2-1+cuda11.8 \
+                                           libnvinfer-samples=8.5.2-1+cuda11.8
 
 # OpenCV Build Stage
 FROM base AS opencv_build

.devcontainer/build_opencv.sh

@@ -104,8 +104,8 @@ install_dependencies () {
 
 # Automatically detect installed CUDA version
 detect_cuda_version() {
-    if command -v /usr/local/cuda/bin/nvcc &> /dev/null; then
-        CUDA_VERSION=$(/usr/local/cuda/bin/nvcc --version | grep -oP 'release \K[0-9]+\.[0-9]+')
+    if command -v /usr/local/cuda-11.8/bin/nvcc &> /dev/null; then
+        CUDA_VERSION=$(/usr/local/cuda-11.8/bin/nvcc --version | grep -oP 'release \K[0-9]+\.[0-9]+')
     else
         echo "CUDA not found. Please install CUDA before running this script."
         exit 1
@@ -153,6 +153,7 @@ configure() {
         -D BUILD_opencv_python3=OFF
         -D CMAKE_BUILD_TYPE=RELEASE
         -D CMAKE_INSTALL_PREFIX=${PREFIX}
+        -D CUDA_TOOLKIT_ROOT_DIR=/usr/local/cuda-11.8
         -D CUDA_ARCH_BIN=${CUDA_ARCH_BIN}
         -D CUDA_FAST_MATH=ON
         -D CUDNN_VERSION='${CUDNN_VERSION}'
@@ -170,8 +171,7 @@ configure() {
         -D WITH_OPENGL=ON"
 
     if [[ "$1" != "test" ]] ; then
-        CMAKEFLAGS="
-        ${CMAKEFLAGS}
+        CMAKEFLAGS="${CMAKEFLAGS}
         -D BUILD_PERF_TESTS=OFF
         -D BUILD_TESTS=OFF"
     fi

.devcontainer/devcontainer.json

@@ -3,7 +3,7 @@
     "customizations": {
         "vscode": {
             "extensions": [
-                "ms-python.python"
+                "ms-vscode.cpptools"
             ],
             "settings": {
                 "files.hotExit": "off",

.devcontainer/devcontainer_all_packages.sh

@@ -11,6 +11,7 @@ main() {
         ccache
         cmake
         curl
+        gdb
         gfortran
         git
         gnupg
@@ -52,6 +53,7 @@ main() {
         python3-dev
         python3-matplotlib
         python3-numpy
+        python3-pip
         python3-rosdep
         qv4l2
         software-properties-common

.github/workflows/ci.yml

@@ -9,7 +9,7 @@ jobs:
 
     strategy:
       matrix:
-        ci_script: [pr_compile]
+        ci_script: [pr_compile, pr_run_tests]
 
     steps:
       - name: Checkout

.gitignore

@@ -8,5 +8,9 @@
 docs/xml
 site/
 
-resources/*
+.devcontainer/*.deb
+*.out
+test/resources/*.engine
+test/resources/*.onnx
 build/*
+test/resources/*.plan

.vscode/launch.json

@@ -0,0 +1,26 @@
+{
+    // Use IntelliSense to learn about possible attributes.
+    // Hover to view descriptions of existing attributes.
+    // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
+    "version": "0.2.0",
+    "configurations": [
+        {
+            "name": "Debug test_depth_anything_v2",
+            "type": "cppdbg",
+            "request": "launch",
+            "program": "${workspaceFolder}/build/devel/lib/usb_cam/test_depth_anything_v2",
+            "cwd": "${workspaceFolder}",
+            "environment": [],
+            "externalConsole": false,
+            "MIMode": "gdb",  // Use gdb for debugging
+            "setupCommands": [
+                {
+                    "description": "Enable pretty-printing for gdb",
+                    "text": "-enable-pretty-printing",
+                    "ignoreFailures": true
+                }
+            ],
+            "miDebuggerPath": "/usr/bin/gdb"
+        }
+    ]
+}

CMakeLists.txt

@@ -46,10 +46,15 @@ endif()
 set(TENSORRT_LIB_DIR /usr/lib/x86_64-linux-gnu CACHE PATH "Path to TensorRT libraries")
 find_library(NVINFER nvinfer PATHS ${TENSORRT_LIB_DIR})
 find_library(NVINFER_PLUGIN nvinfer_plugin PATHS ${TENSORRT_LIB_DIR})
+find_library(NVONNXPARSER nvonnxparser PATHS ${TENSORRT_LIB_DIR})
+
+message(STATUS "TensorRT NVINFER library found at: ${NVINFER}")
+message(STATUS "TensorRT NVINFER_PLUGIN library found at: ${NVINFER_PLUGIN}")
+message(STATUS "TensorRT NVONNXPARSER library found at: ${NVONNXPARSER}")
 
 # Check if TensorRT libraries are found
-if(NOT NVINFER OR NOT NVINFER_PLUGIN)
-  message(FATAL_ERROR "TensorRT libraries not found. Set TENSORRT_LIB_DIR correctly.")
+if(NOT NVINFER OR NOT NVINFER_PLUGIN OR NOT NVONNXPARSER)
+  message(FATAL_ERROR "TensorRT libraries not found. Ensure TENSORRT_LIB_DIR is set correctly.")
 endif()
 
 ## Build the USB camera library
@@ -79,6 +84,7 @@ target_link_libraries(${PROJECT_NAME}
     ${swscale_LIBRARIES}
     ${NVINFER}
     ${NVINFER_PLUGIN}
+    ${NVONNXPARSER}
 )
 
 # Define catkin package
@@ -90,35 +96,39 @@ catkin_package(
 # Build the USB camera node executable
 add_executable(${PROJECT_NAME}_node src/usb_cam_node.cpp)
 target_link_libraries(${PROJECT_NAME}_node
-  ${PROJECT_NAME}
-  ${catkin_LIBRARIES}
-  ${avcodec_LIBRARIES}
-  ${avutil_LIBRARIES}
-  ${swscale_LIBRARIES}
+    ${PROJECT_NAME}
+    ${catkin_LIBRARIES}
+    ${avcodec_LIBRARIES}
+    ${avutil_LIBRARIES}
+    ${swscale_LIBRARIES}
+    ${NVINFER}
+    ${NVINFER_PLUGIN}
+    ${NVONNXPARSER}
 )
+set_target_properties(${PROJECT_NAME}_node PROPERTIES LINK_FLAGS "-Wl,--no-as-needed")
 
 # Ensure include directories are set
 target_include_directories(${PROJECT_NAME}_node PUBLIC
   ${catkin_INCLUDE_DIRS})
 
 # Testing
 if(BUILD_TESTING)
-  if($ENV{ROS_VERSION} EQUAL 2)
-    find_package(ament_lint_auto REQUIRED)
-    ament_lint_auto_find_test_dependencies()
-    find_package(ament_cmake_gtest)
-
-    # Unit tests
-    ament_add_gtest(test_usb_cam_utils test/test_usb_cam_utils.cpp)
-    target_link_libraries(test_usb_cam_utils ${PROJECT_NAME})
-
-    ament_add_gtest(test_pixel_formats test/test_pixel_formats.cpp)
-    target_link_libraries(test_pixel_formats ${PROJECT_NAME})
-
-    # Integration tests (commented out as per TODO)
-    # ament_add_gtest(test_usb_cam_lib test/test_usb_cam_lib.cpp)
-    # target_link_libraries(test_usb_cam_lib ${PROJECT_NAME})
-  endif()
+  # Find GTest package
+  set(CMAKE_BUILD_TYPE Debug)
+  find_package(GTest REQUIRED)
+  include_directories(${GTEST_INCLUDE_DIRS})
+
+  catkin_add_gtest(test_depth_anything_v2 test/test_depth_anything_v2.cpp)
+  target_link_libraries(test_depth_anything_v2
+      ${PROJECT_NAME}
+      ${catkin_LIBRARIES}
+      GTest::gtest_main
+      ${NVINFER}
+      ${NVINFER_PLUGIN}
+      ${NVONNXPARSER}
+      ${CUDA_LIBRARIES}
+  )
+  set_target_properties(test_depth_anything_v2 PROPERTIES LINK_FLAGS "-Wl,--no-as-needed")
 endif()
 
 # Installation rules

ci/pr_run_tests.sh

@@ -0,0 +1,56 @@
+#!/bin/bash
+source /opt/ros/noetic/setup.bash
+
+# Set paths for the model and plan files
+MODEL_PATH="test/resources/depth_anything_v2_vits_16.onnx"
+
+# Check if the ONNX model file exists
+if [ ! -f "$MODEL_PATH" ]; then
+    # If the model file doesn't exist, check if the environment variable is set
+    if [ -z "$DEPTH_ANYTHING_V2_VITS_16_LINK" ]; then
+        echo "The model file does not exist, and the environment variable DEPTH_ANYTHING_V2_VITS_16_LINK is not set."
+        exit 1
+    else
+        # If the environment variable is set, download the model
+        echo "ONNX model file not found. Attempting to download..."
+
+        # Create the directory if it doesn't exist
+        mkdir -p "$(dirname "$MODEL_PATH")"
+
+        # Download the file
+        if wget -O "$MODEL_PATH" "$DEPTH_ANYTHING_V2_VITS_16_LINK"; then
+            echo "Download successful."
+        else
+            echo "Download failed."
+            exit 1
+        fi
+    fi
+else
+    echo "ONNX model file already exists."
+fi
+
+# Build the project and run tests
+rm -rf build
+mkdir -p build
+cd build
+
+if cmake .. -DBUILD_TESTING=ON; then
+    echo "CMake successful."
+    if make test_depth_anything_v2; then
+        echo "Make successful."
+    else
+        echo "Make failed."
+        exit 1
+    fi
+else
+    echo "CMake failed."
+    exit 1
+fi
+
+# Run the test executable
+if ./devel/lib/usb_cam/test_depth_anything_v2; then
+    echo "Tests successful."
+else
+    echo "Tests failed."
+    exit 1
+fi

include/usb_cam/formats/bayer.hpp

@@ -113,7 +113,7 @@ class BAYER_GRBG10 : public pixel_format_base
         }
 
         for (uint8_t i = 0; i < 3; ++i) {
-            channels[i].convertTo(channels[i], CV_8U, avg_gains[i] / 256.0);
+            channels[i].convertTo(channels[i], CV_8U, avg_gains[i] / 255.0);
         }
         ROS_DEBUG("BGR gains: %.3f %.3f %.3f", avg_gains[0], avg_gains[1], avg_gains[2]);
         cv::merge(channels, _rgb_image_out);
@@ -123,7 +123,7 @@ class BAYER_GRBG10 : public pixel_format_base
         cv::cuda::split(_gpu_rgb_image, _gpu_rgb_channels);
 
         for (int i = 0; i < 3; ++i) {
-            _gpu_rgb_channels[i].convertTo(_gpu_rgb_channels[i], CV_8U, _wb_gains[i] / 256.0);
+            _gpu_rgb_channels[i].convertTo(_gpu_rgb_channels[i], CV_8U, _wb_gains[i] / 255.0);
         }
 
         cv::cuda::merge(_gpu_rgb_channels, _gpu_rgb_image_8bit);

include/usb_cam/learning/depth_anything_v2.hpp

@@ -0,0 +1,60 @@
+#ifndef DEPTH_ANYTHING_HPP_
+#define DEPTH_ANYTHING_HPP_
+
+#include "interface.hpp"
+#include "ros/ros.h"
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/opencv.hpp>
+#include <sensor_msgs/Image.h>
+
+class DepthAnythingV2 : public LearningInterface {
+public:
+    DepthAnythingV2(ros::NodeHandle* nh, std::string model_path) {
+        _INPUT_SIZE = cv::Size(_HEIGHT, _WIDTH);
+        _model_path = model_path;
+
+        if (nh != nullptr) {
+            _depth_publication = nh->advertise<sensor_msgs::Image>("depth_anything_v2", 1);
+        }
+    }
+
+    void set_input(sensor_msgs::Image& msg) override {
+        cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8);
+        cv::Mat image = cv_ptr->image;
+
+        // Change size to 518x518 (still uint8)
+        cv::Mat resized_image;
+        cv::resize(image, resized_image, _INPUT_SIZE);
+
+        // Change to float32 between 0 and 1
+        std::vector<cv::Mat> channels;
+        cv::split(resized_image, channels);
+        for (uint8_t i = 0; i < 3; ++i) {
+            channels[i].convertTo(channels[i], CV_32F, 1.0f / 255.0f);
+        }
+        cv::Mat float_image;
+        cv::merge(channels, float_image);
+        _input_data = float_image.reshape(1, 1).ptr<float>(0);
+    }
+
+    void publish() override {
+        if (_depth_publication.getTopic() != "") {
+            cv::Mat depth_prediction = cv::Mat(_HEIGHT, _WIDTH, CV_32FC1, _output_data);
+
+            cv_bridge::CvImage depth_image;
+            depth_image.header.stamp = ros::Time::now();
+            depth_image.header.frame_id = "depth_frame";
+            depth_image.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
+            depth_image.image = depth_prediction;
+            _depth_publication.publish(depth_image.toImageMsg());
+        }
+    }
+
+private:
+    const size_t _HEIGHT = 518;
+    const size_t _WIDTH = 518;
+    cv::Size _INPUT_SIZE;
+    ros::Publisher _depth_publication;
+};
+
+#endif // DEPTH_ANYTHING_HPP_