diff --git a/src/graphics_imgui.rs b/src/graphics_imgui.rs index c35c72cefe..023ae3db47 100644 --- a/src/graphics_imgui.rs +++ b/src/graphics_imgui.rs @@ -13,10 +13,11 @@ use point_cloud::pixel_structs::BgraPixel; use point_cloud::point_cloud_visualiser::{ColorizationStrategy, PointCloudVisualizer, PointCloudVisualizerError}; use point_cloud::viewer_image::ViewerImage; use webcam::WebcamWriter; +use kinect::multi_device_capturer::MultiDeviceCaptureProvider; pub const ENABLE_WEBCAM: bool = false; -pub fn run(capture_provider: Arc, calibration_data: k4a_sys::k4a_calibration_t) { +pub fn run(capture_provider: Arc, calibration_data: k4a_sys::k4a_calibration_t) { let mut webcam_writer = None; @@ -197,19 +198,20 @@ pub fn run(capture_provider: Arc, calibration_data: k4a_sys::k4 window.gl_swap_window(); - if let Some(capture) = capture_provider.get_capture() { - visualizer.update_texture_id(texture.texture_id(), capture) - .map(|_| { - }) - .map_err(|err| { - match err { - PointCloudVisualizerError::MissingDepthImage => { println!("Missing depth image"); }, - PointCloudVisualizerError::MissingColorImage => { println!("Missing color image"); }, - _ => { - unreachable!("Error: {:?}", err); + if let Some(mut captures) = capture_provider.get_captures() { + if let Some(capture) = captures.pop() { + visualizer.update_texture_id(texture.texture_id(), capture) + .map(|_| {}) + .map_err(|err| { + match err { + PointCloudVisualizerError::MissingDepthImage => { println!("Missing depth image"); }, + PointCloudVisualizerError::MissingColorImage => { println!("Missing color image"); }, + _ => { + unreachable!("Error: {:?}", err); + } } - } - }); + }); + } } match sdl_arcball.lock() { diff --git a/src/kinect/multi_device_capturer.rs b/src/kinect/multi_device_capturer.rs index 53432a32de..b9ebdb81d3 100644 --- a/src/kinect/multi_device_capturer.rs +++ b/src/kinect/multi_device_capturer.rs @@ -1,12 +1,18 @@ - -use kinect::k4a_sys_wrapper::{Device, KinectError}; +use kinect::k4a_sys_wrapper::{Device, KinectError, Capture, CaptureError, GetCaptureError}; +use std::sync::Arc; +use std::sync::Mutex; // Allowing at least 160 microseconds between depth cameras should ensure they do not interfere with one another. const MIN_TIME_BETWEEN_DEPTH_CAMERA_PICTURES_USEC : libc::int32_t = 160; pub struct MultiDeviceCapturer { pub primary_device: Device, // TODO: Temporary public viz - secondary_devices: Vec + secondary_devices: Vec, + capture_provider: Arc, +} + +pub struct MultiDeviceCaptureProvider { + captures: Arc>>, // TODO: Shouldn't need to wrap in mutex if we wrap struct instead. } impl MultiDeviceCapturer { @@ -39,6 +45,7 @@ impl MultiDeviceCapturer { Ok(Self { primary_device: primary_device.unwrap(), secondary_devices, + capture_provider: Arc::new(MultiDeviceCaptureProvider::new()), }) } @@ -62,6 +69,72 @@ impl MultiDeviceCapturer { Ok(()) } + + pub fn get_synchronized_captures(&self) -> Result, GetCaptureError> { + let mut captures = Vec::with_capacity(1 + self.secondary_devices.len()); + + // TODO: -1 is K4A_WAIT_INDEFINITE + let capture = self.primary_device.get_capture(-1)?; + captures.push(capture); + + for device in self.secondary_devices.iter() { + let capture = device.get_capture(-1)?; + captures.push(capture); + } + + // TODO TODO TODO - Lots of sync logic goes here + // TODO TODO TODO - Lots of sync logic goes here + // TODO TODO TODO - Lots of sync logic goes here + // TODO TODO TODO - Lots of sync logic goes here + + Ok(captures) + } + + /// Get a capture provider for another thread + pub fn get_sync_capture_provider(&self) -> Arc { + self.capture_provider.clone() + } +} + +impl MultiDeviceCaptureProvider { + pub fn new() -> Self { + Self { + captures: Arc::new(Mutex::new(Vec::new())), + } + } + + /** + * Take the latest captures, if available, through interior mutability. + * This leaves the mutex holding an empty vec. + */ + pub fn get_captures(&self) -> Option> { + self.captures.lock() + .ok() + .map(|mut v| (*v).split_off(0)) + } + + /** + * Consume captures and replace whatever we currently hold. + */ + pub fn set_captures(&self, captures: Vec) { + match self.captures.lock() { + Ok(mut lock) => { + *lock = captures; + }, + Err(_) => {}, + } + } +} + +pub fn start_capture_thread(capturer: MultiDeviceCapturer) { + let mut capture_provider = capturer.get_sync_capture_provider(); + + loop { + let captures = capturer.get_synchronized_captures() + .expect("Should get captures"); + + capture_provider.set_captures(captures); + } } fn get_primary_device_config() -> k4a_sys::k4a_device_configuration_t { @@ -80,6 +153,7 @@ fn get_primary_device_config() -> k4a_sys::k4a_device_configuration_t { // They may need to be adjusted config.color_resolution = k4a_sys::k4a_color_resolution_t_K4A_COLOR_RESOLUTION_2160P; config.depth_mode = k4a_sys::k4a_depth_mode_t_K4A_DEPTH_MODE_NFOV_UNBINNED; + //config.camera_fps = k4a_sys::k4a_fps_t_K4A_FRAMES_PER_SECOND_30; config } @@ -99,6 +173,7 @@ fn get_secondary_device_config() -> k4a_sys::k4a_device_configuration_t { // They may need to be adjusted config.color_resolution = k4a_sys::k4a_color_resolution_t_K4A_COLOR_RESOLUTION_2160P; config.depth_mode = k4a_sys::k4a_depth_mode_t_K4A_DEPTH_MODE_NFOV_UNBINNED; + //config.camera_fps = k4a_sys::k4a_fps_t_K4A_FRAMES_PER_SECOND_30; config } diff --git a/src/main.rs b/src/main.rs index 40ace4803b..064574bb31 100644 --- a/src/main.rs +++ b/src/main.rs @@ -28,7 +28,7 @@ use opencv::prelude::*; use kinect::k4a_sys_wrapper::Device; use kinect::sensor_control::{capture_thread, CaptureProvider}; -use kinect::multi_device_capturer::MultiDeviceCapturer; +use kinect::multi_device_capturer::{MultiDeviceCapturer, start_capture_thread}; pub mod image_debug; pub mod core_types; @@ -50,17 +50,18 @@ pub fn main() { let multi_device = MultiDeviceCapturer::new(2).expect("multi-device create"); multi_device.start_cameras().expect("start cameras"); - let primary_device = multi_device.primary_device; - let depth_mode : k4a_sys::k4a_depth_mode_t = 2; //k4a_sys::K4A_DEPTH_MODE_NFOV_UNBINNED; let color_format: k4a_sys::k4a_color_resolution_t = k4a_sys::k4a_color_resolution_t_K4A_COLOR_RESOLUTION_2160P; + let primary_device = &multi_device.primary_device; let calibration = primary_device.get_calibration(depth_mode, color_format).unwrap(); - let capture_provider = Arc::new(CaptureProvider::new()); - let capture_provider2= capture_provider.clone(); + //let capture_provider = Arc::new(CaptureProvider::new()); + //let capture_provider2= capture_provider.clone(); + let capture_provider = multi_device.get_sync_capture_provider(); - thread::spawn(move || capture_thread(capture_provider, Some(primary_device), false)); + //thread::spawn(move || capture_thread(capture_provider, Some(primary_device), false)); + thread::spawn(move || start_capture_thread(multi_device)); - graphics_imgui::run(capture_provider2, calibration); + graphics_imgui::run(capture_provider, calibration); }