diff --git a/src/kinect/k4a_sys_wrapper.rs b/src/kinect/k4a_sys_wrapper.rs index 8b7f603dc6..f904e69fb9 100644 --- a/src/kinect/k4a_sys_wrapper.rs +++ b/src/kinect/k4a_sys_wrapper.rs @@ -3,10 +3,12 @@ //! but it's in a bad state and won't compile against any version of 'k4a-sys'. use std::mem::MaybeUninit; -use std::ptr; +use std::{ptr, fmt}; use std::ptr::null_mut; use k4a_sys; +use std::error::Error; +use std::fmt::Formatter; /*pub fn device_open() { @@ -251,6 +253,18 @@ pub enum CaptureError { NullCapture, } +impl fmt::Display for CaptureError { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + write!(f, "CaptureError") + } +} + +impl Error for CaptureError { + fn source(&self) -> Option<&(dyn Error + 'static)> { + None + } +} + /// Errors for GetCalibration #[derive(Debug)] pub enum GetCalibrationError { diff --git a/src/point_cloud/debug/camera_image_bytes.rs b/src/point_cloud/debug/camera_image_bytes.rs index 3f33c8d428..5f38574434 100644 --- a/src/point_cloud/debug/camera_image_bytes.rs +++ b/src/point_cloud/debug/camera_image_bytes.rs @@ -6,11 +6,13 @@ use kinect::k4a_sys_wrapper; /// Store either raw bytes, or wrap a k4a::Image /// This is meant to be plumbed through the system instead of a k4a::Image (depth or color image) +#[derive(Clone)] pub struct CameraImageBytes { storage: UnderlyingStorage, } /// The underlying data store +#[derive(Clone)] enum UnderlyingStorage { /// Our own serialization Bytes { @@ -51,6 +53,13 @@ impl CameraImageBytes { } } + pub fn consume_k4a_image(image: k4a_sys_wrapper::Image) -> Self { + let storage = UnderlyingStorage::K4aImage(image); + Self { + storage, + } + } + pub fn is_k4a(&self) -> bool { match &self.storage { UnderlyingStorage::Bytes { .. } => false, @@ -93,4 +102,11 @@ impl CameraImageBytes { UnderlyingStorage::K4aImage(image) => image.get_height_pixels(), } } + + pub fn get_handle(&self) -> k4a_sys::k4a_image_t { + match &self.storage { + UnderlyingStorage::Bytes { height, .. } => unimplemented!("no k4a_image_t"), + UnderlyingStorage::K4aImage(image) => image.get_handle(), + } + } } diff --git a/src/point_cloud/debug/capture_proxy.rs b/src/point_cloud/debug/capture_proxy.rs new file mode 100644 index 0000000000..4bd263e0ed --- /dev/null +++ b/src/point_cloud/debug/capture_proxy.rs @@ -0,0 +1,80 @@ +use anyhow::Result as AnyhowResult; +use kinect::k4a_sys_wrapper; +use point_cloud::debug::camera_image_bytes::CameraImageBytes; +use std::sync::{RwLock, Mutex, PoisonError, MutexGuard, Arc}; +use winit::event::VirtualKeyCode::Mute; + +/// Store either a wrapped capture, or a k4a Capture. +/// Useful for debugging. +#[derive(Clone)] +pub struct CaptureProxy { + storage: UnderlyingStorage, +} + +#[derive(Clone)] +enum UnderlyingStorage { + CameraImageBytes { + color_image: CameraImageBytes, + depth_image: CameraImageBytes, + }, + K4aCapture { + capture: k4a_sys_wrapper::Capture, + color_image: CameraImageBytes, + depth_image: CameraImageBytes, + } +} + +impl CaptureProxy { + + pub fn from_image_proxy_pair(color_image: CameraImageBytes, depth_image: CameraImageBytes) -> Self { + Self { + storage: UnderlyingStorage::CameraImageBytes { + color_image, + depth_image, + } + } + } + + pub fn from_k4a_image(capture: &k4a_sys_wrapper::Capture) -> AnyhowResult { + // NB: We need to increase the refcount. + // K4a manages the memory under the hood. + let capture = capture.clone(); + + let color_image = capture.get_color_image()?; + let depth_image = capture.get_color_image()?; + + // NB: I tried to lazily unpack these, but interior mutability Sync/Send was a nightmare. + // The poor ergonomics were not worth it. + let color_image_wrapper = CameraImageBytes::from_k4a_image(&color_image); + let depth_image_wrapper = CameraImageBytes::from_k4a_image(&depth_image); + + Ok(Self { + storage: UnderlyingStorage::K4aCapture { + capture, + color_image: color_image_wrapper, + depth_image: depth_image_wrapper, + } + }) + } + + pub fn is_k4a(&self) -> bool { + match &self.storage { + UnderlyingStorage::CameraImageBytes{ .. } => false, + UnderlyingStorage::K4aCapture { .. } => true, + } + } + + pub fn get_color_image(&self) -> AnyhowResult<&CameraImageBytes> { + match &self.storage { + UnderlyingStorage::CameraImageBytes { color_image, .. } => Ok(color_image), + UnderlyingStorage::K4aCapture{ color_image, .. } => Ok(color_image), + } + } + + pub fn get_depth_image(&self) -> AnyhowResult<&CameraImageBytes> { + match &self.storage { + UnderlyingStorage::CameraImageBytes { depth_image, .. } => Ok(depth_image), + UnderlyingStorage::K4aCapture{ depth_image, .. } => Ok(depth_image), + } + } +} diff --git a/src/point_cloud/debug/mod.rs b/src/point_cloud/debug/mod.rs index d8fabd8195..2ae688d69c 100644 --- a/src/point_cloud/debug/mod.rs +++ b/src/point_cloud/debug/mod.rs @@ -1 +1,2 @@ -pub mod camera_image_bytes; \ No newline at end of file +pub mod camera_image_bytes; +pub mod capture_proxy;