From 7d19979b1d9c4e52c2e541ce100ee584bd52fa1a Mon Sep 17 00:00:00 2001 From: Brandon Thomas Date: Thu, 16 Apr 2020 05:08:20 -0400 Subject: [PATCH] k4a image constructor --- src/k4a_sys_wrapper.rs | 65 +++++++++++++++++------ src/shaders/point_cloud_shader.rs | 88 ++++++++++++++++++++++++++++--- 2 files changed, 130 insertions(+), 23 deletions(-) diff --git a/src/k4a_sys_wrapper.rs b/src/k4a_sys_wrapper.rs index 3785c587f2..84fa1d1755 100644 --- a/src/k4a_sys_wrapper.rs +++ b/src/k4a_sys_wrapper.rs @@ -8,6 +8,7 @@ use std::ptr; use k4a_sys; use k4a_sys_wrapper::ImageFormat::ColorMjpg; use glutin::platform::unix::x11::ffi::IconMaskHint; +use std::ptr::null_mut; pub fn device_get_installed_count() -> u32 { unsafe { @@ -32,6 +33,7 @@ pub enum KinectError { UnableToOpen { error_code: u32 }, UnableToGetSerialNumber, UnableToStartCameras { error_code: u32 }, + UnableToCreateImage { error_code: u32 }, } /// A Kinect Device Handle @@ -234,6 +236,33 @@ unsafe impl Send for Capture{} pub struct Image(pub k4a_sys::k4a_image_t); impl Image { + + /// Create a blank image. + pub fn create(format: ImageFormat, + width: u32, + height: u32, + stride_bytes: u32) + -> Result + { + let mut handle = null_mut(); + + let result = unsafe { + k4a_sys::k4a_image_create( + format as k4a_sys::k4a_image_format_t, + width as i32, + height as i32, + stride_bytes as i32, + &mut handle + ) + }; + + if result != 0 /* k4a_sys::K4A_RESULT_SUCCEEDED */ { + return Err(KinectError::UnableToCreateImage { error_code: result }); + } + + Ok(Image(handle)) + } + pub fn get_height_pixels(&self) -> usize { unsafe { k4a_sys::k4a_image_get_height_pixels(self.0) as usize @@ -269,21 +298,10 @@ impl Image { /// known format. If the image_handle is invalid, the function will return /// K4A_IMAGE_FORMAT_CUSTOM. pub fn get_format(&self) -> ImageFormat { - unsafe { - let format = k4a_sys::k4a_image_get_format(self.0); - match format { - 0 => ImageFormat::ColorMjpg, - 1 => ImageFormat::ColorNv12, - 2 => ImageFormat::ColorYuy2, - 3 => ImageFormat::ColorBgra32, - 4 => ImageFormat::Depth16, - 5 => ImageFormat::Ir16, - 6 => ImageFormat::Custom8, - 7 => ImageFormat::Custom16, - 8 => ImageFormat::Custom, - _ => ImageFormat::UnknownFormatError, - } - } + let format = unsafe { + k4a_sys::k4a_image_get_format(self.0) + }; + format.into() } } @@ -306,6 +324,23 @@ pub enum ImageFormat { UnknownFormatError, // FIXME: Just return Result? } +impl From for ImageFormat { + fn from(format: k4a_sys::k4a_image_format_t) -> Self { + match format { + 0 => ImageFormat::ColorMjpg, + 1 => ImageFormat::ColorNv12, + 2 => ImageFormat::ColorYuy2, + 3 => ImageFormat::ColorBgra32, + 4 => ImageFormat::Depth16, + 5 => ImageFormat::Ir16, + 6 => ImageFormat::Custom8, + 7 => ImageFormat::Custom16, + 8 => ImageFormat::Custom, + _ => ImageFormat::UnknownFormatError, + } + } +} + /// Deallocate open device handles impl Drop for Device { fn drop(&mut self) { diff --git a/src/shaders/point_cloud_shader.rs b/src/shaders/point_cloud_shader.rs index e6ce88c466..357607bc06 100644 --- a/src/shaders/point_cloud_shader.rs +++ b/src/shaders/point_cloud_shader.rs @@ -2,21 +2,22 @@ //! This code turns depth images into point clouds. use std::ffi::CString; +use std::fmt::{Error, Formatter}; +use std::mem::size_of; use std::os::raw::c_void; use std::ptr; +use std::ptr::null; use std::str; -use libc; use gl; use gl::types::*; +use libc; use k4a_sys_wrapper; -use std::mem::size_of; -use std::ptr::null; -use std::fmt::{Formatter, Error}; - -use opengl_wrapper::Texture; +use k4a_sys_wrapper::Image; +use k4a_sys_wrapper::ImageFormat; use opengl_wrapper::Buffer; +use opengl_wrapper::Texture; pub type Result = std::result::Result; @@ -351,10 +352,81 @@ impl PointCloudComputeShader { // p is the index of a given pixel. // pub fn generate_xy_table(calibration: k4a_sys::k4a_calibration_t, - calibration_type: k4a_sys::k4a_calibration_type_t) -> Result { + calibration_type: k4a_sys::k4a_calibration_type_t) -> Result + { + + /* + typedef enum + { + K4A_CALIBRATION_TYPE_UNKNOWN = -1, /**< Calibration type is unknown */ + K4A_CALIBRATION_TYPE_DEPTH, /**< Depth sensor */ + K4A_CALIBRATION_TYPE_COLOR, /**< Color sensor */ + K4A_CALIBRATION_TYPE_GYRO, /**< Gyroscope sensor */ + K4A_CALIBRATION_TYPE_ACCEL, /**< Accelerometer sensor */ + K4A_CALIBRATION_TYPE_NUM, /**< Number of types excluding unknown type*/ + } k4a_calibration_type_t; + */ + + let camera_calibration : k4a_sys::k4a_calibration_camera_t = match calibration_type { + // FIXME: k4a_sys::K4A_CALIBRATION_TYPE_COLOR should be "1" per above enum. + 1 => calibration.color_camera_calibration, + _ => calibration.depth_camera_calibration, + }; + + let width = camera_calibration.resolution_width as u32; + let height = camera_calibration.resolution_height as u32; + + let stride_bytes = width * size_of::() as u32; + + // TODO: continue impl + let xy_table = Image::create( + ImageFormat::Custom, + width, + height, + stride_bytes, + ); - // TODO: Just a static function unimplemented!(); + /* + + k4a::image xyTable = k4a::image::create(K4A_IMAGE_FORMAT_CUSTOM, + cameraCalibration.resolution_width, + cameraCalibration.resolution_height, + cameraCalibration.resolution_width * + static_cast(sizeof(k4a_float2_t))); + + k4a_float2_t *tableData = reinterpret_cast(xyTable.get_buffer()); + + int width = cameraCalibration.resolution_width; + int height = cameraCalibration.resolution_height; + + k4a_float2_t p; + k4a_float3_t ray; + + for (int y = 0, idx = 0; y < height; y++) + { + p.xy.y = static_cast(y); + for (int x = 0; x < width; x++, idx++) + { + p.xy.x = static_cast(x); + + if (calibration.convert_2d_to_3d(p, 1.f, calibrationType, calibrationType, &ray)) + { + tableData[idx].xy.x = ray.xyz.x; + tableData[idx].xy.y = ray.xyz.y; + } + else + { + // The pixel is invalid. + // + tableData[idx].xy.x = 0.0f; + tableData[idx].xy.y = 0.0f; + } + } + } + + return xyTable; + */ } }