From f50e677bc24d8e634269d8ff2328289708af8575 Mon Sep 17 00:00:00 2001
From: Simon Blaue <sblaue@gmx.de>
Date: Fri, 10 Apr 2026 09:48:26 +0000
Subject: [PATCH] cuda demosaicing if available

---
 cv_bridge/CMakeLists.txt     | 15 +++++++++++++
 cv_bridge/src/CMakeLists.txt |  9 +++++++-
 cv_bridge/src/cv_bridge.cpp  | 42 +++++++++++++++++++++++++++++++++++-
 3 files changed, 64 insertions(+), 2 deletions(-)
 mode change 100644 => 100755 cv_bridge/CMakeLists.txt
 mode change 100644 => 100755 cv_bridge/src/CMakeLists.txt
 mode change 100644 => 100755 cv_bridge/src/cv_bridge.cpp

diff --git a/cv_bridge/CMakeLists.txt b/cv_bridge/CMakeLists.txt
old mode 100644
new mode 100755
index 942cb0fac..6f8f99355
--- a/cv_bridge/CMakeLists.txt
+++ b/cv_bridge/CMakeLists.txt
@@ -48,8 +48,23 @@ find_package(OpenCV 4 QUIET
     opencv_core
     opencv_imgproc
     opencv_imgcodecs
+    opencv_cudaimgproc
   CONFIG
 )
+
+if(OpenCV_FOUND)
+add_compile_definitions("HAVE_OPENCV_CUDAIMGPROC")
+set(WITH_OPENCV_CUDAIMGPROC TRUE)
+else()
+  find_package(OpenCV 4 QUIET
+    COMPONENTS
+      opencv_core
+      opencv_imgproc
+      opencv_imgcodecs
+    CONFIG
+  )
+endif()
+
 if(NOT OpenCV_FOUND)
   find_package(OpenCV 3 REQUIRED
     COMPONENTS
diff --git a/cv_bridge/src/CMakeLists.txt b/cv_bridge/src/CMakeLists.txt
old mode 100644
new mode 100755
index 76ccfd9f9..8ce03b850
--- a/cv_bridge/src/CMakeLists.txt
+++ b/cv_bridge/src/CMakeLists.txt
@@ -14,9 +14,16 @@ target_link_libraries(${PROJECT_NAME} PUBLIC
   ${sensor_msgs_TARGETS}
   opencv_core
   opencv_imgproc
+
   opencv_imgcodecs
   rclcpp::rclcpp
-)
+  opencv_imgcodecs)
+
+if(WITH_OPENCV_CUDAIMGPROC)
+    target_link_libraries(${PROJECT_NAME} PUBLIC
+      opencv_cudaimgproc)
+endif()
+
 target_link_libraries(${PROJECT_NAME} PRIVATE
   Boost::headers
   rcpputils::rcpputils)
diff --git a/cv_bridge/src/cv_bridge.cpp b/cv_bridge/src/cv_bridge.cpp
old mode 100644
new mode 100755
index 9493d107f..e0a4dfdc2
--- a/cv_bridge/src/cv_bridge.cpp
+++ b/cv_bridge/src/cv_bridge.cpp
@@ -39,6 +39,9 @@
 #include <boost/endian/conversion.hpp>
 #include <opencv2/imgcodecs.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
+#ifdef HAVE_OPENCV_CUDAIMGPROC
+#include <opencv2/cudaimgproc.hpp>
+#endif
 #include <sensor_msgs/image_encodings.hpp>
 #include "rcpputils/endian.hpp"
 
@@ -193,6 +196,23 @@ std::map<std::pair<Encoding, Encoding>, std::vector<int>> getConversionCodes()
   res[std::make_pair(YUV422_YUY2, BGRA)].push_back(cv::COLOR_YUV2BGRA_YUY2);
 
   // Deal with Bayer
+  #ifdef HAVE_OPENCV_CUDAIMGPROC
+  res[std::make_pair(BAYER_RGGB, GRAY)].push_back(cv::cuda::COLOR_BayerBG2GRAY_MHT);
+  res[std::make_pair(BAYER_RGGB, RGB)].push_back(cv::cuda::COLOR_BayerBG2RGB_MHT);
+  res[std::make_pair(BAYER_RGGB, BGR)].push_back(cv::cuda::COLOR_BayerBG2BGR_MHT);
+
+  res[std::make_pair(BAYER_BGGR, GRAY)].push_back(cv::cuda::COLOR_BayerRG2GRAY_MHT);
+  res[std::make_pair(BAYER_BGGR, RGB)].push_back(cv::cuda::COLOR_BayerRG2RGB_MHT);
+  res[std::make_pair(BAYER_BGGR, BGR)].push_back(cv::cuda::COLOR_BayerRG2BGR_MHT);
+
+  res[std::make_pair(BAYER_GBRG, GRAY)].push_back(cv::cuda::COLOR_BayerGR2GRAY_MHT);
+  res[std::make_pair(BAYER_GBRG, RGB)].push_back(cv::cuda::COLOR_BayerGR2RGB_MHT);
+  res[std::make_pair(BAYER_GBRG, BGR)].push_back(cv::cuda::COLOR_BayerGR2BGR_MHT);
+
+  res[std::make_pair(BAYER_GRBG, GRAY)].push_back(cv::cuda::COLOR_BayerGB2GRAY_MHT);
+  res[std::make_pair(BAYER_GRBG, RGB)].push_back(cv::cuda::COLOR_BayerGB2RGB_MHT);
+  res[std::make_pair(BAYER_GRBG, BGR)].push_back(cv::cuda::COLOR_BayerGB2BGR_MHT);
+  #else
   res[std::make_pair(BAYER_RGGB, GRAY)].push_back(cv::COLOR_BayerBG2GRAY);
   res[std::make_pair(BAYER_RGGB, RGB)].push_back(cv::COLOR_BayerBG2RGB);
   res[std::make_pair(BAYER_RGGB, BGR)].push_back(cv::COLOR_BayerBG2BGR);
@@ -208,7 +228,7 @@ std::map<std::pair<Encoding, Encoding>, std::vector<int>> getConversionCodes()
   res[std::make_pair(BAYER_GRBG, GRAY)].push_back(cv::COLOR_BayerGB2GRAY);
   res[std::make_pair(BAYER_GRBG, RGB)].push_back(cv::COLOR_BayerGB2RGB);
   res[std::make_pair(BAYER_GRBG, BGR)].push_back(cv::COLOR_BayerGB2BGR);
-
+  #endif
   return res;
 }
 
@@ -366,7 +386,27 @@ CvImagePtr toCvCopyImpl(
         }
       } else {
         // Perform color conversion
+        #ifdef HAVE_OPENCV_CUDAIMGPROC
+        if (cv::cuda::getCudaEnabledDeviceCount() > 0) {          
+          if (src_encoding == enc::BAYER_RGGB8 || src_encoding == enc::BAYER_BGGR8 || src_encoding == enc::BAYER_GBRG8 ||
+            src_encoding == enc::BAYER_GRBG8)
+          {
+            cv::cuda::GpuMat gpu_image1(image1);
+            cv::cuda::GpuMat gpu_image2;
+            cv::cuda::demosaicing(gpu_image1, gpu_image2, conversion_code);
+            gpu_image2.download(image2);
+          } else {
+            cv::cuda::GpuMat gpu_image1(image1);
+            cv::cuda::GpuMat gpu_image2;
+            cv::cuda::cvtColor(gpu_image1, gpu_image2, conversion_code);
+            gpu_image2.download(image2);
+          }
+        } else {
+          cv::cvtColor(image1, image2, conversion_code);
+        }
+        #else
         cv::cvtColor(image1, image2, conversion_code);
+        #endif
       }
       image1 = image2;
     }