implemented temporal accumulation

Author: mohammedari

opengl_ros/launch/depth_image_projection.launch

@@ -11,13 +11,15 @@
   <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_accumulation_weight" default="0.004"/> <!-- 0.004 for 1/255 -->
+  <arg name="grid_map_accumulation_weight" default="0.001"/> 
+  <arg name="grid_map_decay"               default="0.5"/> 
   <arg name="frame_rate"                   default="30"/>
 
   <node pkg="opengl_ros" type="depth_image_projector"
     name="depth_image_projector" output="screen">
     <param name="depth_frame_id"               value="d435_depth_optical"/>
     <param name="map_frame_id"                 value="base_link_hor"/>
+    <param name="fixed_frame_id"               value="map"/>
     <param name="tf_wait_duration"             value="0.1"/>
 
     <param name="depth_width"                  value="$(arg depth_width)"/>
@@ -33,6 +35,7 @@
     <param name="grid_map_resolution"          value="$(arg grid_map_resolution)"/>
     <param name="grid_map_layer_height"        value="$(arg grid_map_layer_height)"/>
     <param name="grid_map_accumulation_weight" value="$(arg grid_map_accumulation_weight)"/>
+    <param name="grid_map_decay"               value="$(arg grid_map_decay)"/>
 
     <param name="vertex_shader"                value="$(find opengl_ros_lib)/shader/vs_depth_image_projection.glsl"/>
     <param name="geometry_shader"              value="$(find opengl_ros_lib)/shader/gs_depth_image_projection.glsl"/>

opengl_ros/src/depth_image_projector_nodecore.cpp

@@ -13,18 +13,20 @@ DepthImageProjectorNode::DepthImageProjectorNode(const ros::NodeHandle& nh, cons
 
     nh_.param<std::string>("depth_frame_id", depth_frame_id_, "d435_depth_optical");
     nh_.param<std::string>("map_frame_id", map_frame_id_, "base_link_hor");
+    nh_.param<std::string>("fixed_frame_id", fixed_frame_id_, "map");
     nh_.param<double>("tf_wait_duration", tf_wait_duration_, 0.1);
 
     int depthWidth, depthHeight;
     nh_.param<int>("depth_width" , depthWidth , 640);
     nh_.param<int>("depth_height", depthHeight, 360);
 
-    double gridMapLayerHeight, gridMapAccumulationWeight;
+    double gridMapLayerHeight, gridMapAccumulationWeight, gridMapDecay;
     nh_.param<int>("grid_map_width" , gridMapWidth_ , 1000);
     nh_.param<int>("grid_map_height", gridMapHeight_, 1000);
     nh_.param<double>("grid_map_resolution", gridMapResolution_, 0.01);
     nh_.param<double>("grid_map_layer_height", gridMapLayerHeight, 1);
-    nh_.param<double>("grid_map_accumulation_weight", gridMapAccumulationWeight, 1);
+    nh_.param<double>("grid_map_accumulation_weight", gridMapAccumulationWeight, 0.001);
+    nh_.param<double>("grid_map_decay", gridMapDecay, 0.5);
 
     double minDepth, maxDepth, depthHitThreshold;
     int unknownDepthColor;
@@ -43,7 +45,7 @@ DepthImageProjectorNode::DepthImageProjectorNode(const ros::NodeHandle& nh, cons
     projector_ = std::make_unique<cgs::DepthImageProjector>(
         depthWidth, depthHeight, 
         gridMapWidth_, gridMapHeight_, gridMapResolution_, 
-        gridMapLayerHeight, gridMapAccumulationWeight,
+        gridMapLayerHeight, gridMapAccumulationWeight, gridMapDecay, 
         minDepth, maxDepth, depthHitThreshold, unknownDepthColor, 
         vertexShader, geometryShader, fragmentShader, 
         vertexShaderScaling, fragmentShaderScaling
@@ -64,8 +66,12 @@ void DepthImageProjectorNode::depthCallback(const sensor_msgs::Image::ConstPtr&
         ROS_ERROR_STREAM("cv_bridge exception: " << e.what());
         return;
     }
+
+    //called first time 
+    if (!previousTimestamp_.isValid())
+        previousTimestamp_ = cameraInfoMsg->header.stamp;    
 
-    std::array<float, 16> depthToMap;
+    std::array<float, 16> depthToMap, mapToPreviousMap;
     try
     {
         if (tfListener_.waitForTransform(map_frame_id_, cameraInfoMsg->header.frame_id,
@@ -76,18 +82,32 @@ void DepthImageProjectorNode::depthCallback(const sensor_msgs::Image::ConstPtr&
                 cameraInfoMsg->header.stamp, transform);
             getTransformMatrixArray(transform, depthToMap);
         }
+
+        if (tfListener_.waitForTransform(map_frame_id_, fixed_frame_id_,
+              cameraInfoMsg->header.stamp, ros::Duration(tf_wait_duration_)))
+        {
+            tf::StampedTransform tramsform;
+            tfListener_.lookupTransform(
+                map_frame_id_, previousTimestamp_,
+                map_frame_id_, cameraInfoMsg->header.stamp, 
+                fixed_frame_id_, tramsform);
+            getTransformMatrixArray(tramsform, mapToPreviousMap);
+        }
     }
     catch (tf::TransformException& ex)
     {
         ROS_WARN("Transform Exception in depthCallback(). %s", ex.what());
         return;
     }
 
+    //Update previous timestamp
+    previousTimestamp_ = cameraInfoMsg->header.stamp;    
+
     //Update projector parameters with camera info
     projector_->updateProjectionMatrix(
-        {static_cast<float>(cameraInfoMsg->K[0])           , static_cast<float>(cameraInfoMsg->K[4])}, 
-        {static_cast<float>(cameraInfoMsg->K[2])           , static_cast<float>(cameraInfoMsg->K[5])}, 
-        depthToMap
+        {static_cast<float>(cameraInfoMsg->K[0]), static_cast<float>(cameraInfoMsg->K[4])}, 
+        {static_cast<float>(cameraInfoMsg->K[2]), static_cast<float>(cameraInfoMsg->K[5])}, 
+        depthToMap, mapToPreviousMap
     );
 
     //Perform projection
@@ -115,7 +135,6 @@ void DepthImageProjectorNode::depthCallback(const sensor_msgs::Image::ConstPtr&
     grid.data.reserve(size);
     std::copy(data, data + size, std::back_inserter(grid.data));
     mapPublisher_.publish(grid);
-
 }
 
 void DepthImageProjectorNode::getTransformMatrixArray(const tf::Transform& transform, std::array<float, 16>& matrix)

opengl_ros/src/depth_image_projector_nodecore.h

@@ -24,8 +24,10 @@ class DepthImageProjectorNode
     //tf
     std::string depth_frame_id_;
     std::string map_frame_id_;
+    std::string fixed_frame_id_;
     tf::TransformListener tfListener_;
     double tf_wait_duration_;
+    ros::Time previousTimestamp_{0};
 
     //Other parameters
     int gridMapWidth_, gridMapHeight_;

opengl_ros_lib/include/depth_image_projector.h

@@ -19,15 +19,15 @@ class DepthImageProjector final
     DepthImageProjector(
         int depthWidth, int depthHeight, 
         int gridMapWidth, int gridMapHeight, float gridMapResolution, 
-        float gridMapLayerHeight, float gridMapAccumulationWeight,
+        float gridMapLayerHeight, float gridMapAccumulationWeight, float gridMapDecay, 
         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();
 
     void updateProjectionMatrix(
-        const std::array<float, 2> depthFocalLength, const std::array<float, 2> depthCenter, 
-        const std::array<float, 16> depthToMap);
+        const std::array<float, 2>& depthFocalLength, const std::array<float, 2>& depthCenter, 
+        const std::array<float, 16>& depthToMap, const std::array<float, 16>& mapToPreviousMap);
     void project(cv::Mat& dest, const cv::Mat& depth);
 
     DepthImageProjector(const DepthImageProjector&) = delete;

opengl_ros_lib/shader/fs_rgb_to_occupancy_grid.glsl

@@ -1,34 +1,48 @@
 #version 450 core
 uniform vec2 resolution;
 uniform sampler2D texture;
+uniform sampler2D accumulationTexture;
 uniform int unknownDepthColor;
+uniform float gridMapDecay;
+uniform float gridMapResolution;
+uniform mat4 mapToPreviousMap;
 
 in vec4 gl_FragCoord;
-out float fragmentColor;
+
+layout(location = 0) 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
 
+layout(location = 1) out vec3 accumulationOutput;
+//output for temporal accumulation
+
 void main(void)
 {
-    float u = gl_FragCoord.x / resolution.x;
-    float v = gl_FragCoord.y / resolution.y;
+    //sample previous buffer
+    vec4 previousFragCoord = mapToPreviousMap * vec4(gl_FragCoord.xy * gridMapResolution, 0, 1) / gridMapResolution;
+    vec2 previousUv = previousFragCoord.xy / resolution;
+    vec3 previousColor = texture2D(accumulationTexture, previousUv).xyz;
 
-    vec3 color = texture2D(texture, vec2(u, v)).xyz;
+    //sample current buffer and accumulate
+    vec2 uv = gl_FragCoord.xy / resolution;
+    vec3 color = texture2D(texture, uv).xyz + gridMapDecay * previousColor;
     //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 
+    //y ... non-zero if valid depth is measured in any ray
+    //z ... non-zero if any ray achieved at the pixel 
 
-    if (color.z < 0.5)
+    //output for temporal acccumulation
+    accumulationOutput = color;
+
+    //output occupancy grid
+    if (color.z == 0)
     {
         fragmentColor = 1.0;
         return;
     }
-    
-    if (color.y < 0.5) {
+    if (color.y == 0) {
         fragmentColor = unknownDepthColor / 255.0;
         return;
     }
-
-    fragmentColor = color.x * 100.0 / 255.0; //rescale to 0~100
+    fragmentColor = clamp(color.x, 0, 1) * 100.0 / 255.0; //rescale to 0~100
 }