Skip to content

Commit

Permalink
Fix: OpenGL based point cloud is empty if depth mapping texture is no…
Browse files Browse the repository at this point in the history 
…t provided.

See: IntelRealSense/realsense-ros#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.
  • Loading branch information
@vahapt
vahapt committed Oct 17, 2024
1 parent e1688cc commit 81b78a1
Show file tree
Hide file tree
Showing 2 changed files with 199 additions and 0 deletions.
195 changes: 195 additions & 0 deletions src/gl/pointcloud-gl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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()
Expand All @@ -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;
}
Expand Down Expand Up @@ -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,
Expand Down
4 changes: 4 additions & 0 deletions src/gl/pointcloud-gl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand Down

0 comments on commit 81b78a1

Please sign in to comment.