From 8457a951e67e0325dfe482a25673ea13dfcdb2d6 Mon Sep 17 00:00:00 2001 From: Brandon Thomas Date: Fri, 19 Mar 2021 03:20:39 -0400 Subject: [PATCH] wip; adding new calculations.. --- src/calibration/color_downscale_info.rs | 78 +++++++++++++++++++++++++ src/calibration/mod.rs | 3 +- src/setup/camera_info.rs | 2 + src/setup/open_devices.rs | 65 +-------------------- 4 files changed, 85 insertions(+), 63 deletions(-) create mode 100644 src/calibration/color_downscale_info.rs diff --git a/src/calibration/color_downscale_info.rs b/src/calibration/color_downscale_info.rs new file mode 100644 index 0000000..dcda626 --- /dev/null +++ b/src/calibration/color_downscale_info.rs @@ -0,0 +1,78 @@ +use kinect::{Calibration, Transformation}; + +pub struct ColorDownscaleInfo { + pub color_image_downscaled_height: i32, + pub color_image_downscaled_width: i32, + pub transformation_color_downscaled: Transformation, +} + +impl ColorDownscaleInfo { + /// From LiveScan3D: + /// It's crucial for this program to output accurately mapped Pointclouds. The highest accuracy mapping is achieved + /// by using the k4a_transformation_depth_image_to_color_camera function. However this converts a small depth image + /// to a larger size, equivalent to the the color image size. This means more points to process and higher processing costs + /// We can however scale the color image to the depth images size beforehand, to reduce proccesing power. + pub fn get_calibration_color_downscaled(calibration: &Calibration) -> Self { + //No way to get the depth pixel values from the SDK at the moment, so this is hardcoded + let mut depth_camera_width = 0; + let mut depth_camera_height = 0; + + /*switch (config.depth_mode) + { + case K4A_DEPTH_MODE_NFOV_UNBINNED: + depth_camera_width = 640; + depth_camera_height = 576; + break; + case K4A_DEPTH_MODE_NFOV_2X2BINNED: + depth_camera_width = 320; + depth_camera_height = 288; + break; + case K4A_DEPTH_MODE_WFOV_UNBINNED: + depth_camera_width = 1024; + depth_camera_height = 1024; + case K4A_DEPTH_MODE_WFOV_2X2BINNED: + depth_camera_width = 512; + depth_camera_height = 512; + break; + default: + break; + }*/ + + // TODO(bt): Hardcoding for my use case. + // NFOV_UNBINNED + depth_camera_width = 640; + depth_camera_height = 576; + + + // We calculate the minimum size that the color Image can be, while preserving its aspect ration + let rescale_ratio: f32 = calibration.0.color_camera_calibration.resolution_height as f32 / depth_camera_height as f32; + + let color_image_downscaled_height = depth_camera_height; + let color_image_downscaled_width = (calibration.0.color_camera_calibration.resolution_width as f32 / rescale_ratio) as i32; + + // We don't only need the size in pixels of the downscaled color image, but also a new k4a_calibration_t which fits the new + // sizes + let mut calibration_color_downscaled = calibration.clone(); + + unsafe { + calibration_color_downscaled.0.color_camera_calibration.resolution_width = + (calibration_color_downscaled.0.color_camera_calibration.resolution_width as f32 / rescale_ratio) as i32; + + calibration_color_downscaled.0.color_camera_calibration.resolution_height = + (calibration_color_downscaled.0.color_camera_calibration.resolution_height as f32 / rescale_ratio) as i32; + + calibration_color_downscaled.0.color_camera_calibration.intrinsics.parameters.param.cx /= rescale_ratio; + calibration_color_downscaled.0.color_camera_calibration.intrinsics.parameters.param.cy /= rescale_ratio; + calibration_color_downscaled.0.color_camera_calibration.intrinsics.parameters.param.fx /= rescale_ratio; + calibration_color_downscaled.0.color_camera_calibration.intrinsics.parameters.param.fy /= rescale_ratio; + } + + let transformation_color_downscaled = Transformation::from_calibration(&calibration_color_downscaled); + + Self { + color_image_downscaled_height, + color_image_downscaled_width, + transformation_color_downscaled, + } + } +} diff --git a/src/calibration/mod.rs b/src/calibration/mod.rs index f6c83d8..3a1ef41 100644 --- a/src/calibration/mod.rs +++ b/src/calibration/mod.rs @@ -1 +1,2 @@ -pub mod world_calibration; \ No newline at end of file +pub mod world_calibration; +pub mod color_downscale_info; diff --git a/src/setup/camera_info.rs b/src/setup/camera_info.rs index a1ca45c..c913dfd 100644 --- a/src/setup/camera_info.rs +++ b/src/setup/camera_info.rs @@ -1,6 +1,7 @@ use kinect::{Device, Calibration, Image, DeviceConfiguration}; use crate::pointcloud::cpu_transformation::DepthTransformer; use crate::calibration::world_calibration::WorldCalibration; +use crate::calibration::color_downscale_info::ColorDownscaleInfo; /// Extra metadata about a camera. pub struct CameraInfo { @@ -9,4 +10,5 @@ pub struct CameraInfo { pub xy_table: Image, pub transformer: DepthTransformer, pub world_calibration: WorldCalibration, + pub color_downscale_info: ColorDownscaleInfo, } diff --git a/src/setup/open_devices.rs b/src/setup/open_devices.rs index 02e3b83..457c332 100644 --- a/src/setup/open_devices.rs +++ b/src/setup/open_devices.rs @@ -6,6 +6,7 @@ use crate::pointcloud::cpu_transformation::DepthTransformer; use crate::AnyhowResult; use k4a_sys_temp as k4a_sys; use crate::calibration::world_calibration::WorldCalibration; +use crate::calibration::color_downscale_info::ColorDownscaleInfo; pub fn open_and_configure_camera(args: &CommandArgs, index: u32) -> AnyhowResult<(Device, CameraInfo)> { @@ -41,6 +42,7 @@ pub fn open_and_configure_camera(args: &CommandArgs, index: u32) -> AnyhowResult println!("Loading world calibration..."); let world_calibration = WorldCalibration::load_from_file_and_device(&device)?; + let color_downscale_info = ColorDownscaleInfo::get_calibration_color_downscaled(&calibration); /* */ @@ -52,69 +54,8 @@ pub fn open_and_configure_camera(args: &CommandArgs, index: u32) -> AnyhowResult xy_table, transformer, world_calibration, + color_downscale_info, }; Ok((device, camera_info)) } - -/// From LiveScan3D: -/// It's crucial for this program to output accurately mapped Pointclouds. The highest accuracy mapping is achieved -/// by using the k4a_transformation_depth_image_to_color_camera function. However this converts a small depth image -/// to a larger size, equivalent to the the color image size. This means more points to process and higher processing costs -/// We can however scale the color image to the depth images size beforehand, to reduce proccesing power. -fn get_calibration_color_downscaled(calibration: &Calibration) { - //No way to get the depth pixel values from the SDK at the moment, so this is hardcoded - let mut depth_camera_width = 0; - let mut depth_camera_height= 0; - - /*switch (config.depth_mode) - { - case K4A_DEPTH_MODE_NFOV_UNBINNED: - depth_camera_width = 640; - depth_camera_height = 576; - break; - case K4A_DEPTH_MODE_NFOV_2X2BINNED: - depth_camera_width = 320; - depth_camera_height = 288; - break; - case K4A_DEPTH_MODE_WFOV_UNBINNED: - depth_camera_width = 1024; - depth_camera_height = 1024; - case K4A_DEPTH_MODE_WFOV_2X2BINNED: - depth_camera_width = 512; - depth_camera_height = 512; - break; - default: - break; - }*/ - - // NFOV_UNBINNED - depth_camera_width = 640; - depth_camera_height = 576; - - - // We calculate the minimum size that the color Image can be, while preserving its aspect ration - let rescale_ratio : f32 = calibration.0.color_camera_calibration.resolution_height as f32 / depth_camera_height as f32; - - let color_image_downscaled_height = depth_camera_height; - let color_image_downscaled_width = (calibration.0.color_camera_calibration.resolution_width as f32 / rescale_ratio) as i32; - - // We don't only need the size in pixels of the downscaled color image, but also a new k4a_calibration_t which fits the new - // sizes - let mut calibration_color_downscaled = calibration.clone(); - - unsafe { - calibration_color_downscaled.0.color_camera_calibration.resolution_width = - (calibration_color_downscaled.0.color_camera_calibration.resolution_width as f32 / rescale_ratio) as i32; - - calibration_color_downscaled.0.color_camera_calibration.resolution_height = - (calibration_color_downscaled.0.color_camera_calibration.resolution_height as f32 / rescale_ratio) as i32; - - calibration_color_downscaled.0.color_camera_calibration.intrinsics.parameters.param.cx /= rescale_ratio; - calibration_color_downscaled.0.color_camera_calibration.intrinsics.parameters.param.cy /= rescale_ratio; - calibration_color_downscaled.0.color_camera_calibration.intrinsics.parameters.param.fx /= rescale_ratio; - calibration_color_downscaled.0.color_camera_calibration.intrinsics.parameters.param.fy /= rescale_ratio; - } - - let transformation_color_downscaled = Transformation::from_calibration(&calibration_color_downscaled); -}