From 81b78a11490e763dc360454fd6b4f6a87723133a Mon Sep 17 00:00:00 2001 From: vahapt <5289529+vahapt@users.noreply.github.com> Date: Thu, 17 Oct 2024 18:02:57 +0300 Subject: [PATCH] Fix: OpenGL based point cloud is empty if depth mapping texture is not provided. See: https://github.com/IntelRealSense/realsense-ros/issues/3206 When called through realsense-ros package, depth mapping texture is not provided, thus point cloud is not generated. A new shader is generated to handle the case. --- src/gl/pointcloud-gl.cpp | 195 +++++++++++++++++++++++++++++++++++++++ src/gl/pointcloud-gl.h | 4 + 2 files changed, 199 insertions(+) diff --git a/src/gl/pointcloud-gl.cpp b/src/gl/pointcloud-gl.cpp index 1487914acc..9fccc7c9e7 100644 --- a/src/gl/pointcloud-gl.cpp +++ b/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(std::make_shared()); _occu_renderer = std::make_shared(std::make_shared()); + _point_cloud_processor = std::make_shared(std::make_shared()); } _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(); + 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(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()) + { + 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()) + { + glDeleteTextures(1, &depth_texture); + } +} + void pointcloud_gl::get_texture_map( rs2::points output, const librealsense::float3* points, diff --git a/src/gl/pointcloud-gl.h b/src/gl/pointcloud-gl.h index d5964ec183..26e6583677 100644 --- a/src/gl/pointcloud-gl.h +++ b/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 _projection_renderer; std::shared_ptr _occu_renderer; + std::shared_ptr _point_cloud_processor; rs2::depth_frame _depth_data; float _depth_scale;