diff --git a/src/kinect/capture/fake_device_capturer.rs b/src/kinect/capture/fake_device_capturer.rs index 013f8dc5f1..a5f6ccdc21 100644 --- a/src/kinect/capture/fake_device_capturer.rs +++ b/src/kinect/capture/fake_device_capturer.rs @@ -2,22 +2,23 @@ use anyhow::Result as AnyhowResult; use kinect::capture::device_capturer::CaptureProvider; use point_cloud::debug::capture_proxy::CaptureProxy; use point_cloud::debug::image_proxy::ImageProxy; -use kinect::k4a_sys_wrapper::ImageFormat; +use kinect::k4a_sys_wrapper::{ImageFormat, Calibration}; pub struct FakeDeviceCaptureProvider { num_cameras: usize, captures: Vec, + calibration: Calibration, } impl FakeDeviceCaptureProvider { pub fn new() -> AnyhowResult { - // TODO: Color dimensions are wrong - let color = ImageProxy::from_file("output/color_frame_0", 0, 0, 0, ImageFormat::ColorBgra32)?; + // TODO: Color dimension stride is wrong (and maybe others) + let color = ImageProxy::from_file("output/color_frame_0", 3840, 2160, 0, ImageFormat::ColorBgra32)?; let depth = ImageProxy::from_file("output/depth_frame_0", 640, 576, 1280, ImageFormat::Depth16)?; let frame_1 = CaptureProxy::from_image_proxy_pair(color, depth); - // TODO: Color dimensions are wrong - let color = ImageProxy::from_file("output/color_frame_1", 0, 0, 0, ImageFormat::ColorBgra32)?; + // TODO: Color dimension stride is wrong (and maybe others) + let color = ImageProxy::from_file("output/color_frame_1", 3840, 2160, 0, ImageFormat::ColorBgra32)?; let depth = ImageProxy::from_file("output/depth_frame_1", 640, 576, 1280, ImageFormat::Depth16)?; let frame_2 = CaptureProxy::from_image_proxy_pair(color, depth); @@ -25,13 +26,144 @@ impl FakeDeviceCaptureProvider { captures.push(frame_1); captures.push(frame_2); + let mut calibration = Calibration::default(); + + // NB: Experimentally determined "constants" from real devices + calibration.0.color_resolution = 5; + calibration.0.depth_mode = 2; + calibration.0.depth_camera_calibration.resolution_width = 640; + calibration.0.depth_camera_calibration.resolution_height = 576; + calibration.0.depth_camera_calibration.metric_radius = 1.7399998; + calibration.0.depth_camera_calibration.extrinsics.rotation = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]; + calibration.0.depth_camera_calibration.extrinsics.translation = [0.0, 0.0, 0.0]; + calibration.0.depth_camera_calibration.intrinsics.type_ = 4; + calibration.0.depth_camera_calibration.intrinsics.parameter_count = 14; + calibration.0.depth_camera_calibration.intrinsics.parameters.param.cx = 325.4652; + calibration.0.depth_camera_calibration.intrinsics.parameters.param.cy = 339.75952; + calibration.0.depth_camera_calibration.intrinsics.parameters.param.fx = 503.72073; + calibration.0.depth_camera_calibration.intrinsics.parameters.param.fy = 503.82776; + calibration.0.depth_camera_calibration.intrinsics.parameters.param.k1 = 5.7966313; + calibration.0.depth_camera_calibration.intrinsics.parameters.param.k2 = 4.1091304; + calibration.0.depth_camera_calibration.intrinsics.parameters.param.k3 = 0.22066948; + calibration.0.depth_camera_calibration.intrinsics.parameters.param.k4 = 6.1226273; + calibration.0.depth_camera_calibration.intrinsics.parameters.param.k5 = 6.053665; + calibration.0.depth_camera_calibration.intrinsics.parameters.param.k6 = 1.1403816; + calibration.0.depth_camera_calibration.intrinsics.parameters.param.codx = 0.0; + calibration.0.depth_camera_calibration.intrinsics.parameters.param.cody = 0.0; + calibration.0.depth_camera_calibration.intrinsics.parameters.param.p2 = -0.00003910405; + calibration.0.depth_camera_calibration.intrinsics.parameters.param.p1 = -0.00009726079; + calibration.0.depth_camera_calibration.intrinsics.parameters.param.metric_radius = 0.0; + + calibration.0.color_camera_calibration.resolution_width = 3840; + calibration.0.color_camera_calibration.resolution_height = 2160; + calibration.0.color_camera_calibration.metric_radius = 1.7; + calibration.0.color_camera_calibration.extrinsics.rotation = [0.99998903, -0.0045318827, 0.001187322, 0.0044161947, 0.9964592, 0.08396203, -0.001563624, -0.08395587, 0.99646825]; + calibration.0.color_camera_calibration.extrinsics.translation = [-32.02683, -2.3786376, 3.9904723]; + calibration.0.color_camera_calibration.intrinsics.type_ = 4; + calibration.0.color_camera_calibration.intrinsics.parameter_count = 14; + calibration.0.color_camera_calibration.intrinsics.parameters.param.cx = 1919.9363; + calibration.0.color_camera_calibration.intrinsics.parameters.param.cy = 1105.1877; + calibration.0.color_camera_calibration.intrinsics.parameters.param.fx = 1832.5886; + calibration.0.color_camera_calibration.intrinsics.parameters.param.fy = 1832.6873; + calibration.0.color_camera_calibration.intrinsics.parameters.param.k1 = 0.4812507; + calibration.0.color_camera_calibration.intrinsics.parameters.param.k2 = -2.5299993; + calibration.0.color_camera_calibration.intrinsics.parameters.param.k3 = 1.4068391; + calibration.0.color_camera_calibration.intrinsics.parameters.param.k4 = 0.36012942; + calibration.0.color_camera_calibration.intrinsics.parameters.param.k5 = -2.355542; + calibration.0.color_camera_calibration.intrinsics.parameters.param.k6 = 1.3372931; + calibration.0.color_camera_calibration.intrinsics.parameters.param.codx = 0.0; + calibration.0.color_camera_calibration.intrinsics.parameters.param.cody = 0.0; + calibration.0.color_camera_calibration.intrinsics.parameters.param.p2 = -0.00037526494; + calibration.0.color_camera_calibration.intrinsics.parameters.param.p1 = 0.00054938474; + calibration.0.color_camera_calibration.intrinsics.parameters.param.metric_radius = 0.0; + calibration.0.extrinsics = [ + [ + k4a_sys::_k4a_calibration_extrinsics_t { + rotation: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], + translation: [0.0, 0.0, 0.0] + }, + k4a_sys::_k4a_calibration_extrinsics_t { + rotation: [0.99998903, -0.0045318827, 0.001187322, 0.0044161947, 0.9964592, 0.08396203, -0.001563624, -0.08395587, 0.99646825], + translation: [-32.02683, -2.3786376, 3.9904723] + }, + k4a_sys::_k4a_calibration_extrinsics_t { + rotation: [0.00908543, 0.1157082, -0.99324167, -0.9999575, -0.0005106917, -0.009206355, -0.001572491, 0.9932831, 0.11569864], + translation: [0.0, 0.0, 0.0] + }, + k4a_sys::_k4a_calibration_extrinsics_t { + rotation: [0.004534903, 0.112131886, -0.993683, -0.9999893, -0.00040909834, -0.004609848, -0.00092342496, 0.99369323, 0.11212883], + translation: [-51.077, 3.2191863, 1.8031791] + } + ], + [ + k4a_sys::_k4a_calibration_extrinsics_t { + rotation: [0.99998903, 0.0044161947, -0.001563624, -0.0045318827, 0.9964592, -0.08395587, 0.001187322, 0.08396203, 0.99646825], + translation: [32.043224, 2.560097, -3.7386374] + }, + k4a_sys::_k4a_calibration_extrinsics_t { + rotation: [1.0, 0.00000000026193447, 0.0, 0.00000000026193447, 1.0, -0.000000007450581, 0.0, -0.000000007450581, 1.0], + translation: [0.0000038146973, 0.0, 0.0] + }, + k4a_sys::_k4a_calibration_extrinsics_t { + rotation: [0.007381657, 0.031944036, -0.99946237, -0.9999551, -0.005697875, -0.007567407, -0.005936545, 0.9994734, 0.03190054], + translation: [4.300721, -32.00875, 2.0599582] + }, + k4a_sys::_k4a_calibration_extrinsics_t { + rotation: [0.0028468627, 0.028323233, -0.99959475, -0.99998194, -0.005210849, -0.0029956135, -0.005293583, 0.9995853, 0.028307885], + translation: [-46.929596, -28.807508, 3.8983316] + } + ], + [ + k4a_sys::_k4a_calibration_extrinsics_t { + rotation: [0.00908543, -0.9999575, -0.001572491, 0.1157082, -0.0005106917, 0.9932831, -0.99324167, -0.009206355, 0.11569864], + translation: [0.0, 0.0, 0.0] + }, + k4a_sys::_k4a_calibration_extrinsics_t { + rotation: [0.007381657, -0.9999551, -0.005936545, 0.031944036, -0.005697875, 0.9994734, -0.99946237, -0.007567407, 0.03190054], + translation: [-32.02683, -2.3786376, 3.9904723] + }, + k4a_sys::_k4a_calibration_extrinsics_t { + rotation: [0.99999994, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.99999994], + translation: [0.0, 0.0, 0.0] + }, + k4a_sys::_k4a_calibration_extrinsics_t { + rotation: [0.99998313, 0.0045562233, -0.003596194, -0.0045539755, 0.99998945, 0.00063277053, 0.0035990402, -0.0006163829, 0.99999326], + translation: [-51.077, 3.2191863, 1.8031791] + } + ], + [ + k4a_sys::_k4a_calibration_extrinsics_t { + rotation: [0.004534903, -0.9999893, -0.00092342496, 0.112131886, -0.00040909834, 0.99369323, -0.993683, -0.004609848, 0.11212883], + translation: [3.452446, 3.9368703, -50.941692] + }, + k4a_sys::_k4a_calibration_extrinsics_t { + rotation: [0.0028468627, -0.99998194, -0.005293583, 0.028323233, -0.005210849, 0.9995853, -0.99959475, -0.0029956135, 0.028307885], + translation: [-28.652746, -2.717628, -47.10723] + }, + k4a_sys::_k4a_calibration_extrinsics_t { + rotation: [0.99998313, -0.0045539755, 0.0035990402, 0.0045562233, 0.99998945, -0.0006163829, -0.003596194, 0.00063277053, 0.99999326], + translation: [51.08431, -2.9853227, -1.9888868] + }, + k4a_sys::_k4a_calibration_extrinsics_t { + rotation: [0.99999994, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.99999994], + translation: [-0.0000038146973, 0.00000023841858, 0.00000011920929] } + ] + ]; + Ok(Self { num_cameras: 2, captures, + calibration, }) } } +impl FakeDeviceCaptureProvider { + pub fn get_calibration(&self) -> &Calibration { + &self.calibration + } +} + impl CaptureProvider for FakeDeviceCaptureProvider { fn get_num_cameras(&self) -> usize { self.num_cameras diff --git a/src/kinect/k4a_sys_wrapper.rs b/src/kinect/k4a_sys_wrapper.rs index a09e367dd5..f3cba60241 100644 --- a/src/kinect/k4a_sys_wrapper.rs +++ b/src/kinect/k4a_sys_wrapper.rs @@ -281,7 +281,6 @@ pub struct Calibration(pub k4a_sys::k4a_calibration_t); impl Calibration { pub fn default() -> Self { - // TODO: k4a_sys::k4a_calibration_type_t_K4A_CALIBRATION_TYPE_UNKNOWN let extrinsics = k4a_sys::_k4a_calibration_extrinsics_t { rotation: [0.0f32, 0.0f32, 0.0f32, 0.0f32, 0.0f32, 0.0f32, 0.0f32, 0.0f32, 0.0f32], translation: [0.0f32, 0.0f32, 0.0f32], @@ -338,8 +337,66 @@ impl Calibration { depth_mode: 0, extrinsics: extrinsics_4_4, }; + Self(calibration) } + + pub fn debug_print(&self) { + println!("===== CALIBRATION ====="); + println!("\t Color resolution: {}", self.0.color_resolution); + println!("\t Depth mode: {}", self.0.depth_mode); + println!("\t Extrinsics: {:?}", self.0.extrinsics); + + println!("\t depth.resolution_width: {}", self.0.depth_camera_calibration.resolution_width); + println!("\t depth.resolution_height: {}", self.0.depth_camera_calibration.resolution_height); + println!("\t depth.metric_radius: {}", self.0.depth_camera_calibration.metric_radius); + println!("\t depth.extrinsics: {:?}", self.0.depth_camera_calibration.extrinsics); + println!("\t depth.intrinsics.type: {}", self.0.depth_camera_calibration.intrinsics.type_); + println!("\t depth.intrinsics.parameter_count: {}", self.0.depth_camera_calibration.intrinsics.parameter_count); + unsafe { + // NB: This is a union field, so we have to use unsafe access + println!("\t depth.intrinsics.parameters.param.cx: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.cx); + println!("\t depth.intrinsics.parameters.param.cy: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.cy); + println!("\t depth.intrinsics.parameters.param.fx: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.fx); + println!("\t depth.intrinsics.parameters.param.fy: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.fy); + println!("\t depth.intrinsics.parameters.param.k1: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.k1); + println!("\t depth.intrinsics.parameters.param.k2: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.k2); + println!("\t depth.intrinsics.parameters.param.k3: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.k3); + println!("\t depth.intrinsics.parameters.param.k4: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.k4); + println!("\t depth.intrinsics.parameters.param.k5: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.k5); + println!("\t depth.intrinsics.parameters.param.k6: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.k6); + println!("\t depth.intrinsics.parameters.param.codx: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.codx); + println!("\t depth.intrinsics.parameters.param.cody: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.cody); + println!("\t depth.intrinsics.parameters.param.p2: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.p2); + println!("\t depth.intrinsics.parameters.param.p1: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.p1); + println!("\t depth.intrinsics.parameters.param.metric_radius: {}", self.0.depth_camera_calibration.intrinsics.parameters.param.metric_radius); + } + println!("\t color.resolution_width: {}", self.0.color_camera_calibration.resolution_width); + println!("\t color.resolution_height: {}", self.0.color_camera_calibration.resolution_height); + println!("\t color.metric_radius: {}", self.0.color_camera_calibration.metric_radius); + println!("\t color.extrinsics: {:?}", self.0.color_camera_calibration.extrinsics); + println!("\t color.intrinsics.type: {}", self.0.color_camera_calibration.intrinsics.type_); + println!("\t color.intrinsics.parameter_count: {}", self.0.color_camera_calibration.intrinsics.parameter_count); + unsafe { + // NB: This is a union field, so we have to use unsafe access + println!("\t color.intrinsics.parameters.param.cx: {}", self.0.color_camera_calibration.intrinsics.parameters.param.cx); + println!("\t color.intrinsics.parameters.param.cy: {}", self.0.color_camera_calibration.intrinsics.parameters.param.cy); + println!("\t color.intrinsics.parameters.param.fx: {}", self.0.color_camera_calibration.intrinsics.parameters.param.fx); + println!("\t color.intrinsics.parameters.param.fy: {}", self.0.color_camera_calibration.intrinsics.parameters.param.fy); + println!("\t color.intrinsics.parameters.param.k1: {}", self.0.color_camera_calibration.intrinsics.parameters.param.k1); + println!("\t color.intrinsics.parameters.param.k2: {}", self.0.color_camera_calibration.intrinsics.parameters.param.k2); + println!("\t color.intrinsics.parameters.param.k3: {}", self.0.color_camera_calibration.intrinsics.parameters.param.k3); + println!("\t color.intrinsics.parameters.param.k4: {}", self.0.color_camera_calibration.intrinsics.parameters.param.k4); + println!("\t color.intrinsics.parameters.param.k5: {}", self.0.color_camera_calibration.intrinsics.parameters.param.k5); + println!("\t color.intrinsics.parameters.param.k6: {}", self.0.color_camera_calibration.intrinsics.parameters.param.k6); + println!("\t color.intrinsics.parameters.param.codx: {}", self.0.color_camera_calibration.intrinsics.parameters.param.codx); + println!("\t color.intrinsics.parameters.param.cody: {}", self.0.color_camera_calibration.intrinsics.parameters.param.cody); + println!("\t color.intrinsics.parameters.param.p2: {}", self.0.color_camera_calibration.intrinsics.parameters.param.p2); + println!("\t color.intrinsics.parameters.param.p1: {}", self.0.color_camera_calibration.intrinsics.parameters.param.p1); + println!("\t color.intrinsics.parameters.param.metric_radius: {}", self.0.color_camera_calibration.intrinsics.parameters.param.metric_radius); + } + println!("=========="); + } } /// Adapted from k4a-sys. Represents a capture. diff --git a/src/main.rs b/src/main.rs index 82bb0c3c09..bd148fe8e7 100644 --- a/src/main.rs +++ b/src/main.rs @@ -27,7 +27,7 @@ use std::thread; use opencv::prelude::*; -use kinect::k4a_sys_wrapper::Device; +use kinect::k4a_sys_wrapper::{Device, Calibration}; use kinect::sensor_control::{capture_thread, CaptureProvider}; use kinect::capture::multi_device_capturer::{MultiDeviceCapturer, start_capture_thread}; use kinect::capture::fake_device_capturer::FakeDeviceCaptureProvider; @@ -63,11 +63,15 @@ pub fn main() { let primary_device = &multi_device.primary_device; let calibration = primary_device.get_calibration(depth_mode, color_format).unwrap(); - let capture_provider = multi_device.get_sync_capture_provider(); + calibration.debug_print(); + + //let capture_provider = multi_device.get_sync_capture_provider(); thread::spawn(move || start_capture_thread(multi_device)); - //let capture_provider = Arc::new(FakeDeviceCaptureProvider::new().unwrap()); + //let calibration = Calibration::default(); + let capture_provider = Arc::new(FakeDeviceCaptureProvider::new().unwrap()); + let calibration = capture_provider.get_calibration().clone(); graphics_imgui::run(capture_provider, calibration, ENABLE_WEBCAM); }