Enabling the use of BGRA in addition to BGR

opengl_ros/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(catkin REQUIRED COMPONENTS
   image_transport
   cv_bridge
   opengl_ros_lib
+  dynamic_reconfigure
 )
 
 ## System dependencies are found with CMake's conventions
@@ -95,7 +96,9 @@ find_package(OpenCV REQUIRED)
 #   cfg/DynReconf1.cfg
 #   cfg/DynReconf2.cfg
 # )
-
+generate_dynamic_reconfigure_options(
+  cfg/opengl_ros.cfg
+)
 ###################################
 ## catkin specific configuration ##
 ###################################

opengl_ros/cfg/opengl_ros.cfg

@@ -0,0 +1,12 @@
+#!/usr/bin/env python
+PACKAGE = "opengl_ros"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("threshold_l",      double_t,   0,  "", 0,    0,   1)
+gen.add("svm_coef_a",       double_t,   0,  "", 0,    0,   1)
+gen.add("svm_coef_b",       double_t,   0,  "", 0,    0,   1)
+gen.add("svm_intercept",    double_t,   0,  "", 0, -100, 100)    
+exit(gen.generate(PACKAGE, "opengl_ros", "opengl_ros"))

opengl_ros/launch/color_extraction_gpu.launch

@@ -17,27 +17,26 @@
     <remap from="/usb_cam/image_raw" to="/image_in"/>
   </node>
 
+  <node name="dynamic_reconfigure_load" pkg="dynamic_reconfigure" type="dynparam" args="load color_extraction_params $(find opengl_ros)/params/color_extraction.yaml" />
   <node pkg="opengl_ros" type="simple_renderer"
     name="simple_renderer" output="screen">
     <param name="width"           value="$(arg width)"/>
     <param name="height"          value="$(arg height)"/>
     <param name="vertex_shader"   value="$(find opengl_ros_lib)/shader/vs_passthrough.glsl"/>
     <param name="fragment_shader" value="$(find opengl_ros)/shader/fs_color_extraction.glsl"/>
-
-    <param name="threshold_l"   value="0"/>
-    <param name="svm_coef_a"    value="0.266602955271"/>
-    <param name="svm_coef_b"    value="0.0444656815907"/>
-    <param name="svm_intercept" value="-44.3271851592"/>
-
-    <remap from="image_in"  to="/image_in"/>
-    <remap from="image_out" to="/image_out"/>
+    <param name="image_in"       value="/image_in"/>
+    <param name="image_out"      value="/image_out"/>
   </node>
 
+  <node pkg="rqt_reconfigure" type="rqt_reconfigure" name="reconfigure_gui"/> 
+
   <node pkg="image_view" type="image_view" name="viewer_in">
     <remap from="image" to="/image_in"/>
+    <param name="window_name" value="Input Image"/>
   </node>
 
   <node pkg="image_view" type="image_view" name="viewer_out">
     <remap from="image" to="/image_out"/>
+    <param name="window_name" value="Output Image"/>
   </node>
 </launch>

opengl_ros/params/color_extraction.yaml

@@ -0,0 +1,7 @@
+!!python/object/new:dynamic_reconfigure.encoding.Config
+dictitems:
+  threshold_l: 0
+  svm_coef_a: 0.266602955271
+  svm_coef_b: 0.0444656815907
+  svm_intercept: -44.3271851592
+state: []

opengl_ros/src/simple_renderer_nodecore.cpp

@@ -6,9 +6,25 @@
 
 using namespace opengl_ros;
 
+void SimpleRendererNode::reconfigure_callback(opengl_ros::opengl_rosConfig &config, uint32_t level) {
+    threshold_l_   = config.threshold_l; 
+    svm_coef_a_    = config.svm_coef_a;  
+    svm_coef_b_    = config.svm_coef_b;  
+    svm_intercept_ = config.svm_intercept;
+
+    renderer_->uniform("threshold_l"  , threshold_l_);
+    renderer_->uniform("svm_coef_a"   , svm_coef_a_);
+    renderer_->uniform("svm_coef_b"   , svm_coef_b_);
+    renderer_->uniform("svm_intercept", svm_intercept_);
+}
+
 SimpleRendererNode::SimpleRendererNode(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private)
     : nh_(nh_private), it_(nh)
 {
+    std::string image_in, image_out;
+    nh_.param<std::string>("image_in"  , image_in  , "");
+    nh_.param<std::string>("image_out" , image_out  , "");
+
     imagePublisher_  = it_.advertise("image_out", 1);
     imageSubscriber_ = it_.subscribe("image_in" , 1, &SimpleRendererNode::imageCallback, this);
 
@@ -25,17 +41,6 @@ SimpleRendererNode::SimpleRendererNode(const ros::NodeHandle& nh, const ros::Nod
         vertexShader, fragmentShader
     );
 
-    float threshold_l, svm_coef_a, svm_coef_b, svm_intercept;
-    nh_.param<float>("threshold_l"  , threshold_l  , 0);
-    nh_.param<float>("svm_coef_a"   , svm_coef_a   , 0);
-    nh_.param<float>("svm_coef_b"   , svm_coef_b   , 0);
-    nh_.param<float>("svm_intercept", svm_intercept, 0);
-
-    renderer_->uniform("threshold_l"  , threshold_l);
-    renderer_->uniform("svm_coef_a"   , svm_coef_a);
-    renderer_->uniform("svm_coef_b"   , svm_coef_b);
-    renderer_->uniform("svm_intercept", svm_intercept);
-
     output_.create(height, width, CV_8UC3);
 }
 
@@ -71,5 +76,10 @@ void SimpleRendererNode::imageCallback(const sensor_msgs::Image::ConstPtr& msg)
 
 void SimpleRendererNode::run()
 {
+    dynamic_reconfigure::Server<opengl_ros::opengl_rosConfig> server(ros::NodeHandle("color_extraction_params"));
+    dynamic_reconfigure::Server<opengl_ros::opengl_rosConfig>::CallbackType f;
+    f = boost::bind(boost::mem_fn(&SimpleRendererNode::reconfigure_callback), this, _1, _2);
+    server.setCallback(f);
+
     ros::spin();
 }

opengl_ros/src/simple_renderer_nodecore.h

@@ -6,9 +6,12 @@
 #include <ros/ros.h>
 #include <sensor_msgs/Image.h>
 #include <image_transport/image_transport.h>
+#include <dynamic_reconfigure/server.h>
+#include <opengl_ros/opengl_rosConfig.h>
 
 #include "simple_renderer.h"
 
+
 namespace opengl_ros {
 
 class SimpleRendererNode
@@ -26,6 +29,9 @@ class SimpleRendererNode
     cv::Mat output_;
 
     void imageCallback(const sensor_msgs::Image::ConstPtr& msg);
+    void reconfigure_callback(opengl_ros::opengl_rosConfig &config, uint32_t level);
+    float threshold_l_, svm_coef_a_, svm_coef_b_, svm_intercept_;
+
 
 public:
     SimpleRendererNode(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private);

opengl_ros_lib/include/simple_renderer.h

@@ -30,6 +30,7 @@ class SimpleRenderer final
     void uniform(const std::string& name, unsigned int v1, unsigned int v2, unsigned int v3, unsigned int v4);
 
     void render(cv::Mat& dest, const cv::Mat& src);
+    void render(cv::Mat& dest, const cv::Mat& src, const cv::Mat& secondSrc);
 
     SimpleRenderer(const SimpleRenderer&) = delete;
     SimpleRenderer& operator=(const SimpleRenderer&) = delete;
@@ -43,4 +44,4 @@ class SimpleRenderer final
 
 } //cgs
 
-#endif
+#endif

opengl_ros_lib/src/simple_renderer.cpp

@@ -37,21 +37,26 @@ struct SimpleRenderer::Impl
 
     Egl egl_;
     const int width_, height_;
+    int channels_ = 3;
+    unsigned int glFormat_ = GL_BGR;
+    unsigned int cvMatType_  = CV_8UC3;
+    bool formatSet_;
 
     std::array<cgs::gl::Shader, 2> shaders_;
     cgs::gl::Program program_;
     cgs::gl::VertexBuffer<Vertex> vbo_;
     cgs::gl::ElementBuffer<uint> ebo_;
     cgs::gl::VertexArray vao_;
-    cgs::gl::Texture2D textureIn_, textureOut_;
+    cgs::gl::Texture2D textureIn_, textureOut_, secondTexture_;
     cgs::gl::Sampler sampler_;
     cgs::gl::FrameBuffer fbo_;
 
     Impl(int width, int height, 
         const std::string& vertexShader, 
         const std::string& fragmentShader);
-
+    void checkFormat(const cv::Mat& src);
     void render(cv::Mat& dest, const cv::Mat& src);
+    void render(cv::Mat& dest, const cv::Mat& src, const cv::Mat& secondSrc);
 };
 
 constexpr std::array<Vertex, 4> SimpleRenderer::Impl::VERTICIES;
@@ -113,7 +118,7 @@ SimpleRenderer::Impl::Egl::Egl() :
 SimpleRenderer::Impl::Impl(
     int width, int height, 
     const std::string& vertexShader, 
-    const std::string& fragmentShader) : 
+    const std::string& fragmentShader) :
     width_(width), height_(height), 
     shaders_({
         cgs::gl::Shader(GL_VERTEX_SHADER,   vertexShader),
@@ -122,47 +127,50 @@ SimpleRenderer::Impl::Impl(
     program_(shaders_), 
     vbo_(VERTICIES, GL_STATIC_DRAW), 
     ebo_(INDICIES, GL_STATIC_DRAW), 
-    textureIn_(GL_SRGB8, width_, height_),  //
-    textureOut_(GL_SRGB8, width_, height_), //Assuming sRGB input and output
+    textureIn_(GL_SRGB8_ALPHA8, width_, height_),  //
+    textureOut_(GL_SRGB8_ALPHA8, width_, height_), //Assuming sRGB input and output
+    secondTexture_(GL_SRGB8_ALPHA8, width_, height_),
     sampler_(GL_LINEAR, GL_LINEAR, GL_CLAMP_TO_EDGE, GL_CLAMP_TO_EDGE),
     fbo_(textureOut_)
 {
     //Shaders setup
     program_.use();
     glUniform2f(glGetUniformLocation(program_.get(), "resolution"), width_, height_);
     glUniform1i(glGetUniformLocation(program_.get(), "texture"), 0);
+    glUniform1i(glGetUniformLocation(program_.get(), "secondTexture"), 1);
 
     //Verticies setup
     vao_.mapVariable(vbo_, glGetAttribLocation(program_.get(), "position"), 3, GL_FLOAT, 0);
     vao_.mapVariable(ebo_);
 }
-
-void SimpleRenderer::Impl::render(cv::Mat& dest, const cv::Mat& src)
+void SimpleRenderer::Impl::checkFormat(const cv::Mat& src)
 {
-    if (width_ != dest.cols || height_ != dest.rows || CV_8UC3 != dest.type())
-    {
-        ROS_ERROR_STREAM(
-            "Destination image resolution does not match." << 
-            "width:     texture=" << width_  << ", input=" << dest.cols << 
-            "height:    texture=" << height_ << ", input=" << dest.rows << 
-            "channel:   texture=" << 3       << ", input=" << dest.channels() << 
-            "elemSize1: texture=" << 1       << ", input=" << dest.elemSize1());
-        return;
+    if (!formatSet_) {
+        if (src.channels() == 4)
+        {
+            channels_ = 4;
+            glFormat_ = GL_BGRA;
+            cvMatType_  = CV_8UC4;
+        }
+        formatSet_ = true;
     }
-
-    if (width_ != src.cols || height_ != src.rows || CV_8UC3 != dest.type())
+    if (width_ != src.cols || height_ != src.rows || cvMatType_ != src.type())
     {
         ROS_ERROR_STREAM(
-            "Source image resolution does not match." << 
-            "width:     texture=" << width_  << ", input=" << src.cols << 
-            "height:    texture=" << height_ << ", input=" << src.rows << 
-            "channel:   texture=" << 3       << ", input=" << src.channels() << 
-            "elemSize1: texture=" << 1       << ", input=" << src.elemSize1());
+                "Image resolution does not match." <<
+                      "width:     texture=" << width_  << ", input=" << src.cols <<
+                      "height:    texture=" << height_ << ", input=" << src.rows <<
+                      "channel:   texture=" << channels_<< ", input=" << src.channels() <<
+                      "elemSize1: texture=" << 1       << ", input=" << src.elemSize1());
         return;
     }
-
+}
+void SimpleRenderer::Impl::render(cv::Mat& dest, const cv::Mat& src)
+{
+    checkFormat(src);
+    checkFormat(dest);
     //Perform rendering
-    textureIn_.write(GL_BGR, GL_UNSIGNED_BYTE, src.data);
+    textureIn_.write(glFormat_, GL_UNSIGNED_BYTE, src.data);
     textureIn_.bindToUnit(0);
     sampler_.bindToUnit(0);
 
@@ -175,7 +183,16 @@ void SimpleRenderer::Impl::render(cv::Mat& dest, const cv::Mat& src)
     glFinish();
 
     //Read result
-    textureOut_.read(GL_BGR, GL_UNSIGNED_BYTE, dest.data, dest.rows * dest.cols * dest.channels());
+    textureOut_.read(glFormat_, GL_UNSIGNED_BYTE, dest.data, dest.rows * dest.cols * dest.channels());
+}
+
+void SimpleRenderer::Impl::render(cv::Mat& dest, const cv::Mat& src, const cv::Mat& secondSrc)
+{
+    checkFormat(secondSrc);
+    secondTexture_.write(glFormat_, GL_UNSIGNED_BYTE, secondSrc.data);
+    secondTexture_.bindToUnit(1);
+    sampler_.bindToUnit(1);
+    this->render(dest, src);
 }
 
 SimpleRenderer::SimpleRenderer(
@@ -198,7 +215,6 @@ catch (cgs::gl::Exception& e)
 }
 
 SimpleRenderer::~SimpleRenderer() = default;
-
 void SimpleRenderer::uniform(const std::string& name, float v1) { 
     glUniform1f(glGetUniformLocation(impl_->program_.get(), name.c_str()), v1);
 }
@@ -236,7 +252,12 @@ void SimpleRenderer::uniform(const std::string& name, unsigned int v1, unsigned
     glUniform4ui(glGetUniformLocation(impl_->program_.get(), name.c_str()), v1, v2, v3, v4);
 }
 
+void SimpleRenderer::render(cv::Mat& dest, const cv::Mat& src, const cv::Mat& secondSrc)
+{
+    impl_->render(dest, src, secondSrc);
+}
+
 void SimpleRenderer::render(cv::Mat& dest, const cv::Mat& src)
 {
     impl_->render(dest, src);
-}
+}