synchronized capture! (still only reading one image)

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<CaptureProvider>, calibration_data: k4a_sys::k4a_calibration_t) {
+pub fn run(capture_provider: Arc<MultiDeviceCaptureProvider>, calibration_data: k4a_sys::k4a_calibration_t) {
 
   let mut webcam_writer = None;
 
@@ -197,19 +198,20 @@ pub fn run(capture_provider: Arc<CaptureProvider>, 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() {

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<Device>
+  secondary_devices: Vec<Device>,
+  capture_provider: Arc<MultiDeviceCaptureProvider>,
+}
+
+pub struct MultiDeviceCaptureProvider {
+  captures: Arc<Mutex<Vec<Capture>>>, // 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<Vec<Capture>, 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<MultiDeviceCaptureProvider> {
+    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<Vec<Capture>> {
+    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<Capture>) {
+    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
 }

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);
 }