From b5b85892e9e06f16ad71b1acc9e470e50a4c6f8e Mon Sep 17 00:00:00 2001 From: Michael Bleyer Date: Thu, 11 Apr 2019 10:55:52 -0700 Subject: [PATCH 1/4] set x,y,z values to zero in point cloud function if pixel falls outside of lens radius --- src/transformation/rgbz.c | 36 ++++++++++++++++++++----- src/transformation/transformation.c | 5 +++- tests/Transformation/transformation.cpp | 2 +- 3 files changed, 35 insertions(+), 8 deletions(-) diff --git a/src/transformation/rgbz.c b/src/transformation/rgbz.c index e1c7693e..4d378b1d 100644 --- a/src/transformation/rgbz.c +++ b/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 || context->xy_tables->x_table[depth_index] == NAN) { 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 (x_tab == x_tab) // check for NAN + { + 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++)); diff --git a/src/transformation/transformation.c b/src/transformation/transformation.c index a738f49c..dd1595ee 100644 --- a/src/transformation/transformation.c +++ b/src/transformation/transformation.c @@ -7,6 +7,7 @@ // System dependencies #include +#include 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 diff --git a/tests/Transformation/transformation.cpp b/tests/Transformation/transformation.cpp index cb2e3785..3c485aaa 100644 --- a/tests/Transformation/transformation.cpp +++ b/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); From f15ac8fe1ef64067a7c1ef20631af62c8d2da293 Mon Sep 17 00:00:00 2001 From: Michael Bleyer Date: Mon, 15 Apr 2019 12:00:31 -0700 Subject: [PATCH 2/4] check for not equal instead of check against NaN --- src/transformation/rgbz.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/transformation/rgbz.c b/src/transformation/rgbz.c index 4d378b1d..b4df33e1 100644 --- a/src/transformation/rgbz.c +++ b/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 || context->xy_tables->x_table[depth_index] == NAN) + if (depth == 0 || (context->xy_tables->x_table[depth_index] != context->xy_tables->x_table[depth_index])) { memset(correspondence, 0, sizeof(k4a_correspondence_t)); return K4A_RESULT_SUCCEEDED; From 8437b8758bd91035cd47ea9d8b006dc3e62f1f77 Mon Sep 17 00:00:00 2001 From: Michael Bleyer Date: Mon, 15 Apr 2019 13:24:38 -0700 Subject: [PATCH 3/4] undoing previous commit due to Linux compilation issue --- src/transformation/rgbz.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/transformation/rgbz.c b/src/transformation/rgbz.c index b4df33e1..4d378b1d 100644 --- a/src/transformation/rgbz.c +++ b/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 || (context->xy_tables->x_table[depth_index] != context->xy_tables->x_table[depth_index])) + if (depth == 0 || context->xy_tables->x_table[depth_index] == NAN) { memset(correspondence, 0, sizeof(k4a_correspondence_t)); return K4A_RESULT_SUCCEEDED; From f5a65863a97e22fe2b44dea19b8babd8370afc99 Mon Sep 17 00:00:00 2001 From: Michael Bleyer Date: Mon, 15 Apr 2019 14:56:06 -0700 Subject: [PATCH 4/4] use isnan() --- src/transformation/rgbz.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/transformation/rgbz.c b/src/transformation/rgbz.c index 4d378b1d..35e7b68b 100644 --- a/src/transformation/rgbz.c +++ b/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 || context->xy_tables->x_table[depth_index] == NAN) + if (depth == 0 || isnan(context->xy_tables->x_table[depth_index])) { memset(correspondence, 0, sizeof(k4a_correspondence_t)); return K4A_RESULT_SUCCEEDED; @@ -806,7 +806,7 @@ static void transformation_depth_to_xyz(k4a_transformation_xy_tables_t *xy_table { float x_tab = xy_tables->x_table[i]; - if (x_tab == x_tab) // check for NAN + if (!isnan(x_tab)) { z = (int16_t)depth_image_data_uint16[i]; x = (int16_t)(floorf(x_tab * (float)z + 0.5f));