Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix: OpenGL based point cloud is empty if depth mapping texture is not provided #13462

Open
wants to merge 1 commit into
base: development
Choose a base branch
from
Open

Fix: OpenGL based point cloud is empty if depth mapping texture is not provided #13462

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
195 changes: 195 additions & 0 deletions src/gl/pointcloud-gl.cpp
View file
Open in desktop
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
Open in desktop
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
Loading