From e5d33744bfcaceb7e938b0313dd2595e184ee85d Mon Sep 17 00:00:00 2001 From: Brandon Thomas Date: Wed, 3 Feb 2021 02:00:34 -0500 Subject: [PATCH] add dimension check for point cloud calculation --- src/main_zeromq.rs | 8 ++++---- src/zeromq/cpu_calculate_point_cloud.rs | 12 +++++++++++- 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/src/main_zeromq.rs b/src/main_zeromq.rs index 8a67996eba..372a51f9f0 100644 --- a/src/main_zeromq.rs +++ b/src/main_zeromq.rs @@ -54,14 +54,14 @@ fn main() -> AnyhowResult<()> { // NB: Similar to NFOV_UNBINNED, but less dense (faster). // Not used by any program I'm aware of. - config.0.depth_mode = k4a_sys::k4a_depth_mode_t_K4A_DEPTH_MODE_NFOV_2X2BINNED; + config.0.depth_mode = k4a_sys::k4a_depth_mode_t_K4A_DEPTH_MODE_NFOV_2X2BINNED; // 320x228 // NB: WFOV_2X2BINNED was used by the original 'cloudcam_zeromq'. // It's less dense, and wider. Much more performant. Fast on CPU. //config.0.depth_mode = k4a_sys::k4a_depth_mode_t_K4A_DEPTH_MODE_WFOV_2X2BINNED; config.0.color_format = k4a_sys::k4a_image_format_t_K4A_IMAGE_FORMAT_COLOR_BGRA32; - config.0.color_resolution = k4a_sys::k4a_color_resolution_t_K4A_COLOR_RESOLUTION_720P; + config.0.color_resolution = k4a_sys::k4a_color_resolution_t_K4A_COLOR_RESOLUTION_720P; // 1280x721 //config.0.color_resolution = k4a_sys::k4a_color_resolution_t_K4A_COLOR_RESOLUTION_2160P; // 4K, what the original program did let calibration = device.get_calibration(config.0.depth_mode, config.0.color_resolution)?; @@ -164,10 +164,10 @@ fn get_point_cloud(device: &Device, xy_table: &Image, color: Color, transformer: let depth_image = capture.get_depth_image() .ok_or(anyhow!("capture not present"))?; - //let depth_image2 = transformer.transform(&depth_image)?; + let depth_image2 = transformer.transform(&depth_image)?; let mut points = - calculate_point_cloud2(&depth_image, &xy_table, color)?; + calculate_point_cloud2(&depth_image2, &xy_table, color)?; //println!("Points: {}", points.len()); diff --git a/src/zeromq/cpu_calculate_point_cloud.rs b/src/zeromq/cpu_calculate_point_cloud.rs index ea31272a0c..e51526bdd0 100644 --- a/src/zeromq/cpu_calculate_point_cloud.rs +++ b/src/zeromq/cpu_calculate_point_cloud.rs @@ -1,11 +1,12 @@ +use anyhow::anyhow; use cgmath::num_traits::Float; use crate::AnyhowResult; +use crate::zeromq::color::Color; use crate::zeromq::point::Point; use k4a_sys_temp as k4a_sys; use kinect::Image; use kinect::ImageFormat; use std::mem::size_of; -use crate::zeromq::color::Color; pub struct PointCloudResult { pub point_cloud_image: Image, @@ -60,6 +61,15 @@ pub fn calculate_point_cloud2(depth_image: &Image, xy_table_image: &Image, color let width = depth_image.get_width_pixels(); let height = depth_image.get_height_pixels(); + { + let xy_width = xy_table_image.get_width_pixels(); + let xy_height = xy_table_image.get_height_pixels(); + if width != xy_width || height != xy_height { + return Err(anyhow!("Depth image ({}x{}) and XY table ({}x{}) dimensions are not equal!", + width, height, xy_width, xy_height)); + } + } + let xy_table_data = xy_table_image.get_buffer() as *mut k4a_sys::k4a_float2_t; let depth_data = depth_image.get_buffer() as *mut u16; // uint16_t