changed depth hit threshold parameter implementation, tweeked parameters

Author: mohammedari

opengl_ros/launch/depth_image_projection.launch

@@ -3,16 +3,16 @@
   <arg name="bag_prefix"                   default="/mnt/log/d435"/>
   <arg name="depth_width"                  default="640"/>
   <arg name="depth_height"                 default="360"/>
-  <arg name="depth_min"                    default="0.105"/>
+  <arg name="depth_min"                    default="0.3"/>
   <arg name="depth_max"                    default="10"/>
-  <arg name="depth_hit_threshold"          default="0.95"/>
+  <arg name="depth_hit_threshold"          default="0.1"/>
   <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.1"/>
-  <arg name="grid_map_layer_height"        default="3.0"/>
+  <arg name="grid_map_layer_height"        default="2.0"/>
   <arg name="grid_map_accumulation_weight" default="0.001"/> 
-  <arg name="grid_map_decay"               default="0.5"/> 
+  <arg name="grid_map_decay"               default="0.95"/> 
   <arg name="frame_rate"                   default="30"/>
 
   <node pkg="opengl_ros" type="depth_image_projector"

opengl_ros/src/depth_image_projector_nodecore.cpp

@@ -32,7 +32,7 @@ DepthImageProjectorNode::DepthImageProjectorNode(const ros::NodeHandle& nh, cons
     int unknownDepthColor;
     nh_.param<double>("min_depth", minDepth, 0.105);
     nh_.param<double>("max_depth", maxDepth, 10);
-    nh_.param<double>("depth_hit_threshold", depthHitThreshold, 0.95);
+    nh_.param<double>("depth_hit_threshold", depthHitThreshold, 0.01);
     nh_.param<int>("unknown_depth_color", unknownDepthColor, 255);
 
     std::string vertexShader, geometryShader, fragmentShader, vertexShaderScaling, fragmentShaderScaling;

opengl_ros_lib/shader/fs_depth_image_projection.glsl

@@ -17,15 +17,16 @@ 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)
+    float height = gl_FragCoord.z / gl_FragCoord.w;
+    if (height < -gridMapLayerHeight / 2 || gridMapLayerHeight / 2 < height)
         discard;
 
     float validDepthMeasured = 1.0;
     if (validDepthInMeter.y <= hitDepth)
         validDepthMeasured = 0.0;
 
     float accumulatedProbability = 0.0;
-    if (0 < validDepthMeasured && hitDepth * depthHitThreshold < depth)
+    if (0 < validDepthMeasured && hitDepth - depthHitThreshold < depth)
         accumulatedProbability = gridMapAccumulationWeight;
 
     //store accumulated value in R channel

opengl_ros_lib/shader/fs_rgb_to_occupancy_grid.glsl

@@ -7,8 +7,6 @@ uniform float gridMapDecay;
 uniform float gridMapResolution;
 uniform mat4 mapToPreviousMap;
 
-in vec4 gl_FragCoord;
-
 layout(location = 0) out float fragmentColor;
 //output 0 - 100 if valid depth is measured
 //output `unknownDepthColor` if valid depth is not measured

opengl_ros_lib/shader/vs_depth_image_projection.glsl

@@ -19,13 +19,13 @@ in vec3 input_pixel; //This input position is integer pixel coordinate in the op
 
 out vertex
 {
-    vec4 position;   //The vertex coordinate of the point in grid map space.
+    vec4 position;   //The vertex coordinate of the point in grid map space
                      //x = -1 to 1; which represents left to right
                      //y = -1 to 1; which represents far to near, note this is flipped upside down
                      //z = -1 to 1; which represents low to high, 
                      //w = 1
 
-    float depth;     //The depth value in meter of the point.
+    float depth;     //The depth value in meter of the point
 } output_vertex;
 
 void main(void)
@@ -57,10 +57,10 @@ void main(void)
     //Ouptut vertex coordinate
     //Projecting the point to the occupancy grid plane, in top down orthognal projection
     output_vertex.position = vec4(
-        p.x / gridMapResolution / gridMapSize.x * 2,     //convert to -1 to 1
-        p.y / gridMapResolution / gridMapSize.y * 2,     //set the origin at the center and flip upside down
-        p.z / (gridMapLayerHeight / 2),                  //mapping -height/2 to height/2, to -1 to 1
+        p.x / gridMapResolution / gridMapSize.x * 2, //convert to -1 to 1
+        p.y / gridMapResolution / gridMapSize.y * 2, //set the origin at the center and flip upside down
+        p.z / gridMapLayerHeight * 2,                //mapping -height/2 to height/2, to -1 to 1
         1.0
     );
-    output_vertex.depth   = depth;
+    output_vertex.depth = depth;
 }