From c65d0c9b64ba77e4e69958024cf51de3b9c719b6 Mon Sep 17 00:00:00 2001 From: Brandon Thomas Date: Sun, 19 Apr 2020 06:02:49 -0400 Subject: [PATCH] fix xy_table init 'valid' bit, lots of debug statements... --- src/graphics_gl.rs | 9 +-- src/point_cloud/point_cloud_compute_shader.rs | 70 ++++++++++++++++--- src/point_cloud/point_cloud_visualiser.rs | 29 +++++--- 3 files changed, 88 insertions(+), 20 deletions(-) diff --git a/src/graphics_gl.rs b/src/graphics_gl.rs index 5541ac8c2e..3ef4fa9626 100644 --- a/src/graphics_gl.rs +++ b/src/graphics_gl.rs @@ -364,11 +364,12 @@ pub fn run(capture_provider: Arc, calibration_data: k4a_sys::k4 // TODO: This belongs in a worker thread with buffers on both producer and consumer. if let Some(capture) = capture_provider.get_capture() { - println!("Got capture: {:?}", capture.0); + println!("\n\n------- event loop: got capture ------"); + println!("capture: {:?}", capture.0); unsafe { // TODO: CLEARING FOR TEMPORARY DEBUGGING. - gl::ClearColor(0.8, 0.0, 0.0, 1.0); - gl::Clear(gl::COLOR_BUFFER_BIT); + //gl::ClearColor(0.8, 0.0, 0.0, 1.0); + //gl::Clear(gl::COLOR_BUFFER_BIT); } visualizer.update_texture(&texture, capture) @@ -385,7 +386,7 @@ pub fn run(capture_provider: Arc, calibration_data: k4a_sys::k4 } }); - println!("Updated capture"); + println!("swapping buffers"); gl_window.swap_buffers().unwrap(); } }); diff --git a/src/point_cloud/point_cloud_compute_shader.rs b/src/point_cloud/point_cloud_compute_shader.rs index 8d36738f21..1a4428fe11 100644 --- a/src/point_cloud/point_cloud_compute_shader.rs +++ b/src/point_cloud/point_cloud_compute_shader.rs @@ -16,20 +16,35 @@ use libc; use k4a_sys_wrapper; use k4a_sys_wrapper::Image; use k4a_sys_wrapper::ImageFormat; -use opengl_wrapper::Buffer; +use opengl_wrapper::{Buffer, gl_get_error}; use opengl_wrapper::Texture; +use opengl_wrapper::OpenGlError; use point_cloud::compile_shader::compile_shader; pub type Result = std::result::Result; #[derive(Clone, Debug)] pub enum PointCloudComputeError { + OpenGlError(OpenGlError), UnknownError, } +impl From for PointCloudComputeError { + fn from(error: OpenGlError) -> Self { + PointCloudComputeError::OpenGlError(error) + } +} + impl std::fmt::Display for PointCloudComputeError { fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result { - write!(f, "unknown point cloud COMPUTE error") + let description = match self { + PointCloudComputeError::OpenGlError(inner) => { + format!("Compute OpenGL error: {}", inner) + }, + PointCloudComputeError::UnknownError => "Unknown Compute Error".into(), + }; + + write!(f, "{}", description) } } @@ -180,7 +195,10 @@ impl PointCloudComputeShader { depth_image: &k4a_sys_wrapper::Image, output_texture: &mut Texture) -> Result<()> { + println!("==== ComputeShader.convert()"); + if !self.xy_table_texture.is_initialized() { + println!("Error: xyz texture is not initialized!!!"); // throw std::logic_error("You must call SetActiveXyTable at least once before calling Convert!"); return Err(PointCloudComputeError::UnknownError); } @@ -189,30 +207,40 @@ impl PointCloudComputeShader { let height = depth_image.get_height_pixels() as i32; if !output_texture.is_initialized() { + println!("Output texture is not initialized. Initializing."); + output_texture.init(); unsafe { + println!("-> gl::ActiveTexture(TEXTURE0)"); gl::ActiveTexture(gl::TEXTURE0); + println!("-> gl::BindTexture(): {:?}", output_texture.id()); gl::BindTexture(gl::TEXTURE_2D, output_texture.id()); // The format that the point cloud texture uses internally to store points. // If you want to use the texture that this outputs from your shader, you // need to pass this as the format argument to glBindImageTexture(). // static constexpr GLenum PointCloudTextureFormat = GL_RGBA32F; + println!("-> gl::TexStorage2D(...)"); gl::TexStorage2D(gl::TEXTURE_2D, 1, POINT_CLOUD_TEXTURE_FORMAT, width, height); + println!("-> gl::TexParameteri()"); gl::TexParameteri(gl::TEXTURE_2D, gl::TEXTURE_MIN_FILTER, gl::NEAREST as i32); + println!("-> gl::TexParameteri()"); gl::TexParameteri(gl::TEXTURE_2D, gl::TEXTURE_MAG_FILTER, gl::NEAREST as i32); } } unsafe { // Upload data to uniform texture + println!("-> gl::BindBuffer(): {:?}", self.depth_image_pixel_buffer.id()); gl::BindBuffer(gl::PIXEL_UNPACK_BUFFER, self.depth_image_pixel_buffer.id()); + println!("-> gl::BindTexture(): {:?}", self.depth_image_texture.id()); gl::BindTexture(gl::TEXTURE_2D, self.depth_image_texture.id()); let num_bytes: GLuint = (width * height * size_of::() as i32) as GLuint; // libc::uint16_t = u16 + println!("-> gl::MapBufferRange(...)"); // GLubyte *textureMappedBuffer = reinterpret_cast(...) let mut texture_mapped_buffer = gl::MapBufferRange( gl::PIXEL_UNPACK_BUFFER, @@ -222,6 +250,7 @@ impl PointCloudComputeShader { ); if texture_mapped_buffer as usize == 0 { + println!("Error: Texture buffer mapping failed."); return Err(PointCloudComputeError::UnknownError); } @@ -230,14 +259,18 @@ impl PointCloudComputeShader { // std::copy(depthSrc, depthSrc + numBytes, textureMappedBuffer); // NB: Slightly different calculation; libc::uint16_t = u16 (TODO: is this byte multiples?) // Rust function copies count * size_of::() + println!("Copying depth_src to texture_mapped_buffer"); let count = (width * height * 2) as usize; std::ptr::copy_nonoverlapping::(depth_src, texture_mapped_buffer as *mut u8, count); + println!("-> gl::UnmapBuffer(PIXEL_UNPACK_BUFFER)"); let result = gl::UnmapBuffer(gl::PIXEL_UNPACK_BUFFER); if result == gl::FALSE { + println!("Error: unknown errror"); return Err(PointCloudComputeError::UnknownError); } + println!("-> gl::TexSubImage2D(...)"); gl::TexSubImage2D( gl::TEXTURE_2D, // target 0, // level @@ -249,13 +282,18 @@ impl PointCloudComputeShader { gl::UNSIGNED_SHORT, //constexpr GLenum depthImageDataType = GL_UNSIGNED_SHORT; null(), // data ); + println!("-> gl::BindBuffer(PIXEL_UNPACK_BUFFER)"); gl::BindBuffer(gl::PIXEL_UNPACK_BUFFER, 0); + println!("-> gl::UseProgram(): {:?}", self.program_id); gl::UseProgram(self.program_id); // Bind textures that we're going to pass to the texture + println!("-> gl::ActiveTexture(TEXTURE0)"); gl::ActiveTexture(gl::TEXTURE0); + println!("-> gl::BindTexture(): {:?}", output_texture.id()); gl::BindTexture(gl::TEXTURE_2D, output_texture.id()); + println!("-> gl::BindImageTexture(...)"); gl::BindImageTexture( 0, output_texture.id(), @@ -266,8 +304,11 @@ impl PointCloudComputeShader { POINT_CLOUD_TEXTURE_FORMAT ); + println!("-> gl::ActiveTexture(TEXTURE1)"); gl::ActiveTexture(gl::TEXTURE1); + println!("-> gl::BindTexture(): {:?}", self.depth_image_texture.id()); gl::BindTexture(gl::TEXTURE_2D, self.depth_image_texture.id()); + println!("-> gl::BindImageTexture(...): {:?}", self.depth_image_texture.id()); gl::BindImageTexture( 1, self.depth_image_texture.id(), @@ -277,10 +318,14 @@ impl PointCloudComputeShader { gl::READ_ONLY, gl::R16UI, //constexpr GLenum depthImageInternalFormat = GL_R16UI; ); + println!("-> gl::Uniform1i(): {:?}", self.depth_image_id); gl::Uniform1i(self.depth_image_id, 1); + println!("-> gl::ActiveTexture(TEXTURE2)"); gl::ActiveTexture(gl::TEXTURE2); + println!("-> gl::BindTexture(): {:?}", self.xy_table_texture.id()); gl::BindTexture(gl::TEXTURE_2D, self.xy_table_texture.id()); + println!("-> gl::BindImageTexture(): {:?}", self.xy_table_texture.id()); gl::BindImageTexture( 2, self.xy_table_texture.id(), @@ -293,6 +338,8 @@ impl PointCloudComputeShader { gl::Uniform1i(self.xy_table_id, 2); // Render point cloud + println!("-> gl::DispatchCompute(): {}x{}", + depth_image.get_width_pixels(), depth_image.get_height_pixels()); gl::DispatchCompute( depth_image.get_width_pixels() as u32, depth_image.get_height_pixels() as u32, @@ -300,10 +347,11 @@ impl PointCloudComputeShader { ); // Wait for the rendering to finish before allowing reads to the texture we just wrote + println!("-> gl::MemoryBarrier(TEXTURE_FETCH_BARRIER_BIT)"); gl::MemoryBarrier(gl::TEXTURE_FETCH_BARRIER_BIT); // TODO: Return status or error. - let status = gl::GetError(); + gl_get_error()?; Ok(()) } @@ -476,8 +524,8 @@ impl PointCloudComputeShader { for x in 0..width { p.xy.x = x as f32; - let mut result : c_int = -1; - unsafe { + let mut valid: c_int = -1; + let result = unsafe { // https://docs.rs/k4a-sys/0.2.0/k4a_sys/fn.k4a_calibration_2d_to_3d.html k4a_sys::k4a_calibration_2d_to_3d( &calibration, @@ -486,18 +534,24 @@ impl PointCloudComputeShader { calibration_type, // source camera calibration_type, // target camera &mut ray, // target point3d mm - &mut result // set to zero when valid result - ); + &mut valid // set to 1 when valid result, 0 when coordinate is not valid + ) }; - if result == 0 { + if result != k4a_sys::k4a_buffer_result_t_K4A_BUFFER_RESULT_SUCCEEDED { + return Err(PointCloudComputeError::UnknownError); + } + + if valid == 1 { unsafe { + //println!("This pixel is GOOD: {}, {}", ray.xyz.x, ray.xyz.y); xy_table_buffer3[idx].xy.x = ray.xyz.x; xy_table_buffer3[idx].xy.y = ray.xyz.y; } } else { unsafe { // This pixel is invalid + //println!("This pixel is invalid: {}", idx); xy_table_buffer3[idx].xy.x = 0.0; xy_table_buffer3[idx].xy.y = 0.0; } diff --git a/src/point_cloud/point_cloud_visualiser.rs b/src/point_cloud/point_cloud_visualiser.rs index 409e46c162..16e9ecf0c8 100644 --- a/src/point_cloud/point_cloud_visualiser.rs +++ b/src/point_cloud/point_cloud_visualiser.rs @@ -23,6 +23,7 @@ pub enum PointCloudVisualizerError { PointCloudRendererError(PointCloudRendererError), PointCloudComputeError(PointCloudComputeError), FramebufferError, + DepthToColorConversionFailed, /// Capture is missing depth info; drop the frame MissingDepthImage, /// Capture is missing color info; drop the frame @@ -46,6 +47,7 @@ impl std::fmt::Display for PointCloudVisualizerError { format!("Visualizer PointCloudCompute error: {}", inner) }, PointCloudVisualizerError::FramebufferError => "Visualizer Framebuffer Error".into(), + PointCloudVisualizerError::DepthToColorConversionFailed => "Visualizer Depth To Color Conversion Error".into(), PointCloudVisualizerError::UnsupportedMode => "Visualizer Unsupported Mode Error".into(), PointCloudVisualizerError::MissingDepthImage => "Visualizer Missing Depth Image (drop frame)".into(), PointCloudVisualizerError::MissingColorImage => "Visualizer Missing Color Image (drop frame)".into(), @@ -284,15 +286,25 @@ impl PointCloudVisualizer { .expect("Must be present");*/ if let Some(transformed_depth_image) = self.transformed_depth_image.as_ref() { - println!("Take transformed depth image..."); + println!("Take transformed depth image... {}x{} [{:?}]", + transformed_depth_image.get_width_pixels(), + transformed_depth_image.get_height_pixels(), + transformed_depth_image.get_format()); + unsafe { // TODO: Totally missed error handling here. - k4a_sys::k4a_transformation_depth_image_to_color_camera( + let result = k4a_sys::k4a_transformation_depth_image_to_color_camera( self.transformation.get_handle(), depth_image.get_handle(), transformed_depth_image.get_handle(), ); + if result != k4a_sys::k4a_buffer_result_t_K4A_BUFFER_RESULT_SUCCEEDED { + println!("FAILED: Depth to color conversion"); + return Err(PointCloudVisualizerError::DepthToColorConversionFailed); + } + + println!("Saving transformed depth image: {:?}", transformed_depth_image.get_format()); depth_image = transformed_depth_image.clone(); } } @@ -308,14 +320,15 @@ impl PointCloudVisualizer { return Err(PointCloudVisualizerError::PointCloudComputeError(err)); } - self.last_capture = Some(capture); // TODO: Inefficient. Should take ownership. + self.last_capture = Some(capture); if self.colorization_strategy == ColorizationStrategy::Color { - // TODO - // m_pointCloudColorization = std::move(colorImage); - //return Err(PointCloudVisualizerError::ColorSupportNotYetImplemented); - self.point_cloud_colorization = Some(maybe_color_image - .expect("logic above should handle")); + let color_image = maybe_color_image.expect("logic above should ensure present"); + println!("Owning point_cloud_colorization image: {}x{} {:?}", + color_image.get_width_pixels(), + color_image.get_height_pixels(), + color_image.get_format()); + self.point_cloud_colorization = Some(color_image); } else { // This creates a color spectrum based on depth.