diff --git a/src/conversion.rs b/src/conversion.rs index 0247b1488c..c0dd8f33f7 100644 --- a/src/conversion.rs +++ b/src/conversion.rs @@ -15,7 +15,7 @@ use glium::glutin::event_loop::{ControlFlow, EventLoop}; use glium::Texture2d; use glium::texture::RawImage2d; use glium::vertex::VertexBufferAny; -use image::{DynamicImage, ImageFormat}; +use image::{DynamicImage}; use image::flat::{FlatSamples, SampleLayout}; use image::GenericImage; use image::ImageBuffer; @@ -33,6 +33,7 @@ use opencv::prelude::*; use k4a_sys_wrapper::Device; use k4a_sys_wrapper::device_get_installed_count; use k4a_sys_wrapper::Image; +use point_cloud::pixel_structs::DepthPixel; /// We can't send trait 'Texture2dDataSource' impl 'RawImage2d' as it requires its data has /// the same lifetime, so here we collect it together here. @@ -90,3 +91,36 @@ pub fn depth_to_image(image: &Image) -> Result { Ok(img) } + +// Not efficient, but good for debugging +pub fn k4a_image_to_rust_image_for_debug(image: &Image) -> Result { + let width = image.get_width_pixels() as u32; + let height= image.get_height_pixels() as u32; + let image_buffer = image.get_buffer(); + let image_format = image.get_format(); + + let rgb_image = match image_format { + Depth16 => { + let output_image = DynamicImage::new_rgb8(width, height); + let typed_buffer = image_buffer as *const DepthPixel; + + let mut rgb_image = output_image.to_rgb(); + + let mut offset = 0; + for x in 0 .. width { + for y in 0 .. height { + let pixel = unsafe { *typed_buffer.offset(offset) }; + let scaled_pixel = (pixel as f32 / std::u16::MAX as f32); + let scaled_pixel = (scaled_pixel * std::u8::MAX as f32) as u8; + rgb_image.put_pixel(x, y, Rgb([scaled_pixel, scaled_pixel, scaled_pixel])); + offset += 1; + } + } + + rgb_image + }, + _ => unimplemented!("conversion not implemented for: {:?}", image_format), + }; + + return Ok(rgb_image) +} diff --git a/src/graphics_imgui.rs b/src/graphics_imgui.rs index ffd5c9186c..d404391e4b 100644 --- a/src/graphics_imgui.rs +++ b/src/graphics_imgui.rs @@ -59,15 +59,15 @@ pub fn run(capture_provider: Arc, calibration_data: k4a_sys::k4 let mut visualizer = PointCloudVisualizer::new( true, - ColorizationStrategy::Color, + ColorizationStrategy::Simple, calibration_data ); //rebinder.restore(); let mut texture = ViewerImage::create( - 800, - 800, + 2000, + 2000, None, None ).expect("ViewerImage texture creation should work"); diff --git a/src/point_cloud/gpu_point_cloud_converter.rs b/src/point_cloud/gpu_point_cloud_converter.rs index 34830f3eeb..e7f4dcd822 100644 --- a/src/point_cloud/gpu_point_cloud_converter.rs +++ b/src/point_cloud/gpu_point_cloud_converter.rs @@ -20,6 +20,9 @@ use opengl_wrapper::{Buffer, gl_get_error}; use opengl_wrapper::Texture; use opengl_wrapper::OpenGlError; use point_cloud::compile_shader::compile_shader; +use point_cloud::pixel_structs::DepthPixel; +use conversion::{depth_to_image, k4a_image_to_rust_image_for_debug}; +use std::path::Path; pub type Result = std::result::Result; @@ -175,22 +178,22 @@ impl GpuPointCloudConverter { } } - // Takes depth data and turns it into a texture containing the XYZ coordinates of the depth map - // using the most recently set-to-active XY table. The input depth image and output texture - // (if already set) must be of the same resolution that was used to generate that XY table, or - // else behavior is undefined. - // - // Essentially a reimplementation of k4a::transform::depth_image_to_point_cloud on the GPU. - // This is much more performant than k4a::transform::depth_image_to_point_cloud, but is a bit - // more unwieldly to use since you have to use its output in shaders. - // - // The output texture has an internal format of GL_RGBA32F and is intended to be used directly - // by other OpenGL shaders as an image2d uniform. - // - // To avoid excess image allocations, you can reuse a texture that was previously output - // by this function, provided the depth image and XY table previously used was for the same - // sized texture. - // + /// Takes depth data and turns it into a texture containing the XYZ coordinates of the depth map + /// using the most recently set-to-active XY table. The input depth image and output texture + /// (if already set) must be of the same resolution that was used to generate that XY table, or + /// else behavior is undefined. + /// + /// Essentially a reimplementation of k4a::transform::depth_image_to_point_cloud on the GPU. + /// This is much more performant than k4a::transform::depth_image_to_point_cloud, but is a bit + /// more unwieldly to use since you have to use its output in shaders. + /// + /// The output texture has an internal format of GL_RGBA32F and is intended to be used directly + /// by other OpenGL shaders as an image2d uniform. + /// + /// To avoid excess image allocations, you can reuse a texture that was previously output + /// by this function, provided the depth image and XY table previously used was for the same + /// sized texture. + /// pub fn convert(&self, depth_image: &k4a_sys_wrapper::Image, output_texture: &mut Texture) -> Result<()> @@ -200,6 +203,32 @@ impl GpuPointCloudConverter { return Err(PointCloudComputeError::UnknownError); } + k4a_image_to_rust_image_for_debug(depth_image) + .expect("depth_to_image should work") + .save(Path::new("depth_before.png")) + .expect("should save"); + + unsafe { + let width = depth_image.get_width_pixels() as i32; + let height = depth_image.get_height_pixels() as i32; + let format = depth_image.get_format(); + println!("Depth Image Dimensions: {}x{} (format: {:?})", width, height, format); + + let depth_image_buffer = depth_image.get_buffer(); + let mut typed_buffer = depth_image_buffer as *mut DepthPixel; + + // TODO: This should overwrite the first 150 lines. + for i in 0 .. 1280 * 150 { + (*typed_buffer.offset(i)) = 5000; + (*typed_buffer.offset(i)) = 5000; + } + } + + k4a_image_to_rust_image_for_debug(depth_image) + .expect("depth_to_image should work") + .save(Path::new("depth_after.png")) + .expect("should save"); + let width = depth_image.get_width_pixels() as i32; let height = depth_image.get_height_pixels() as i32; @@ -227,6 +256,7 @@ impl GpuPointCloudConverter { 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 + //let num_bytes: GLuint = (width * height * size_of::() as i32) as GLuint; // libc::uint16_t = u16 // GLubyte *textureMappedBuffer = reinterpret_cast(...) let mut texture_mapped_buffer = gl::MapBufferRange( @@ -242,19 +272,18 @@ impl GpuPointCloudConverter { let mut depth_src = depth_image.get_buffer(); - // TODO TESTING + // TODO TESTING - setting this to nothing destroys the final output "line". Hmm... //std::ptr::write_bytes(texture_mapped_buffer as *mut u8, 120, num_bytes as usize); - std::ptr::copy_nonoverlapping::(depth_src, texture_mapped_buffer as *mut u8, num_bytes as usize); - - //std::ptr::copy::(depth_src, texture_mapped_buffer as *mut u8, num_bytes as usize); + //std::ptr::copy_nonoverlapping::(depth_src, texture_mapped_buffer as *mut u8, num_bytes as usize); + std::ptr::copy::(depth_src, texture_mapped_buffer as *mut u8, num_bytes as usize); let result = gl::UnmapBuffer(gl::PIXEL_UNPACK_BUFFER); if result == gl::FALSE { return Err(PointCloudComputeError::UnknownError); } - // TODO: Changing the bounds here affected the "line" that is being rendered. + // TODO TESTING - Changing the bounds here affected the final output "line" that is being rendered. gl::TexSubImage2D( gl::TEXTURE_2D, // target 0, // level diff --git a/src/point_cloud/point_cloud_renderer.rs b/src/point_cloud/point_cloud_renderer.rs index 03c1cebecc..2232ade21e 100644 --- a/src/point_cloud/point_cloud_renderer.rs +++ b/src/point_cloud/point_cloud_renderer.rs @@ -410,15 +410,14 @@ impl PointCloudRenderer { let mut color_src = color_image.get_buffer(); let result = unsafe { - std::ptr::copy::(color_src, vertex_mapped_buffer as *mut u8, - color_image_size_bytes as usize); - //std::ptr::copy_nonoverlapping::(color_src, vertex_mapped_buffer as *mut u8, // color_image_size_bytes as usize); - // TODO: Based on writing pure white, this isn't the problem: + // TODO TESTING - writing pure white changes the color of the final output "line" to white: //std::ptr::write_bytes(vertex_mapped_buffer as *mut u8, 255, color_image_size_bytes as usize); + std::ptr::copy::(color_src, vertex_mapped_buffer as *mut u8, color_image_size_bytes as usize); + gl::UnmapBuffer(gl::ARRAY_BUFFER) }; diff --git a/src/point_cloud/point_cloud_visualiser.rs b/src/point_cloud/point_cloud_visualiser.rs index 58652cf848..dec0e9c200 100644 --- a/src/point_cloud/point_cloud_visualiser.rs +++ b/src/point_cloud/point_cloud_visualiser.rs @@ -368,6 +368,7 @@ impl PointCloudVisualizer { let mut dst_pixel_2: *mut BgraPixel = std::mem::transmute_copy(&dst_pixel); let mut dst_pixel_3= std::slice::from_raw_parts_mut(dst_pixel_2, dst_length as usize); + //unimplemented!("FIX THIS SHIT. PROBABLY THE BUG"); // TODO // let end_pixel = dst_pixel_3 + dst_length; /*while src_pixel_3 != end_pixel {