k4a image constructor

src/k4a_sys_wrapper.rs

@@ -8,6 +8,7 @@ use std::ptr;
 use k4a_sys;
 use k4a_sys_wrapper::ImageFormat::ColorMjpg;
 use glutin::platform::unix::x11::ffi::IconMaskHint;
+use std::ptr::null_mut;
 
 pub fn device_get_installed_count() -> u32 {
   unsafe {
@@ -32,6 +33,7 @@ pub enum KinectError {
   UnableToOpen { error_code: u32 },
   UnableToGetSerialNumber,
   UnableToStartCameras { error_code: u32 },
+  UnableToCreateImage { error_code: u32 },
 }
 
 /// A Kinect Device Handle
@@ -234,6 +236,33 @@ unsafe impl Send for Capture{}
 pub struct Image(pub k4a_sys::k4a_image_t);
 
 impl Image {
+
+  /// Create a blank image.
+  pub fn create(format: ImageFormat,
+                width: u32,
+                height: u32,
+                stride_bytes: u32)
+    -> Result<Self, KinectError>
+  {
+    let mut handle = null_mut();
+
+    let result = unsafe {
+      k4a_sys::k4a_image_create(
+        format as k4a_sys::k4a_image_format_t,
+        width as i32,
+        height as i32,
+        stride_bytes as i32,
+        &mut handle
+      )
+    };
+
+    if result != 0 /* k4a_sys::K4A_RESULT_SUCCEEDED */ {
+      return Err(KinectError::UnableToCreateImage { error_code: result });
+    }
+
+    Ok(Image(handle))
+  }
+
   pub fn get_height_pixels(&self) -> usize {
     unsafe {
       k4a_sys::k4a_image_get_height_pixels(self.0) as usize
@@ -269,21 +298,10 @@ impl Image {
   /// known format. If the image_handle is invalid, the function will return
   /// K4A_IMAGE_FORMAT_CUSTOM.
   pub fn get_format(&self) -> ImageFormat {
-    unsafe {
-      let format = k4a_sys::k4a_image_get_format(self.0);
-      match format {
-        0 => ImageFormat::ColorMjpg,
-        1 => ImageFormat::ColorNv12,
-        2 => ImageFormat::ColorYuy2,
-        3 => ImageFormat::ColorBgra32,
-        4 => ImageFormat::Depth16,
-        5 => ImageFormat::Ir16,
-        6 => ImageFormat::Custom8,
-        7 => ImageFormat::Custom16,
-        8 => ImageFormat::Custom,
-        _ => ImageFormat::UnknownFormatError,
-      }
-    }
+    let format = unsafe {
+      k4a_sys::k4a_image_get_format(self.0)
+    };
+    format.into()
   }
 }
 
@@ -306,6 +324,23 @@ pub enum ImageFormat {
   UnknownFormatError, // FIXME: Just return Result<T>?
 }
 
+impl From<k4a_sys::k4a_image_format_t> for ImageFormat {
+  fn from(format: k4a_sys::k4a_image_format_t) -> Self {
+    match format {
+      0 => ImageFormat::ColorMjpg,
+      1 => ImageFormat::ColorNv12,
+      2 => ImageFormat::ColorYuy2,
+      3 => ImageFormat::ColorBgra32,
+      4 => ImageFormat::Depth16,
+      5 => ImageFormat::Ir16,
+      6 => ImageFormat::Custom8,
+      7 => ImageFormat::Custom16,
+      8 => ImageFormat::Custom,
+      _ => ImageFormat::UnknownFormatError,
+    }
+  }
+}
+
 /// Deallocate open device handles
 impl Drop for Device {
   fn drop(&mut self) {

src/shaders/point_cloud_shader.rs

@@ -2,21 +2,22 @@
 //! This code turns depth images into point clouds.
 
 use std::ffi::CString;
+use std::fmt::{Error, Formatter};
+use std::mem::size_of;
 use std::os::raw::c_void;
 use std::ptr;
+use std::ptr::null;
 use std::str;
-use libc;
 
 use gl;
 use gl::types::*;
+use libc;
 
 use k4a_sys_wrapper;
-use std::mem::size_of;
-use std::ptr::null;
-use std::fmt::{Formatter, Error};
-
-use opengl_wrapper::Texture;
+use k4a_sys_wrapper::Image;
+use k4a_sys_wrapper::ImageFormat;
 use opengl_wrapper::Buffer;
+use opengl_wrapper::Texture;
 
 pub type Result<T> = std::result::Result<T,PointCloudError>;
 
@@ -351,10 +352,81 @@ impl PointCloudComputeShader {
   // p is the index of a given pixel.
   //
   pub fn generate_xy_table(calibration: k4a_sys::k4a_calibration_t,
-                           calibration_type: k4a_sys::k4a_calibration_type_t) -> Result<k4a_sys_wrapper::Image> {
+                           calibration_type: k4a_sys::k4a_calibration_type_t) -> Result<k4a_sys_wrapper::Image>
+  {
+
+    /*
+    typedef enum
+    {
+        K4A_CALIBRATION_TYPE_UNKNOWN = -1, /**< Calibration type is unknown */
+        K4A_CALIBRATION_TYPE_DEPTH,        /**< Depth sensor */
+        K4A_CALIBRATION_TYPE_COLOR,        /**< Color sensor */
+        K4A_CALIBRATION_TYPE_GYRO,         /**< Gyroscope sensor */
+        K4A_CALIBRATION_TYPE_ACCEL,        /**< Accelerometer sensor */
+        K4A_CALIBRATION_TYPE_NUM,          /**< Number of types excluding unknown type*/
+    } k4a_calibration_type_t;
+    */
+
+    let camera_calibration :  k4a_sys::k4a_calibration_camera_t = match calibration_type {
+      // FIXME: k4a_sys::K4A_CALIBRATION_TYPE_COLOR  should be "1" per above enum.
+      1 => calibration.color_camera_calibration,
+      _ => calibration.depth_camera_calibration,
+    };
+
+    let width = camera_calibration.resolution_width as u32;
+    let height = camera_calibration.resolution_height as u32;
+
+    let stride_bytes = width * size_of::<k4a_sys::k4a_float2_t>() as u32;
+
+    // TODO: continue impl
+    let xy_table = Image::create(
+      ImageFormat::Custom,
+      width,
+      height,
+      stride_bytes,
+    );
 
-    // TODO: Just a static function
     unimplemented!();
+    /*
+
+    k4a::image xyTable = k4a::image::create(K4A_IMAGE_FORMAT_CUSTOM,
+                                            cameraCalibration.resolution_width,
+                                            cameraCalibration.resolution_height,
+                                            cameraCalibration.resolution_width *
+                                                static_cast<int>(sizeof(k4a_float2_t)));
+
+    k4a_float2_t *tableData = reinterpret_cast<k4a_float2_t *>(xyTable.get_buffer());
+
+    int width = cameraCalibration.resolution_width;
+    int height = cameraCalibration.resolution_height;
+
+    k4a_float2_t p;
+    k4a_float3_t ray;
+
+    for (int y = 0, idx = 0; y < height; y++)
+    {
+        p.xy.y = static_cast<float>(y);
+        for (int x = 0; x < width; x++, idx++)
+        {
+            p.xy.x = static_cast<float>(x);
+
+            if (calibration.convert_2d_to_3d(p, 1.f, calibrationType, calibrationType, &ray))
+            {
+                tableData[idx].xy.x = ray.xyz.x;
+                tableData[idx].xy.y = ray.xyz.y;
+            }
+            else
+            {
+                // The pixel is invalid.
+                //
+                tableData[idx].xy.x = 0.0f;
+                tableData[idx].xy.y = 0.0f;
+            }
+        }
+    }
+
+    return xyTable;
+    */
   }
 }