From c8d875c95d42c4ac64043d0df8cd1654993c3490 Mon Sep 17 00:00:00 2001 From: Brandon Thomas Date: Fri, 11 Sep 2020 11:56:19 -0400 Subject: [PATCH] per-device calibration handling (no longer distorted) We were only using the primary device's calibration --- src/graphics_imgui.rs | 4 +- src/kinect/capture/device_capturer.rs | 4 +- src/kinect/capture/fake_device_capturer.rs | 13 +++-- src/kinect/capture/multi_device_capturer.rs | 37 +++++++----- src/main.rs | 4 +- src/point_cloud/point_cloud_visualiser.rs | 62 ++++++++++++++------- 6 files changed, 81 insertions(+), 43 deletions(-) diff --git a/src/graphics_imgui.rs b/src/graphics_imgui.rs index c5dbc91e5c..0cd0cdf095 100644 --- a/src/graphics_imgui.rs +++ b/src/graphics_imgui.rs @@ -16,7 +16,7 @@ use std::path::Path; use std::sync::{Arc, Mutex, PoisonError, MutexGuard}; use std::time::{Instant, Duration}; -pub fn run(capture_provider: Arc, calibration_data: Calibration, program_args: ProgramArgs) -> AnyhowResult<()> { +pub fn run(capture_provider: Arc, program_args: ProgramArgs) -> AnyhowResult<()> { let mut webcam_writer = None; if program_args.enable_webcam_output_writing { @@ -63,9 +63,9 @@ pub fn run(capture_provider: Arc, calibration_data: Calibration let mut visualizer = PointCloudVisualizer::new( capture_provider.get_num_cameras(), + capture_provider.get_calibrations(), true, colorization_strategy, - calibration_data, clear_color, sdl_arcball.clone(), ); diff --git a/src/kinect/capture/device_capturer.rs b/src/kinect/capture/device_capturer.rs index 907e5189f2..49a804c267 100644 --- a/src/kinect/capture/device_capturer.rs +++ b/src/kinect/capture/device_capturer.rs @@ -12,6 +12,6 @@ pub trait CaptureProvider { */ fn get_captures(&self) -> Option>; - /** Get device calibration (from one of the cameras). */ - fn get_calibration(&self) -> &Calibration; + /** Get device calibrations for all cameras. */ + fn get_calibrations(&self) -> &Vec; } diff --git a/src/kinect/capture/fake_device_capturer.rs b/src/kinect/capture/fake_device_capturer.rs index ac6997075e..a8fbf1dfdc 100644 --- a/src/kinect/capture/fake_device_capturer.rs +++ b/src/kinect/capture/fake_device_capturer.rs @@ -7,7 +7,7 @@ use crate::point_cloud::debug::image_proxy::ImageProxy; pub struct FakeDeviceCaptureProvider { num_cameras: usize, captures: Vec, - calibration: Calibration, + all_device_calibrations: Vec, } impl FakeDeviceCaptureProvider { @@ -151,10 +151,15 @@ impl FakeDeviceCaptureProvider { ] ]; + let mut all_device_calibrations = Vec::new(); + for _ in 0 .. 2 { + all_device_calibrations.push(calibration.clone()); + } + Ok(Self { num_cameras: 2, captures, - calibration, + all_device_calibrations, }) } } @@ -171,7 +176,7 @@ impl CaptureProvider for FakeDeviceCaptureProvider { Some(capture_clones) } - fn get_calibration(&self) -> &Calibration { - &self.calibration + fn get_calibrations(&self) -> &Vec { + &self.all_device_calibrations } } diff --git a/src/kinect/capture/multi_device_capturer.rs b/src/kinect/capture/multi_device_capturer.rs index 6ece4cf88b..1c07a11c10 100644 --- a/src/kinect/capture/multi_device_capturer.rs +++ b/src/kinect/capture/multi_device_capturer.rs @@ -11,13 +11,15 @@ pub struct MultiDeviceCapturer { num_cameras: usize, pub primary_device: Device, // TODO: Temporary public viz secondary_devices: Vec, - calibration: Calibration, + /// Calibrations for primary + all secondaries in order. + all_device_calibrations: Vec, capture_provider: Arc, } pub struct MultiDeviceCaptureProvider { num_cameras: usize, - calibration: Calibration, + /// Calibrations for primary + all secondaries in order. + all_device_calibrations: Vec, captures: Arc>>, // TODO: Shouldn't need to wrap in mutex if we wrap struct instead. } @@ -28,6 +30,10 @@ impl MultiDeviceCapturer { pub fn new(num_devices: u32, limit_secondary_devices: Option) -> Result { let mut primary_device = None; let mut secondary_devices = Vec::new(); + let mut calibrations = Vec::new(); + + 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; for i in 0..num_devices { let mut device = Device::open(i)?; @@ -38,7 +44,12 @@ impl MultiDeviceCapturer { // NB: This assumes a daisy chain topology. panic!("We already one primary device. We can't have two."); } + + let calibration = device.get_calibration(depth_mode, color_format).unwrap(); + + calibrations.push(calibration); primary_device = Some(device); + continue; } @@ -46,7 +57,10 @@ impl MultiDeviceCapturer { panic!("Secondary device does not have 'in' jack connected!"); } + let calibration = device.get_calibration(depth_mode, color_format).unwrap(); + secondary_devices.push(device); + calibrations.push(calibration); } if primary_device.is_none() { @@ -59,19 +73,16 @@ impl MultiDeviceCapturer { let num_cameras = 1 + secondary_devices.len(); - 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 = primary_device.expect("There must be a primary device"); - let calibration = primary.get_calibration(depth_mode, color_format).unwrap(); - let calibration2 = calibration.clone(); + + let calibrations2 = calibrations.clone(); Ok(Self { num_cameras, primary_device: primary, secondary_devices, - calibration, - capture_provider: Arc::new(MultiDeviceCaptureProvider::new(num_cameras, calibration2)), + all_device_calibrations: calibrations, + capture_provider: Arc::new(MultiDeviceCaptureProvider::new(num_cameras, calibrations2)), }) } @@ -128,10 +139,10 @@ impl MultiDeviceCapturer { } impl MultiDeviceCaptureProvider { - pub fn new(num_cameras: usize, calibration: Calibration) -> Self { + pub fn new(num_cameras: usize, all_device_calibrations: Vec) -> Self { Self { num_cameras, - calibration, + all_device_calibrations, captures: Arc::new(Mutex::new(Vec::new())), } } @@ -169,8 +180,8 @@ impl CaptureProvider for MultiDeviceCaptureProvider { .collect()) } - fn get_calibration(&self) -> &Calibration { - &self.calibration + fn get_calibrations(&self) -> &Vec { + &self.all_device_calibrations } } diff --git a/src/main.rs b/src/main.rs index 609659dc02..04507379c1 100644 --- a/src/main.rs +++ b/src/main.rs @@ -127,10 +127,10 @@ pub fn main() -> AnyhowResult<()> { Arc::new(FakeDeviceCaptureProvider::new().unwrap()) }; - let calibration = capture_provider.get_calibration().clone(); + //let calibration = capture_provider.get_calibration().clone(); //calibration.debug_print(); - graphics_imgui::run(capture_provider, calibration, program_args)?; + graphics_imgui::run(capture_provider, program_args)?; Ok(()) } diff --git a/src/point_cloud/point_cloud_visualiser.rs b/src/point_cloud/point_cloud_visualiser.rs index ae94c465e2..2c800415cf 100644 --- a/src/point_cloud/point_cloud_visualiser.rs +++ b/src/point_cloud/point_cloud_visualiser.rs @@ -23,6 +23,7 @@ use crate::point_cloud::pixel_structs::DepthPixel; use crate::point_cloud::point_cloud_renderer::{PointCloudRenderer, PointCloudRendererError}; use crate::point_cloud::util::{colorize_depth_blue_to_red, get_depth_mode_range, ValueRange}; use crate::point_cloud::viewer_image::ViewerImage; +use crate::kinect::sensor_control::CaptureProvider; pub type Result = std::result::Result; @@ -108,8 +109,15 @@ pub struct PointCloudVisualizer { pub point_cloud_renderer: PointCloudRenderer, pub point_cloud_converters: Vec, - calibration_data: Calibration, - transformation: Transformation, // TODO: WAT k4a_sys::k4a_transformation_t + /// Calibration data for all cameras + all_device_calibrations: Vec, + + /// Transformations are constructed from camera calibrations + transformations: Vec, + + /// Near and far minima/maxima for the current depth sensor mode. + /// These are constructed for each camera from the camera calibrations. + expected_depth_value_ranges: Vec, frame_buffer: Framebuffer, depth_buffer: Renderbuffer, @@ -131,9 +139,6 @@ pub struct PointCloudVisualizer { color_xy_tables: Vec, depth_xy_tables: Vec, - /// Near and far minima/maxima for the current depth sensor mode. - depth_value_range: ValueRange, - /// Background clear color for OpenGL. clear_color: RgbaF32, @@ -160,9 +165,9 @@ impl PointCloudVisualizer { /// CTOR /// pub fn new(num_cameras: usize, + all_device_calibrations: &Vec, enable_color_point_cloud: bool, initial_colorization_strategy: ColorizationStrategy, - calibration_data: Calibration, clear_color: RgbaF32, arcball_camera: Arc>) -> Self { @@ -178,8 +183,17 @@ impl PointCloudVisualizer { gl::RenderbufferStorage(gl::RENDERBUFFER, gl::DEPTH_COMPONENT, width, height); } - let expected_value_range = get_depth_mode_range(calibration_data.0.depth_mode) - .expect("Should be in correct depth sensor mode."); + let mut expected_depth_value_ranges = Vec::new(); + let mut transformations = Vec::new(); + + for calibration in all_device_calibrations { + let value_range = get_depth_mode_range(calibration.0.depth_mode) + .expect("Should be in correct depth sensor mode."); + expected_depth_value_ranges.push(value_range); + + let transformation = Transformation::from_calibration(&calibration); + transformations.push(transformation); + } let mut point_cloud_converters = Vec::with_capacity(num_cameras); let mut transformed_depth_images = Vec::with_capacity(num_cameras); @@ -189,21 +203,23 @@ impl PointCloudVisualizer { let mut color_xy_tables = Vec::with_capacity(num_cameras); let mut depth_xy_tables = Vec::with_capacity(num_cameras); - for _ in 0 .. num_cameras { + for i in 0 .. num_cameras { point_cloud_converters.push(GpuPointCloudConverter::new()); transformed_depth_images.push(None); point_cloud_colorizations.push(None); last_captures.push(None); xyz_textures.push(Texture::new()); + let calibration = all_device_calibrations.get(i).unwrap(); + // TODO: generate color xytable only on `enable_color_point_cloud`. let color_xy_table = GpuPointCloudConverter::generate_xy_table( - &calibration_data, + &calibration, k4a_sys::k4a_calibration_type_t_K4A_CALIBRATION_TYPE_COLOR, ).unwrap(); let depth_xy_table = GpuPointCloudConverter::generate_xy_table( - &calibration_data, + &calibration, k4a_sys::k4a_calibration_type_t_K4A_CALIBRATION_TYPE_DEPTH, ).unwrap(); @@ -232,16 +248,16 @@ impl PointCloudVisualizer { color_xy_tables, depth_xy_tables, colorization_strategy: initial_colorization_strategy, - calibration_data: calibration_data.clone(), - transformation: Transformation::from_calibration(&calibration_data), + transformations, last_captures, transformed_depth_images, point_cloud_colorizations, xyz_textures, - depth_value_range: expected_value_range, + expected_depth_value_ranges, clear_color, debug_static_color_frames, debug_static_depth_frames, + all_device_calibrations: all_device_calibrations.clone(), }; visualizer.set_colorization_strategy(initial_colorization_strategy).expect("Should work"); @@ -379,6 +395,8 @@ impl PointCloudVisualizer { // Depth Image extractor // + let transformation = self.transformations.get(camera_index).unwrap(); + let mut depth_image = match capture.get_depth_image() { Ok(img) => img, Err(_e) => { @@ -401,7 +419,7 @@ impl PointCloudVisualizer { unsafe { let result = k4a_sys::k4a_transformation_depth_image_to_color_camera( - self.transformation.get_handle(), + transformation.get_handle(), depth_image.get_handle(), transformed_depth_image.get_handle(), ); @@ -449,6 +467,8 @@ impl PointCloudVisualizer { // This creates a color spectrum based on depth. let dst_length = use_depth_image.get_width_pixels() * use_depth_image.get_height_pixels(); + let depth_value_range = self.expected_depth_value_ranges.get(camera_index).unwrap(); + unsafe { // src: DepthPixel let src_pixel_buffer = use_depth_image.get_buffer(); @@ -466,7 +486,7 @@ impl PointCloudVisualizer { for i in 0 .. dst_length as isize { let src_pixel = *typed_src_pixel_buffer.offset(i); - let pixel = colorize_depth_blue_to_red(src_pixel, &self.depth_value_range); + let pixel = colorize_depth_blue_to_red(src_pixel, depth_value_range); (*typed_dst_pixel_buffer.offset(i)).red = pixel.red; (*typed_dst_pixel_buffer.offset(i)).green = pixel.green; (*typed_dst_pixel_buffer.offset(i)).blue = pixel.blue; @@ -493,12 +513,14 @@ impl PointCloudVisualizer { return Err(PointCloudVisualizerError::UnsupportedMode); } + let calibration = self.all_device_calibrations.get(camera_index).unwrap(); + self.colorization_strategy = strategy; self.point_cloud_renderer.set_enable_shading(strategy == ColorizationStrategy::Shaded); if strategy == ColorizationStrategy::Color { - let width = self.calibration_data.0.color_camera_calibration.resolution_width; - let height = self.calibration_data.0.color_camera_calibration.resolution_height; + let width = calibration.0.color_camera_calibration.resolution_width; + let height = calibration.0.color_camera_calibration.resolution_height; let stride = width * size_of::() as i32; // TODO: TEMP MULTI-CAMERA SUPPORT @@ -516,8 +538,8 @@ impl PointCloudVisualizer { } } else { - let width = self.calibration_data.0.depth_camera_calibration.resolution_width as u32; - let height = self.calibration_data.0.depth_camera_calibration.resolution_height as u32; + let width = calibration.0.depth_camera_calibration.resolution_width as u32; + let height = calibration.0.depth_camera_calibration.resolution_height as u32; let stride = width as i32 * size_of::() as i32; // TODO: TEMP MULTI-CAMERA SUPPORT