add dimension check for point cloud calculation

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

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