Merge pull request #236 from Microsoft/user/mbleyer/xyz-invalid-bug

Set x,y,z values to zero in point cloud function if pixel falls outside of lens radius

src/transformation/rgbz.c

@@ -104,7 +104,7 @@ static k4a_result_t transformation_compute_correspondence(const int depth_index,
                                                           const k4a_transformation_rgbz_context_t *context,
                                                           k4a_correspondence_t *correspondence)
 {
-    if (depth == 0)
+    if (depth == 0 || isnan(context->xy_tables->x_table[depth_index]))
     {
         memset(correspondence, 0, sizeof(k4a_correspondence_t));
         return K4A_RESULT_SUCCEEDED;
@@ -800,12 +800,24 @@ static void transformation_depth_to_xyz(k4a_transformation_xy_tables_t *xy_table
 {
     const uint16_t *depth_image_data_uint16 = (const uint16_t *)depth_image_data;
     int16_t *xyz_data_int16 = (int16_t *)xyz_image_data;
+    int16_t x, y, z;
 
     for (int i = 0; i < xy_tables->width * xy_tables->height; i++)
     {
-        int16_t z = (int16_t)depth_image_data_uint16[i];
-        int16_t x = (int16_t)(floorf(xy_tables->x_table[i] * (float)z + 0.5f));
-        int16_t y = (int16_t)(floorf(xy_tables->y_table[i] * (float)z + 0.5f));
+        float x_tab = xy_tables->x_table[i];
+
+        if (!isnan(x_tab))
+        {
+            z = (int16_t)depth_image_data_uint16[i];
+            x = (int16_t)(floorf(x_tab * (float)z + 0.5f));
+            y = (int16_t)(floorf(xy_tables->y_table[i] * (float)z + 0.5f));
+        }
+        else
+        {
+            x = 0;
+            y = 0;
+            z = 0;
+        }
 
         xyz_data_int16[3 * i + 0] = x;
         xyz_data_int16[3 * i + 1] = y;
@@ -838,16 +850,28 @@ static void transformation_depth_to_xyz_sse(k4a_transformation_xy_tables_t *xy_t
     // z2, z5, z0, z3, z6, z1, z4, z7
     __m128i z_shuffle = _mm_setr_epi16(pos2, pos5, pos0, pos3, pos6, pos1, pos4, pos7);
 
+    __m128i valid_shuffle = _mm_setr_epi16(pos0, pos2, pos4, pos6, pos0, pos2, pos4, pos6);
+
     for (int i = 0; i < xy_tables->width * xy_tables->height / 8; i++)
     {
         __m128i z = *depth_image_data_m128i++;
 
+        __m128 x_tab_lo = *x_table_m128++;
+        __m128 x_tab_hi = *x_table_m128++;
+        __m128 valid_lo = _mm_cmpeq_ps(x_tab_lo, x_tab_lo);
+        __m128 valid_hi = _mm_cmpeq_ps(x_tab_hi, x_tab_hi);
+        __m128i valid_shuffle_lo = _mm_shuffle_epi8(*((__m128i *)&valid_lo), valid_shuffle);
+        __m128i valid_shuffle_hi = _mm_shuffle_epi8(*((__m128i *)&valid_hi), valid_shuffle);
+        __m128i valid = _mm_blend_epi16(valid_shuffle_lo, valid_shuffle_hi, 0xF0);
+        z = _mm_blendv_epi8(_mm_setzero_si128(), z, valid);
+
         __m128 depth_lo = _mm_cvtepi32_ps(_mm_unpacklo_epi16(z, _mm_setzero_si128()));
         __m128 depth_hi = _mm_cvtepi32_ps(_mm_unpackhi_epi16(z, _mm_setzero_si128()));
 
-        __m128i x_lo = _mm_cvtps_epi32(_mm_mul_ps(depth_lo, *x_table_m128++));
-        __m128i x_hi = _mm_cvtps_epi32(_mm_mul_ps(depth_hi, *x_table_m128++));
+        __m128i x_lo = _mm_cvtps_epi32(_mm_mul_ps(depth_lo, x_tab_lo));
+        __m128i x_hi = _mm_cvtps_epi32(_mm_mul_ps(depth_hi, x_tab_hi));
         __m128i x = _mm_packs_epi32(x_lo, x_hi);
+        x = _mm_blendv_epi8(_mm_setzero_si128(), x, valid);
         x = _mm_shuffle_epi8(x, x_shuffle);
 
         __m128i y_lo = _mm_cvtps_epi32(_mm_mul_ps(depth_lo, *y_table_m128++));

src/transformation/transformation.c

@@ -7,6 +7,7 @@
 
 // System dependencies
 #include <stdlib.h>
+#include <math.h>
 
 k4a_result_t transformation_get_mode_specific_calibration(const k4a_calibration_camera_t *depth_camera_calibration,
                                                           const k4a_calibration_camera_t *color_camera_calibration,
@@ -316,7 +317,9 @@ static k4a_buffer_result_t transformation_init_xy_tables(const k4a_calibration_t
 
                 if (valid == 0)
                 {
-                    xy_tables->x_table[idx] = 0.f;
+                    // x table value of NAN marks invalid
+                    xy_tables->x_table[idx] = NAN;
+                    // set y table value to 0 to speed up SSE implementation
                     xy_tables->y_table[idx] = 0.f;
                 }
                 else

tests/Transformation/transformation.cpp

@@ -353,7 +353,7 @@ TEST_F(transformation_ut, transformation_depth_image_to_point_cloud)
 
     // Comparison against reference hash value computed over the entire image. If result image is changed (e.g., due to
     // using a different calibration), the reference value needs to be updated.
-    const double reference_val = 633.99727884928382;
+    const double reference_val = 562.20976003011071;
     if (std::abs(check_sum - reference_val) > 0.001)
     {
         ASSERT_EQ(check_sum, reference_val);