diff --git a/src/conversion.rs b/src/conversion.rs index c0dd8f33f7..c7ba9007aa 100644 --- a/src/conversion.rs +++ b/src/conversion.rs @@ -33,7 +33,8 @@ 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; +use point_cloud::pixel_structs::{DepthPixel, BgraPixel}; +use k4a_sys_wrapper; /// We can't send trait 'Texture2dDataSource' impl 'RawImage2d' as it requires its data has /// the same lifetime, so here we collect it together here. @@ -93,18 +94,20 @@ pub fn depth_to_image(image: &Image) -> Result { } // Not efficient, but good for debugging -pub fn k4a_image_to_rust_image_for_debug(image: &Image) -> Result { +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(); + println!("k4a_image_to_rust: {}x{} (format={:?}", width, height, image_format); + let rgb_image = match image_format { - Depth16 => { + k4a_sys_wrapper::ImageFormat::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 rgba_image = output_image.to_rgba(); let mut offset = 0; for x in 0 .. width { @@ -112,12 +115,27 @@ pub fn k4a_image_to_rust_image_for_debug(image: &Image) -> Result { + let output_image = DynamicImage::new_rgba16(width, height); + let typed_buffer = image_buffer as *const BgraPixel; - rgb_image + let mut rgba_image = output_image.to_rgba(); + + let mut offset = 0; + for x in 0 .. width { + for y in 0 .. height { + let pixel = unsafe { &*typed_buffer.offset(offset) }; + rgba_image.put_pixel(x, y, Rgba([pixel.red, pixel.blue, pixel.green, pixel.alpha])); + offset += 1; + } + } + rgba_image }, _ => unimplemented!("conversion not implemented for: {:?}", image_format), }; diff --git a/src/point_cloud/gpu_point_cloud_converter.rs b/src/point_cloud/gpu_point_cloud_converter.rs index 25cabfbd57..99f8cd4dd7 100644 --- a/src/point_cloud/gpu_point_cloud_converter.rs +++ b/src/point_cloud/gpu_point_cloud_converter.rs @@ -218,10 +218,10 @@ impl GpuPointCloudConverter { 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; - } + /*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) diff --git a/src/point_cloud/point_cloud_renderer.rs b/src/point_cloud/point_cloud_renderer.rs index 2232ade21e..7f68c02ed4 100644 --- a/src/point_cloud/point_cloud_renderer.rs +++ b/src/point_cloud/point_cloud_renderer.rs @@ -21,6 +21,8 @@ use point_cloud::gpu_point_cloud_converter::POINT_CLOUD_TEXTURE_FORMAT; use graphics_gl::{get_stride, get_pointer_offset}; use point_cloud::pixel_structs::BgraPixel; use point_cloud::point_cloud_visualiser::PointCloudVisualizer; +use conversion::k4a_image_to_rust_image_for_debug; +use std::path::Path; pub type Result = std::result::Result; @@ -371,6 +373,12 @@ impl PointCloudRenderer { pub fn update_point_clouds(&mut self, color_image: &k4a_sys_wrapper::Image, point_cloud_texture: &Texture) -> Result<()> { + + k4a_image_to_rust_image_for_debug(color_image) + .expect("depth_to_image should work") + .save(Path::new("update_point_cloud_color_image.png")) + .expect("should save"); + unsafe { gl::BindVertexArray(self.vertex_array_object.id());