Merge remote-tracking branch 'origin/develop' into feature/csharp

src/transformation/rgbz.c

@@ -245,7 +245,32 @@ static bool transformation_check_valid_correspondences(const k4a_correspondence_
     }
 
     // If two or more vertices are invalid then we can't create a valid triangle
-    return num_invalid < 2;
+    bool valid = num_invalid < 2;
+
+    // Ignore interpolation at large depth discontinuity without disrupting slanted surface
+    // Skip interpolation threshold is estimated based on the following logic:
+    // - angle between two pixels is: theta = 0.234375 degree (120 degree / 512) in binning resolution mode
+    // - distance between two pixels at same depth approximately is: A ~= sin(theta) * depth
+    // - distance between two pixels at highly slanted surface (e.g. alpha = 85 degree) is: B = A / cos(alpha)
+    // - skip_interpolation_ratio ~= sin(theta) / cos(alpha)
+    // We use B as the threshold that to skip interpolation if the depth difference in the triangle is larger
+    // than B. This is a conservative threshold to estimate largest distance on a highly slanted surface at given depth,
+    // in reality, given distortion, distance, resolution difference, B can be smaller
+    const float skip_interpolation_ratio = 0.04693441759f;
+    float d1 = valid_top_left->depth;
+    float d2 = valid_top_right->depth;
+    float d3 = valid_bottom_right->depth;
+    float d4 = valid_bottom_left->depth;
+    float depth_min = transformation_min2f(transformation_min2f(d1, d2), transformation_min2f(d3, d4));
+    float depth_max = transformation_max2f(transformation_max2f(d1, d2), transformation_max2f(d3, d4));
+    float depth_delta = depth_max - depth_min;
+    float skip_interpolation_threshold = skip_interpolation_ratio * depth_min;
+    if (depth_delta > skip_interpolation_threshold)
+    {
+        valid = false;
+    }
+
+    return valid;
 }
 
 static inline float transformation_area_function(const k4a_float2_t *a, const k4a_float2_t *b, const k4a_float2_t *c)

tests/FirmwareTests/CMakeLists.txt

@@ -14,6 +14,7 @@ target_link_libraries(firmware_fw PRIVATE
     k4ainternal::conn_ex_utility
     k4ainternal::depth_mcu
     k4ainternal::color_mcu
+    k4ainternal::calibration
     k4ainternal::firmware
     k4ainternal::logging
     k4ainternal::usb_cmd)

tests/FirmwareTests/firmware_fw.cpp

@@ -6,6 +6,7 @@
 
 #include <k4ainternal/logging.h>
 #include <k4ainternal/usbcommand.h>
+#include <k4ainternal/calibration.h>
 
 #include <azure_c_shared_utility/tickcounter.h>
 #include <azure_c_shared_utility/threadapi.h>
@@ -59,17 +60,155 @@ class firmware_fw : public ::testing::Test
             serial_number = nullptr;
             serial_number_length = 0;
         }
+
+        if (calibration_pre_update != nullptr)
+        {
+            free(calibration_pre_update);
+            calibration_pre_update = nullptr;
+            calibration_pre_update_size = 0;
+        }
+
+        if (calibration_post_update != nullptr)
+        {
+            free(calibration_post_update);
+            calibration_post_update = nullptr;
+            calibration_post_update_size = 0;
+        }
+    }
+
+    k4a_result_t connect_device()
+    {
+        k4a_result_t result = g_connection_exerciser->set_usb_port(g_k4a_port_number);
+
+        if (K4A_SUCCEEDED(result))
+        {
+            int retry = 0;
+            uint32_t device_count = 0;
+
+            while (device_count == 0 && retry++ < 20)
+            {
+                ThreadAPI_Sleep(500);
+                usb_cmd_get_device_count(&device_count);
+            }
+
+            if (device_count == 0 || retry >= 20)
+            {
+                result = K4A_RESULT_FAILED;
+            }
+        }
+
+        return result;
+    }
+
+    k4a_result_t disconnect_device()
+    {
+        if (firmware_handle != nullptr)
+        {
+            firmware_destroy(firmware_handle);
+            firmware_handle = nullptr;
+        }
+
+        k4a_result_t result = g_connection_exerciser->set_usb_port(0);
+        if (K4A_SUCCEEDED(result))
+        {
+            ThreadAPI_Sleep(1000);
+        }
+
+        return result;
+    }
+
+    bool compare_calibration()
+    {
+        if (calibration_pre_update_size == calibration_post_update_size &&
+            0 == memcmp(calibration_pre_update, calibration_post_update, calibration_pre_update_size))
+        {
+            return true;
+        }
+        else
+        {
+            printf("Calibration pre and post update do not match!\n");
+            printf("Calibration pre-update: %s\n", (char *)calibration_pre_update);
+            printf("Calibration post-update: %s\n", (char *)calibration_post_update);
+        }
+
+        return false;
     }
 
     firmware_t firmware_handle = nullptr;
     char *serial_number = nullptr;
     size_t serial_number_length = 0;
+
+    uint8_t *calibration_pre_update = nullptr;
+    size_t calibration_pre_update_size = 0;
+
+    uint8_t *calibration_post_update = nullptr;
+    size_t calibration_post_update_size = 0;
 };
 
+k4a_result_t read_calibration(uint8_t **calibration_data, size_t *calibration_size)
+{
+    RETURN_VALUE_IF_ARG(K4A_RESULT_FAILED, calibration_data == NULL);
+    RETURN_VALUE_IF_ARG(K4A_RESULT_FAILED, calibration_size == NULL);
+
+    if (*calibration_data != nullptr)
+    {
+        free(*calibration_data);
+        *calibration_data = nullptr;
+        *calibration_size = 0;
+    }
+
+    k4a_result_t result;
+    depthmcu_t depth_handle = nullptr;
+    calibration_t calibration_handle = nullptr;
+
+    result = TRACE_CALL(depthmcu_create(K4A_DEVICE_DEFAULT, &depth_handle));
+
+    if (K4A_SUCCEEDED(result))
+    {
+        result = TRACE_CALL(calibration_create(depth_handle, &calibration_handle));
+    }
+
+    if (K4A_SUCCEEDED(result))
+    {
+        if (K4A_BUFFER_RESULT_TOO_SMALL ==
+            TRACE_BUFFER_CALL(calibration_get_raw_data(calibration_handle, nullptr, calibration_size)))
+        {
+            *calibration_data = (uint8_t *)malloc(*calibration_size);
+        }
+        else
+        {
+            result = K4A_RESULT_FAILED;
+        }
+    }
+
+    if (K4A_SUCCEEDED(result))
+    {
+        if (K4A_FAILED(
+                TRACE_BUFFER_CALL(calibration_get_raw_data(calibration_handle, *calibration_data, calibration_size))))
+        {
+            result = K4A_RESULT_FAILED;
+        }
+    }
+
+    if (calibration_handle != nullptr)
+    {
+        calibration_destroy(calibration_handle);
+        calibration_handle = nullptr;
+    }
+
+    if (depth_handle != nullptr)
+    {
+        depthmcu_destroy(depth_handle);
+        depth_handle = nullptr;
+    }
+
+    return result;
+}
+
 TEST_F(firmware_fw, DISABLED_update_timing)
 {
     LOG_INFO("Beginning the manual test to get update timings.", 0);
-    ASSERT_EQ(K4A_RESULT_SUCCEEDED, g_connection_exerciser->set_usb_port(g_k4a_port_number));
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, connect_device());
 
     ASSERT_EQ(K4A_RESULT_SUCCEEDED, open_firmware_device(&firmware_handle));
 
@@ -105,7 +244,9 @@ TEST_F(firmware_fw, DISABLED_update_timing)
 TEST_F(firmware_fw, simple_update_from_lkg)
 {
     LOG_INFO("Beginning the basic update test from the LKG firmware.", 0);
-    ASSERT_EQ(K4A_RESULT_SUCCEEDED, g_connection_exerciser->set_usb_port(g_k4a_port_number));
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, connect_device());
+
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, read_calibration(&calibration_pre_update, &calibration_pre_update_size));
 
     ASSERT_EQ(K4A_RESULT_SUCCEEDED, open_firmware_device(&firmware_handle));
 
@@ -125,6 +266,13 @@ TEST_F(firmware_fw, simple_update_from_lkg)
                                     g_lkg_firmware_package_info,
                                     false));
 
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, disconnect_device());
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, connect_device());
+
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, read_calibration(&calibration_post_update, &calibration_post_update_size));
+    ASSERT_TRUE(compare_calibration());
+
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, open_firmware_device(&firmware_handle));
     ASSERT_TRUE(compare_device_serial_number(firmware_handle, serial_number));
 
     LOG_INFO("Updating the device to the Candidate firmware.");
@@ -135,6 +283,13 @@ TEST_F(firmware_fw, simple_update_from_lkg)
                                     g_candidate_firmware_package_info,
                                     false));
 
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, disconnect_device());
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, connect_device());
+
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, read_calibration(&calibration_post_update, &calibration_post_update_size));
+    ASSERT_TRUE(compare_calibration());
+
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, open_firmware_device(&firmware_handle));
     ASSERT_TRUE(compare_device_serial_number(firmware_handle, serial_number));
 
     LOG_INFO("Updating the device to the Test firmware.");
@@ -145,13 +300,22 @@ TEST_F(firmware_fw, simple_update_from_lkg)
                                     g_test_firmware_package_info,
                                     false));
 
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, disconnect_device());
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, connect_device());
+
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, read_calibration(&calibration_post_update, &calibration_post_update_size));
+    ASSERT_TRUE(compare_calibration());
+
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, open_firmware_device(&firmware_handle));
     ASSERT_TRUE(compare_device_serial_number(firmware_handle, serial_number));
 }
 
 TEST_F(firmware_fw, simple_update_from_factory)
 {
     LOG_INFO("Beginning the basic update test from the Factory firmware.", 0);
-    ASSERT_EQ(K4A_RESULT_SUCCEEDED, g_connection_exerciser->set_usb_port(g_k4a_port_number));
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, connect_device());
+
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, read_calibration(&calibration_pre_update, &calibration_pre_update_size));
 
     ASSERT_EQ(K4A_RESULT_SUCCEEDED, open_firmware_device(&firmware_handle));
 
@@ -171,6 +335,13 @@ TEST_F(firmware_fw, simple_update_from_factory)
                                     g_factory_firmware_package_info,
                                     false));
 
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, disconnect_device());
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, connect_device());
+
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, read_calibration(&calibration_post_update, &calibration_post_update_size));
+    ASSERT_TRUE(compare_calibration());
+
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, open_firmware_device(&firmware_handle));
     ASSERT_TRUE(compare_device_serial_number(firmware_handle, serial_number));
 
     LOG_INFO("Updating the device to the Candidate firmware.");
@@ -181,6 +352,13 @@ TEST_F(firmware_fw, simple_update_from_factory)
                                     g_candidate_firmware_package_info,
                                     false));
 
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, disconnect_device());
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, connect_device());
+
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, read_calibration(&calibration_post_update, &calibration_post_update_size));
+    ASSERT_TRUE(compare_calibration());
+
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, open_firmware_device(&firmware_handle));
     ASSERT_TRUE(compare_device_serial_number(firmware_handle, serial_number));
 
     LOG_INFO("Updating the device to the Test firmware.");
@@ -191,5 +369,12 @@ TEST_F(firmware_fw, simple_update_from_factory)
                                     g_test_firmware_package_info,
                                     false));
 
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, disconnect_device());
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, connect_device());
+
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, read_calibration(&calibration_post_update, &calibration_post_update_size));
+    ASSERT_TRUE(compare_calibration());
+
+    ASSERT_EQ(K4A_RESULT_SUCCEEDED, open_firmware_device(&firmware_handle));
     ASSERT_TRUE(compare_device_serial_number(firmware_handle, serial_number));
 }

tests/Transformation/transformation.cpp

@@ -354,7 +354,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;
-    if (abs(check_sum - reference_val) > 0.001)
+    if (std::abs(check_sum - reference_val) > 0.001)
     {
         ASSERT_EQ(check_sum, reference_val);
     }
@@ -563,4 +563,4 @@ TEST_F(transformation_ut, transformation_create_color_only)
 int main(int argc, char **argv)
 {
     return k4a_test_commmon_main(argc, argv);
-}
+}