changed to output correct occupancy grid

Author: mohammedari

opengl_ros/launch/depth_image_projection.launch

@@ -5,8 +5,8 @@
   <arg name="depth_height"                 default="360"/>
   <arg name="depth_min"                    default="0.105"/>
   <arg name="depth_max"                    default="10"/>
-  <arg name="depth_hit_threshold"          default="0.9"/>
-  <arg name="depth_unknown_color"          default="1.0"/> <!-- set 0~1.0 for 0~255 -->
+  <arg name="depth_hit_threshold"          default="0.95"/>
+  <arg name="depth_unknown_color"          default="0"/> <!-- 0 to 255 -->
   <arg name="grid_map_width"               default="200"/>
   <arg name="grid_map_height"              default="200"/>
   <arg name="grid_map_resolution"          default="0.025"/>
@@ -74,7 +74,7 @@
     <param name="geometry_shader"           value="$(find opengl_ros_lib)/shader/gs_depth_image_projection.glsl"/>
     <param name="fragment_shader"           value="$(find opengl_ros_lib)/shader/fs_depth_image_projection.glsl"/>
     <param name="vertex_shader_scaling"     value="$(find opengl_ros_lib)/shader/vs_passthrough.glsl"/>
-    <param name="fragment_shader_scaling"   value="$(find opengl_ros_lib)/shader/fs_passthrough.glsl"/>
+    <param name="fragment_shader_scaling"   value="$(find opengl_ros_lib)/shader/fs_rgb_to_occupancy_grid.glsl"/>
 
     <remap from="~depth_to_color" to="/d435/extrinsics/depth_to_color"/>
     <remap from="color_in"        to="/d435/color/image_raw"/>

opengl_ros/src/depth_image_projector_nodecore.cpp

@@ -33,11 +33,12 @@ DepthImageProjectorNode::DepthImageProjectorNode(const ros::NodeHandle& nh, cons
     nh_.param<double>("gridMapLayerHeight", gridMapLayerHeight, 1);
     nh_.param<double>("gridMapAccumulationWeight", gridMapAccumulationWeight, 1);
 
-    double minDepth, maxDepth, depthHitThreshold, unknownDepthColor;
+    double minDepth, maxDepth, depthHitThreshold;
+    int unknownDepthColor;
     nh_.param<double>("minDepth", minDepth, 0.105);
     nh_.param<double>("maxDepth", maxDepth, 10);
     nh_.param<double>("depthHitThreshold", depthHitThreshold, 0.95);
-    nh_.param<double>("unknownDepthColor", unknownDepthColor, 1.0); //255
+    nh_.param<int>("unknownDepthColor", unknownDepthColor, 255);
 
     std::string vertexShader, geometryShader, fragmentShader, vertexShaderScaling, fragmentShaderScaling;
     nh_.param<std::string>("vertex_shader"   , vertexShader  , "");
@@ -68,7 +69,7 @@ DepthImageProjectorNode::DepthImageProjectorNode(const ros::NodeHandle& nh, cons
     //projector_->uniform("svm_coef_b"   , svm_coef_b);
     //projector_->uniform("svm_intercept", svm_intercept);
 
-    output_.create(gridMapHeight, gridMapWidth, CV_8UC3);
+    output_.create(gridMapHeight, gridMapWidth, CV_8UC1);
 }
 
 void DepthImageProjectorNode::colorCallback(const sensor_msgs::Image::ConstPtr& imageMsg, const sensor_msgs::CameraInfoConstPtr & cameraInfoMsg)
@@ -134,7 +135,7 @@ void DepthImageProjectorNode::depthCallback(const sensor_msgs::Image::ConstPtr&
     outImage.header.seq      = cv_ptr->header.seq;
     outImage.header.stamp    = cv_ptr->header.stamp;
     outImage.header.frame_id = "occupancy_grid"; //TODO set from parameter
-    outImage.encoding = sensor_msgs::image_encodings::RGB8;
+    outImage.encoding = sensor_msgs::image_encodings::MONO8;
     outImage.image = output_;
     imagePublisher_.publish(outImage.toImageMsg());
 }

opengl_ros_lib/include/depth_image_projector.h

@@ -21,7 +21,7 @@ class DepthImageProjector final
         int depthWidth, int depthHeight, 
         int gridMapWidth, int gridMapHeight, float gridMapResolution, 
         float gridMapLayerHeight, float gridMapAccumulationWeight,
-        float minDepth, float maxDepth, float depthHitThreshold, float unknownDepthColor, 
+        float minDepth, float maxDepth, float depthHitThreshold, int unknownDepthColor, 
         const std::string& vertexShader, const std::string& geometryShader, const std::string& fragmentShader,
         const std::string& vertexScalingShader, const std::string& fragmentScalingShader); 
     ~DepthImageProjector();

opengl_ros_lib/shader/fs_depth_image_projection.glsl

@@ -39,5 +39,5 @@ void main(void)
     //store accumulated value in R channel
     //store 1.0 in G channel if the pixel is on the line
     //always store 1.0 in B channel 
-    fragmentColor = vec3(accumulatedProbability, validDepthmeasured, 1.0); 
+    fragmentColor = vec3(accumulatedProbability, validDepthMeasured, 1.0); 
 }

opengl_ros_lib/shader/fs_rgb_to_occupancy_grid.glsl

@@ -1,44 +1,34 @@
 #version 450 core
-uniform float gridMapLayerHeight;
-uniform float gridMapAccumulationWeight;
-uniform float depthHitThreshold;
-uniform float unknownDepthColor;
-uniform vec2 validDepthInMeter;
+uniform vec2 resolution;
+uniform sampler2D texture;
+uniform int unknownDepthColor;
 
-out vec3 fragmentColor;
-//x ... accumulated probability of object existance
-//y ... 1.0 if valid depth is measured in any ray
-//z ... 1.0 if any ray achieved at the pixel 
-
-//TODO Remove entire color image process as color image is not used anymore
-uniform sampler2D colorTexture; 
-uniform float threshold_l;
-uniform float svm_coef_a;
-uniform float svm_coef_b;
-uniform float svm_intercept;
-//TODO Remove entire color image process as color image is not used anymore
-
-in float depth;
-in float hitDepth;
+in vec4 gl_FragCoord;
+out float fragmentColor;
+//output 0 - 100 if valid depth is measured
+//output `unknownDepthColor` if valid depth is not measured
+//output 255 (unknown) if ray does not reach to the pixel
 
 void main(void)
 {
-    //TODO multilayer output based on depth which represents height
-
-    //If the height is out of the range, discard the fragment.
-    if (gl_FragCoord.z < -gridMapLayerHeight / 2 || gridMapLayerHeight / 2 < gl_FragCoord.z)
-        discard;
-
-    float validDepthMeasured = 1.0;
-    if (validDepthInMeter.y <= hitDepth)
-        validDepthMeasured = 0.0;
-
-    float accumulatedProbability = 0.0;
-    if (0 < validDepthMeasured && hitDepth * depthHitThreshold < depth)
-        accumulatedProbability = gridMapAccumulationWeight;
-
-    //store accumulated value in R channel
-    //store 1.0 in G channel if the pixel is on the line
-    //always store 1.0 in B channel 
-    fragmentColor = vec3(accumulatedProbability, validDepthmeasured, 1.0); 
-}
+    float u = gl_FragCoord.x / resolution.x;
+    float v = gl_FragCoord.y / resolution.y;
+
+    vec3 color = texture2D(texture, vec2(u, v)).xyz;
+    //x ... accumulated probability of object existance
+    //y ... 1.0 if valid depth is measured in any ray
+    //z ... 1.0 if any ray achieved at the pixel 
+
+    if (color.z < 0.5)
+    {
+        fragmentColor = 1.0;
+        return;
+    }
+    
+    if (color.y < 0.5) {
+        fragmentColor = unknownDepthColor / 255.0;
+        return;
+    }
+
+    fragmentColor = color.x * 100.0 / 255.0; //rescale to 0~100
+}

opengl_ros_lib/src/depth_image_projector.cpp

@@ -43,7 +43,7 @@ struct DepthImageProjector::Impl
     const float minDepth_;
     const float maxDepth_;
     const float depthHitThreshold_;
-    const float unknownDepthColor_; 
+    const int unknownDepthColor_; 
 
     //first path
     std::array<cgs::gl::Shader, 3> shaders_;
@@ -73,7 +73,7 @@ struct DepthImageProjector::Impl
         int depthWidth, int depthHeight,
         int gridMapWidth, int gridMapHeight, float gridMapResolution, 
         float gridMapLayerHeight, float gridMapAccumulationWeight,
-        float minDepth, float maxDepth, float depthHitThreshold, float unknownDepthColor, 
+        float minDepth, float maxDepth, float depthHitThreshold, int unknownDepthColor, 
         const std::string& vertexShader, const std::string& geometryShader, const std::string& fragmentShader, 
         const std::string& vertexScalingShader, const std::string& fragmentScalingShader);
 
@@ -92,7 +92,7 @@ DepthImageProjector::Impl::Impl(
     int depthWidth, int depthHeight, 
     int gridMapWidth, int gridMapHeight, float gridMapResolution, 
     float gridMapLayerHeight, float gridMapAccumulationWeight,
-    float minDepth, float maxDepth, float depthHitThreshold, float unknownDepthColor, 
+    float minDepth, float maxDepth, float depthHitThreshold, int unknownDepthColor, 
     const std::string& vertexShader, const std::string& geometryShader, const std::string& fragmentShader, 
     const std::string& vertexScalingShader, const std::string& fragmentScalingShader) :
     context_(true),
@@ -130,7 +130,7 @@ DepthImageProjector::Impl::Impl(
     programScaling_(shaderScaling_),
     vboScaling_(SQUARE_VERTICIES, GL_STATIC_DRAW), 
     eboScaling_(SQUARE_INDICIES, GL_STATIC_DRAW), 
-    textureOut_(GL_R8UI, gridMapWidth, gridMapHeight),
+    textureOut_(GL_R8, gridMapWidth, gridMapHeight),
     intermediateSampler_(GL_NEAREST, GL_NEAREST, GL_CLAMP_TO_EDGE, GL_CLAMP_TO_EDGE),
     fboScaling_(textureOut_)
 {
@@ -156,15 +156,15 @@ void DepthImageProjector::Impl::updateProjectionMatrix(
 
 void DepthImageProjector::Impl::project(cv::Mat& dest, const cv::Mat& color, const cv::Mat& depth)
 {
-    if (gridMapWidth_ != dest.cols || gridMapHeight_ != dest.rows || CV_8UC3 != dest.type())
+    if (gridMapWidth_ != dest.cols || gridMapHeight_ != dest.rows || CV_8UC1 != dest.type())
     {
         ROS_ERROR_STREAM(
             "Destination image resolution does not match."                            << std::endl <<
             "width:     texture=" << gridMapWidth_  << ", input=" << dest.cols        << std::endl <<
             "height:    texture=" << gridMapHeight_ << ", input=" << dest.rows        << std::endl <<
-            "channel:   texture=" << 3              << ", input=" << dest.channels()  << std::endl <<
+            "channel:   texture=" << 1              << ", input=" << dest.channels()  << std::endl <<
             "elemSize1: texture=" << 1              << ", input=" << dest.elemSize1() << std::endl <<
-            "type:      texture=" << CV_8UC3        << ", input=" << dest.type());
+            "type:      texture=" << CV_8UC1        << ", input=" << dest.type());
         return;
     }
 
@@ -239,45 +239,44 @@ void DepthImageProjector::Impl::project(cv::Mat& dest, const cv::Mat& color, con
         glDrawArrays(GL_POINTS, 0, verticies_.size());
     }
 
-    ////second path
-    //{
-    //    //Shaders setup
-    //    program_.use();
-    //    glUniform2f(glGetUniformLocation(program_.get(), "resolution"), width_, height_);
-    //    glUniform1i(glGetUniformLocation(program_.get(), "texture"), 0);
-    //    glUniform1f(glGetUniformLocation(program_.get() , "unknownDepthColor"), unknownDepthColor_);
+    //second path
+    {
+        //Shaders setup
+        programScaling_.use();
+        glUniform2f(glGetUniformLocation(programScaling_.get(), "resolution"), gridMapWidth_, gridMapHeight_);
+        glUniform1i(glGetUniformLocation(programScaling_.get(), "texture"), 0);
+        glUniform1i(glGetUniformLocation(programScaling_.get(), "unknownDepthColor"), unknownDepthColor_);
 
-    //    //Verticies setup
-    //    vaoScaling_.mapVariable(vboScaling_, glGetAttribLocation(program_.get(), "position"), 3, GL_FLOAT, 0);
-    //    vaoScaling_.mapVariable(eboScaling_);
+        //Verticies setup
+        vaoScaling_.mapVariable(vboScaling_, glGetAttribLocation(programScaling_.get(), "position"), 3, GL_FLOAT, 0);
+        vaoScaling_.mapVariable(eboScaling_);
 
-    //    //Disable flags
-    //    glDisable(GL_DEPTH_CLAMP); 
-    //    glDisable(GL_BLEND);
+        //Disable flags
+        glDisable(GL_DEPTH_CLAMP); 
+        glDisable(GL_BLEND);
 
-    //    textureIntermediate_.bindToUnit(0);
-    //    samplerIntermediate_.bindToUnit(0);
+        textureIntermediate_.bindToUnit(0);
+        intermediateSampler_.bindToUnit(0);
 
-    //    fboScaling_.bind();
-    //    glViewport(0, 0, width_, height_);
+        fboScaling_.bind();
+        glViewport(0, 0, gridMapWidth_, gridMapHeight_);
 
-    //    vaoScaling_.bind();
-    //    glDrawElements(GL_TRIANGLES, INDICIES.size(), GL_UNSIGNED_INT, nullptr);
-    //}
+        vaoScaling_.bind();
+        glDrawElements(GL_TRIANGLES, SQUARE_INDICIES.size(), GL_UNSIGNED_INT, nullptr);
+    }
 
     glFinish();
 
     //Read result
-    //textureOut_.read(GL_RED, GL_UNSIGNED_BYTE, dest.data, dest.rows * dest.cols * dest.channels());
-    textureIntermediate_.read(GL_RGB, GL_UNSIGNED_BYTE, dest.data, dest.rows * dest.cols * dest.channels());
+    textureOut_.read(GL_RED, GL_UNSIGNED_BYTE, dest.data, dest.rows * dest.cols * dest.channels());
 }
 
 DepthImageProjector::DepthImageProjector(
     int colorWidth, int colorHeight, 
     int depthWidth, int depthHeight, 
     int gridMapWidth, int gridMapHeight, float gridMapResolution, 
     float gridMapLayerHeight, float gridMapAccumulationWeight, 
-    float minDepth, float maxDepth, float depthHitThreshold, float unknownDepthColor, 
+    float minDepth, float maxDepth, float depthHitThreshold, int unknownDepthColor, 
     const std::string& vertexShader, const std::string& geometryShader, const std::string& fragmentShader,
     const std::string& vertexScalingShader, const std::string& fragmentScalingShader)
 try