Pointcloud gl fix

src/gl/pointcloud-gl.cpp

@@ -22,6 +22,62 @@ using namespace rs2;
 using namespace librealsense;
 using namespace librealsense::gl;
 
+static const char* extract_points_text =
+"#version 130\n"
+"in vec2 textCoords;\n"
+"out vec4 output_xyz;\n"
+"uniform sampler2D textureSampler;\n"
+"uniform vec2 focal;\n"
+"uniform vec2 principal;\n"
+"uniform float is_bc;\n"
+"uniform float coeffs[5];\n"
+"\n"
+"uniform float depth_scale;\n"
+"uniform float width;\n"
+"uniform float height;\n"
+"\n"
+"void main(void) \n"
+"{\n"
+"    float px = textCoords.x * width;\n"
+"    float py = (1.0 - textCoords.y) * height;\n"
+"    float x = (px - principal.x) / focal.x;\n"
+"    float y = (py - principal.y) / focal.y;\n"
+"    float xo = x;\n"
+"    float yo = y;\n"
+"    if(is_bc == 2.0)\n"
+"    {\n"
+"       for (int i = 0; i < 10; i++)\n"
+"       {\n"
+"           float r2 = x * x + y * y;\n"
+"           float icdist = 1.0 / (1.0 + ((coeffs[4] * r2 + coeffs[1])*r2 + coeffs[0])*r2);\n"
+"           float xq = x / icdist;\n"
+"           float yq = y / icdist;\n"
+"           float delta_x = 2 * coeffs[2] * xq*yq + coeffs[3] * (r2 + 2 * xq*xq);\n"
+"           float delta_y = 2 * coeffs[3] * xq*yq + coeffs[2] * (r2 + 2 * yq*yq);\n"
+"           x = (xo - delta_x)*icdist;\n"
+"           y = (yo - delta_y)*icdist;\n"
+"       }\n"
+"    }\n"
+"    if (is_bc == 4.0)\n"
+"    {\n"
+"        for (int i = 0; i < 10; i++)\n"
+"        {\n"
+"            float r2 = x * x + y * y;\n"
+"            float icdist = 1.0 / (1.0 + ((coeffs[4] * r2 + coeffs[1])*r2 + coeffs[0])*r2);\n"
+"            float delta_x = 2 * coeffs[2] * x*y + coeffs[3] * (r2 + 2 * x*x);\n"
+"            float delta_y = 2 * coeffs[3] * x*y + coeffs[2] * (r2 + 2 * y*y);\n"
+"            x = (xo - delta_x)*icdist;\n"
+"            y = (yo - delta_y)*icdist;\n"
+"        }\n"
+"    }\n"
+"    vec2 tex = vec2(textCoords.x, 1.0 - textCoords.y);\n"
+"    vec4 dp = texture(textureSampler, tex);\n"
+"    float nd = (dp.x + dp.y * 256.0) * 256.0;\n"
+"    float depth = depth_scale * nd;\n"
+"    vec4 xyz = vec4(x * depth, y * depth, depth, 1.0);\n"
+"    output_xyz = xyz;\n"
+"}";
+
 static const char* project_fragment_text =
 "#version 130\n"
 "in vec2 textCoords;\n"
@@ -234,6 +290,58 @@ static const char* occulution_fragment_text =
 "    }\n"
 "}";
 
+class extract_points_shader : public texture_2d_shader
+{
+public:
+    extract_points_shader()
+        : texture_2d_shader(shader_program::load(
+            texture_2d_shader::default_vertex_shader(),
+            extract_points_text,
+            "position", "textureCoords",
+            "output_xyz", nullptr))
+    {
+        _focal_location = _shader->get_uniform_location("focal");
+        _principal_location = _shader->get_uniform_location("principal");
+        _is_bc_location = _shader->get_uniform_location("is_bc");
+        _coeffs_location = _shader->get_uniform_location("coeffs");
+
+        _depth_scale_location = _shader->get_uniform_location("depth_scale");
+        _width_location = _shader->get_uniform_location("width");
+        _height_location = _shader->get_uniform_location("height");
+    }
+
+    void set_size(int w, int h)
+    {
+        _shader->load_uniform(_width_location, (float)w);
+        _shader->load_uniform(_height_location, (float)h);
+    }
+
+    void set_intrinsics(const rs2_intrinsics& intr)
+    {
+        rs2::float2 focal{ intr.fx, intr.fy };
+        rs2::float2 principal{ intr.ppx, intr.ppy };
+        _shader->load_uniform(_focal_location, focal);
+        _shader->load_uniform(_principal_location, principal);
+        _shader->load_uniform(_is_bc_location, (float)(int)intr.model);
+        glUniform1fv(_coeffs_location, 5, intr.coeffs);
+    }
+
+    void set_depth_scale(float depth_scale)
+    {
+        _shader->load_uniform(_depth_scale_location, depth_scale);
+    }
+
+private:
+    int _focal_location;
+    int _principal_location;
+    int _is_bc_location;
+    int _coeffs_location;
+    int _depth_scale_location;
+
+    int _width_location;
+    int _height_location;
+};
+
 class project_shader : public texture_2d_shader
 {
 public:
@@ -375,6 +483,7 @@ void pointcloud_gl::cleanup_gpu_resources()
 {
     _projection_renderer.reset();
     _occu_renderer.reset();
+    _point_cloud_processor.reset();
     _enabled = 0;
 }
 void pointcloud_gl::create_gpu_resources()
@@ -383,6 +492,7 @@ void pointcloud_gl::create_gpu_resources()
     {
         _projection_renderer = std::make_shared<visualizer_2d>(std::make_shared<project_shader>());
         _occu_renderer = std::make_shared<visualizer_2d>(std::make_shared<occulution_shader>());
+        _point_cloud_processor = std::make_shared<visualizer_2d>(std::make_shared<extract_points_shader>());
     }
     _enabled = glsl_enabled() ? 1 : 0;
 }
@@ -423,12 +533,97 @@ const librealsense::float3* pointcloud_gl::depth_to_points(
         _depth_data = depth_frame;
         _depth_scale = depth_frame.get_units();
         _depth_intr = depth_intrinsics;
+
+        //if map_texture is false (e.g. headless operation)
+        if(!_extrinsics.has_value() || !_other_intrinsics.has_value())
+        {
+          auto vid_frame = depth_frame.as<rs2::video_frame>();
+          auto height = vid_frame.get_height();
+          auto width = vid_frame.get_width();
+          get_gl_points(output, width, height);
+        }
+        //else, defer point extraction as it will be calculated along with texture mapping process
     }, [&]{
         _enabled = false;
     });
     return nullptr;
 }
 
+void pointcloud_gl::get_gl_points(rs2::points& output, const unsigned int width, const unsigned int height)
+{
+    auto viz = _point_cloud_processor;
+    auto frame_ref = (frame_interface*)output.get();
+
+    auto gf = dynamic_cast<gpu_addon_interface*>(frame_ref);
+
+    if (!gf)
+        throw std::runtime_error("Frame interface is not gpu addon interface");
+
+    uint32_t depth_texture;
+
+    if (auto input_frame = _depth_data.as<rs2::gl::gpu_frame>())
+    {
+        depth_texture = input_frame.get_texture_id(0);
+    }
+    else
+    {
+        glGenTextures(1, &depth_texture);
+        glBindTexture(GL_TEXTURE_2D, depth_texture);
+        auto depth_data = _depth_data.get_data();
+        glTexImage2D(GL_TEXTURE_2D, 0, GL_RG8, width, height, 0, GL_RG, GL_UNSIGNED_BYTE, depth_data);
+        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+    }
+
+    // rendering output
+    uint32_t output_xyz;
+
+    gf->get_gpu_section().output_texture(0, &output_xyz, TEXTYPE_XYZ);
+
+    // fbo1 output
+    uint32_t fbo1_xyz = output_xyz;
+
+    // fbo1 - projection rendering
+    fbo fbo1(width, height);
+
+    glBindTexture(GL_TEXTURE_2D, fbo1_xyz);
+    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB16F, width, height, 0, GL_RGB, GL_FLOAT, nullptr);
+    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+
+    glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, fbo1_xyz, 0);
+    glBindTexture(GL_TEXTURE_2D, 0);
+
+    gf->get_gpu_section().set_size(width, height);
+
+    fbo1.bind();
+
+    GLuint attachments[2] = { GL_COLOR_ATTACHMENT0, GL_COLOR_ATTACHMENT1 };
+    glDrawBuffers(2, attachments);
+
+    glClearColor(0, 0, 0, 1);
+    glClear(GL_COLOR_BUFFER_BIT);
+
+    auto& shader = (extract_points_shader&)viz->get_shader();
+    shader.begin();
+    shader.set_depth_scale(_depth_scale);
+    shader.set_intrinsics(_depth_intr);
+    shader.set_size(width, height);
+
+    viz->draw_texture(depth_texture);
+    shader.end();
+
+    fbo1.unbind();
+
+    glBindTexture(GL_TEXTURE_2D, 0);
+    if (!_depth_data.is<rs2::gl::gpu_frame>())
+    {
+        glDeleteTextures(1, &depth_texture);
+    }
+}
+
 void pointcloud_gl::get_texture_map(
     rs2::points output,
     const librealsense::float3* points,

src/gl/pointcloud-gl.h

@@ -31,6 +31,9 @@ namespace librealsense
                 const rs2_intrinsics &other_intrinsics,
                 const rs2_extrinsics& extr,
                 float2* pixels_ptr) override;
+            void get_gl_points(rs2::points& output,
+                const unsigned int width,
+                const unsigned int height);
             rs2::points allocate_points(
                 const rs2::frame_source& source, 
                 const rs2::frame& f) override;
@@ -39,6 +42,7 @@ namespace librealsense
 
             std::shared_ptr<rs2::visualizer_2d> _projection_renderer;
             std::shared_ptr<rs2::visualizer_2d> _occu_renderer;
+            std::shared_ptr<rs2::visualizer_2d> _point_cloud_processor;
 
             rs2::depth_frame _depth_data;
             float _depth_scale;