Skip to content
This repository was archived by the owner on Aug 22, 2024. It is now read-only.

Commit 52cfb4a

Browse files
wes-bGreendogo
wes-b
and
Greendogo
authored
Latency (#1085)
* Latency Measuring test was added * Latency Test Added * Added Test to IMU_ft to ensure IMU and color device timestamps measured up * updated * Test speed up * Remove useless EXPECT_EQ comparing the same 64ints * Fixing Clang breaks * Fixing Clang breaks Co-authored-by: Paul McElroy <[email protected]>
1 parent 1d8878e commit 52cfb4a

File tree

13 files changed

+1167
-43
lines changed

13 files changed

+1167
-43
lines changed

‎include/k4ainternal/common.h‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,10 @@ typedef struct _guid_t
3636
#define MAX_SERIAL_NUMBER_LENGTH \
3737
(13 * 2) // Current schema is for 12 digits plus NULL, the extra size is in case that grows in the future.
3838

39+
#define HZ_TO_PERIOD_MS(Hz) (1000 / Hz)
40+
#define HZ_TO_PERIOD_US(Hz) (1000000 / Hz)
41+
#define HZ_TO_PERIOD_NS(Hz) (1000000000 / Hz)
42+
3943
inline static uint32_t k4a_convert_fps_to_uint(k4a_fps_t fps)
4044
{
4145
uint32_t fps_int;

‎src/capturesync/capturesync.c‎

Lines changed: 14 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -59,8 +59,6 @@ K4A_DECLARE_CONTEXT(capturesync_t, capturesync_context_t);
5959
#define DEPTH_CAPTURE (false)
6060
#define COLOR_CAPTURE (true)
6161

62-
#define MICRO_SECONDS(seconds) (seconds * 1000000)
63-
6462
/**
6563
* This function is responsible for updating the information in either capturesync_context_t->depth_ir or in
6664
* capturesync_context_t->color. capturesync_context_t holds the capture, image, and ts for the sample we are currenly
@@ -488,41 +486,31 @@ k4a_result_t capturesync_start(capturesync_t capturesync_handle, const k4a_devic
488486
RETURN_VALUE_IF_HANDLE_INVALID(K4A_RESULT_FAILED, capturesync_t, capturesync_handle);
489487
RETURN_VALUE_IF_ARG(K4A_RESULT_FAILED, config == NULL);
490488
capturesync_context_t *sync = capturesync_t_get_context(capturesync_handle);
491-
k4a_result_t result = K4A_RESULT_SUCCEEDED;
492489

493490
// Reset frames to drop
494491
sync->waiting_for_clean_depth_ts = true;
495492
sync->synchronized_images_only = config->synchronized_images_only;
496493

497-
uint32_t camera_fps = k4a_convert_fps_to_uint(config->camera_fps);
494+
sync->fps_period = HZ_TO_PERIOD_US(k4a_convert_fps_to_uint(config->camera_fps));
495+
sync->fps_1_quarter_period = sync->fps_period / 4;
496+
sync->depth_delay_off_color_usec = config->depth_delay_off_color_usec;
497+
sync->sync_captures = true;
498+
sync->depth_captures_dropped = 0;
498499

499-
result = K4A_RESULT_FROM_BOOL(camera_fps > 0);
500-
if (K4A_SUCCEEDED(result))
500+
if (config->color_resolution == K4A_COLOR_RESOLUTION_OFF || config->depth_mode == K4A_DEPTH_MODE_OFF)
501501
{
502-
sync->fps_period = MICRO_SECONDS(1) / camera_fps;
503-
sync->fps_1_quarter_period = sync->fps_period / 4;
504-
sync->depth_delay_off_color_usec = config->depth_delay_off_color_usec;
505-
sync->sync_captures = true;
506-
sync->depth_captures_dropped = 0;
507-
508-
if (config->color_resolution == K4A_COLOR_RESOLUTION_OFF || config->depth_mode == K4A_DEPTH_MODE_OFF)
509-
{
510-
// Only 1 sensor is running, disable synchronization
511-
sync->sync_captures = false;
512-
}
502+
// Only 1 sensor is running, disable synchronization
503+
sync->sync_captures = false;
513504
}
514505

515-
if (K4A_SUCCEEDED(result))
516-
{
517-
queue_enable(sync->color.queue);
518-
queue_enable(sync->depth_ir.queue);
519-
queue_enable(sync->sync_queue);
506+
queue_enable(sync->color.queue);
507+
queue_enable(sync->depth_ir.queue);
508+
queue_enable(sync->sync_queue);
520509

521-
// Not taking the lock as we don't need to synchronize this on start
522-
sync->running = true;
523-
}
510+
// Not taking the lock as we don't need to synchronize this on start
511+
sync->running = true;
524512

525-
return result;
513+
return K4A_RESULT_SUCCEEDED;
526514
}
527515

528516
void capturesync_stop(capturesync_t capturesync_handle)

‎src/dewrapper/dewrapper.c‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -135,7 +135,7 @@ static k4a_result_t depth_engine_start_helper(dewrapper_context_t *dewrapper,
135135
assert(dewrapper->calibration_memory != NULL);
136136

137137
// Max comput time is the configured FPS
138-
*depth_engine_max_compute_time_ms = 1000 / k4a_convert_fps_to_uint(fps);
138+
*depth_engine_max_compute_time_ms = HZ_TO_PERIOD_MS(k4a_convert_fps_to_uint(fps));
139139
result = K4A_RESULT_FROM_BOOL(*depth_engine_max_compute_time_ms != 0);
140140

141141
if (K4A_SUCCEEDED(result))

‎src/sdk/k4a.c‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -681,7 +681,7 @@ static k4a_result_t validate_configuration(k4a_context_t *device, const k4a_devi
681681
if (config->wired_sync_mode == K4A_WIRED_SYNC_MODE_SUBORDINATE &&
682682
config->subordinate_delay_off_master_usec != 0)
683683
{
684-
uint32_t fps_in_usec = 1000000 / k4a_convert_fps_to_uint(config->camera_fps);
684+
uint32_t fps_in_usec = HZ_TO_PERIOD_US(k4a_convert_fps_to_uint(config->camera_fps));
685685
if (config->subordinate_delay_off_master_usec > fps_in_usec)
686686
{
687687
result = K4A_RESULT_FAILED;
@@ -716,7 +716,7 @@ static k4a_result_t validate_configuration(k4a_context_t *device, const k4a_devi
716716

717717
if (depth_enabled && color_enabled)
718718
{
719-
int64_t fps = 1000000 / k4a_convert_fps_to_uint(config->camera_fps);
719+
int64_t fps = HZ_TO_PERIOD_US(k4a_convert_fps_to_uint(config->camera_fps));
720720
if (config->depth_delay_off_color_usec < -fps || config->depth_delay_off_color_usec > fps)
721721
{
722722
result = K4A_RESULT_FAILED;

‎tests/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@ add_subdirectory(executables)
1212
add_subdirectory(ExternLibraries)
1313
add_subdirectory(FirmwareTests)
1414
add_subdirectory(global)
15+
add_subdirectory(latency)
1516
add_subdirectory(logging)
1617
add_subdirectory(IMUTests)
1718
add_subdirectory(multidevice)

‎tests/ColorTests/FunctionalTest/color_ft.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -716,7 +716,7 @@ int32_t color_control_test::map_manual_exposure(int32_t value, bool sixty_hertz)
716716
// Limit exposure setting based on FPS setting.
717717
int32_t color_control_test::limit_exposure_to_fps_setting(int32_t value, bool sixty_hertz, k4a_fps_t fps)
718718
{
719-
int fps_usec = 1000000 / k4a_convert_fps_to_uint(fps);
719+
int fps_usec = HZ_TO_PERIOD_US(k4a_convert_fps_to_uint(fps));
720720
int last_exposure;
721721

722722
if (value < fps_usec)

‎tests/IMUTests/FunctionalTest/imu_ft.cpp‎

Lines changed: 44 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@
1212
//**************Symbolic Constant Macros (defines) *************
1313
#define STREAM_RUN_TIME_SEC 4
1414
#define ERROR_START_STREAM_TIME 10000
15+
#define SECOND_TO_MICROSECONDS(sec) (sec * 1000 * 1000)
1516

1617
// Total ACC range is +/- 147.15 m/s^2.
1718
#define MIN_ACC_READING -15.0f
@@ -85,6 +86,8 @@ static void RunStreamConfig(k4a_device_t device, uint32_t expected_fps)
8586
tickcounter_ms_t end_ms;
8687
tickcounter_ms_t delta_ms;
8788
uint32_t error_tolerance;
89+
int first_sample_inspected = 0;
90+
uint64_t fps_period_us;
8891

8992
stream_count = STREAM_RUN_TIME_SEC * expected_fps;
9093
tick_count = tickcounter_create();
@@ -95,8 +98,11 @@ static void RunStreamConfig(k4a_device_t device, uint32_t expected_fps)
9598
config.color_resolution = K4A_COLOR_RESOLUTION_2160P;
9699
config.depth_mode = K4A_DEPTH_MODE_NFOV_UNBINNED;
97100
config.camera_fps = K4A_FRAMES_PER_SECOND_30;
101+
config.synchronized_images_only = true;
98102
ASSERT_EQ(K4A_RESULT_SUCCEEDED, k4a_device_start_cameras(device, &config));
99103

104+
fps_period_us = HZ_TO_PERIOD_US(k4a_convert_fps_to_uint(config.camera_fps));
105+
100106
// start streaming.
101107
ASSERT_EQ(K4A_RESULT_SUCCEEDED, k4a_device_start_imu(device));
102108

@@ -114,7 +120,16 @@ static void RunStreamConfig(k4a_device_t device, uint32_t expected_fps)
114120
{
115121
// get frames as available
116122
ASSERT_EQ(K4A_WAIT_RESULT_SUCCEEDED, k4a_device_get_imu_sample(device, &imu_sample, timeout_ms));
117-
123+
if (!first_sample_inspected)
124+
{
125+
// Time stamps should not go backwards and the first time stamps should be around zero as the color camera
126+
// staring will device time stamps reset to zero.
127+
ASSERT_LT(imu_sample.acc_timestamp_usec, SECOND_TO_MICROSECONDS(2));
128+
ASSERT_LT(imu_sample.gyro_timestamp_usec, SECOND_TO_MICROSECONDS(2));
129+
std::cout << "Initial Timestamps are: " << imu_sample.gyro_timestamp_usec << " and "
130+
<< imu_sample.gyro_timestamp_usec << "\n";
131+
first_sample_inspected = 1;
132+
}
118133
ASSERT_GT(imu_sample.acc_timestamp_usec, last_acc_dev_ts);
119134
last_acc_dev_ts = imu_sample.acc_timestamp_usec;
120135
ASSERT_GT(imu_sample.gyro_timestamp_usec, last_gyro_dev_ts);
@@ -128,6 +143,34 @@ static void RunStreamConfig(k4a_device_t device, uint32_t expected_fps)
128143
ASSERT_EQ(true, is_float_in_range(imu_sample.gyro_sample.xyz.y, MIN_GYRO_READING, MAX_GYRO_READING, "GYRO_Y"));
129144
ASSERT_EQ(true, is_float_in_range(imu_sample.gyro_sample.xyz.z, MIN_GYRO_READING, MAX_GYRO_READING, "GYRO_Z"));
130145

146+
{
147+
k4a_capture_t capture;
148+
k4a_wait_result_t wresult;
149+
ASSERT_NE(wresult = k4a_device_get_capture(device, &capture, 0), K4A_WAIT_RESULT_FAILED);
150+
if (wresult == K4A_WAIT_RESULT_SUCCEEDED)
151+
{
152+
k4a_image_t image = k4a_capture_get_color_image(capture);
153+
int64_t ts_c_dev = (int64_t)k4a_image_get_device_timestamp_usec(image);
154+
EXPECT_LT(std::abs(ts_c_dev - (int64_t)imu_sample.gyro_timestamp_usec), (int64_t)fps_period_us * 4);
155+
EXPECT_LT(std::abs(ts_c_dev - (int64_t)imu_sample.acc_timestamp_usec), (int64_t)fps_period_us * 4);
156+
k4a_image_release(image);
157+
158+
image = k4a_capture_get_ir_image(capture);
159+
int64_t ts_ir_dev = (int64_t)k4a_image_get_device_timestamp_usec(image);
160+
EXPECT_LT(std::abs(ts_ir_dev - (int64_t)imu_sample.gyro_timestamp_usec), (int64_t)fps_period_us * 4);
161+
EXPECT_LT(std::abs(ts_ir_dev - (int64_t)imu_sample.acc_timestamp_usec), (int64_t)fps_period_us * 4);
162+
k4a_image_release(image);
163+
164+
// printf("IMU PTS delta %" PRId64 " %" PRId64 " %" PRId64 " %" PRId64 " \n",
165+
// (int64_t)imu_sample.gyro_timestamp_usec - ts_c_dev,
166+
// (int64_t)imu_sample.acc_timestamp_usec - ts_c_dev,
167+
// (int64_t)imu_sample.gyro_timestamp_usec - ts_ir_dev,
168+
// (int64_t)imu_sample.acc_timestamp_usec - ts_ir_dev);
169+
170+
k4a_capture_release(capture);
171+
}
172+
}
173+
131174
stream_count--;
132175
};
133176

‎tests/RecordTests/UnitTest/playback_ut.cpp‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -92,7 +92,7 @@ TEST_F(playback_ut, open_large_file)
9292
k4a_capture_t capture = NULL;
9393
k4a_stream_result_t stream_result = K4A_STREAM_RESULT_FAILED;
9494
uint64_t timestamps[3] = { 0, 1000, 1000 };
95-
uint64_t timestamp_delta = 1000000 / k4a_convert_fps_to_uint(config.camera_fps);
95+
uint64_t timestamp_delta = HZ_TO_PERIOD_US(k4a_convert_fps_to_uint(config.camera_fps));
9696
size_t i = 0;
9797
for (; i < 50; i++)
9898
{
@@ -177,7 +177,7 @@ TEST_F(playback_ut, open_delay_offset_file)
177177
k4a_capture_t capture = NULL;
178178
k4a_stream_result_t stream_result = K4A_STREAM_RESULT_FAILED;
179179
uint64_t timestamps[3] = { 0, 10000, 10000 };
180-
uint64_t timestamp_delta = 1000000 / k4a_convert_fps_to_uint(config.camera_fps);
180+
uint64_t timestamp_delta = HZ_TO_PERIOD_US(k4a_convert_fps_to_uint(config.camera_fps));
181181

182182
// Read forward
183183
for (size_t i = 0; i < test_frame_count; i++)
@@ -281,7 +281,7 @@ TEST_F(playback_ut, playback_seek_test)
281281
k4a_capture_t capture = NULL;
282282
k4a_stream_result_t stream_result = K4A_STREAM_RESULT_FAILED;
283283
uint64_t timestamps[3] = { 0, 1000, 1000 };
284-
uint64_t timestamp_delta = 1000000 / k4a_convert_fps_to_uint(config.camera_fps);
284+
uint64_t timestamp_delta = HZ_TO_PERIOD_US(k4a_convert_fps_to_uint(config.camera_fps));
285285

286286
k4a_imu_sample_t imu_sample = { 0 };
287287
uint64_t imu_timestamp = 1150;
@@ -546,7 +546,7 @@ TEST_F(playback_ut, open_skipped_frames_file)
546546
k4a_capture_t capture = NULL;
547547
k4a_stream_result_t stream_result = K4A_STREAM_RESULT_FAILED;
548548
uint64_t timestamps[3] = { 1000000, 1001000, 1001000 };
549-
uint64_t timestamp_delta = 1000000 / k4a_convert_fps_to_uint(config.camera_fps);
549+
uint64_t timestamp_delta = HZ_TO_PERIOD_US(k4a_convert_fps_to_uint(config.camera_fps));
550550

551551
// Test initial state
552552
stream_result = k4a_playback_get_previous_capture(handle, &capture);
@@ -817,7 +817,7 @@ TEST_F(playback_ut, open_start_offset_file)
817817
k4a_stream_result_t stream_result = K4A_STREAM_RESULT_FAILED;
818818
uint64_t timestamps[3] = { 1000000, 1000000, 1000000 };
819819
uint64_t imu_timestamp = 1001150;
820-
uint64_t timestamp_delta = 1000000 / k4a_convert_fps_to_uint(config.camera_fps);
820+
uint64_t timestamp_delta = HZ_TO_PERIOD_US(k4a_convert_fps_to_uint(config.camera_fps));
821821
uint64_t last_timestamp = k4a_playback_get_recording_length_usec(handle) +
822822
(uint64_t)config.start_timestamp_offset_usec;
823823
ASSERT_EQ(last_timestamp, (uint64_t)config.start_timestamp_offset_usec + 3333150);

‎tests/RecordTests/UnitTest/sample_recordings.cpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -65,7 +65,7 @@ void SampleRecordings::SetUp()
6565

6666
uint64_t timestamps[3] = { 0, 1000, 1000 }; // Offset the Depth and IR tracks by 1ms to test
6767
uint64_t imu_timestamp = 1150;
68-
uint32_t timestamp_delta = 1000000 / k4a_convert_fps_to_uint(record_config_full.camera_fps);
68+
uint32_t timestamp_delta = HZ_TO_PERIOD_US(k4a_convert_fps_to_uint(record_config_full.camera_fps));
6969
k4a_capture_t capture = NULL;
7070
for (size_t i = 0; i < test_frame_count; i++)
7171
{
@@ -108,7 +108,7 @@ void SampleRecordings::SetUp()
108108
uint64_t timestamps[3] = { 0,
109109
(uint64_t)record_config_delay.depth_delay_off_color_usec,
110110
(uint64_t)record_config_delay.depth_delay_off_color_usec };
111-
uint32_t timestamp_delta = 1000000 / k4a_convert_fps_to_uint(record_config_delay.camera_fps);
111+
uint32_t timestamp_delta = HZ_TO_PERIOD_US(k4a_convert_fps_to_uint(record_config_delay.camera_fps));
112112
k4a_capture_t capture = NULL;
113113
for (size_t i = 0; i < test_frame_count; i++)
114114
{
@@ -169,7 +169,7 @@ void SampleRecordings::SetUp()
169169
}
170170

171171
uint64_t timestamps[3] = { 1000000, 1001000, 1001000 }; // Start recording at 1s
172-
uint32_t timestamp_delta = 1000000 / k4a_convert_fps_to_uint(record_config_full.camera_fps);
172+
uint32_t timestamp_delta = HZ_TO_PERIOD_US(k4a_convert_fps_to_uint(record_config_full.camera_fps));
173173
for (size_t i = 0; i < test_frame_count; i++)
174174
{
175175
// Create a known pattern of dropped / missing frames that can be tested against
@@ -235,7 +235,7 @@ void SampleRecordings::SetUp()
235235

236236
uint64_t timestamps[3] = { 1000000, 1000000, 1000000 };
237237
uint64_t imu_timestamp = 1001150;
238-
uint32_t timestamp_delta = 1000000 / k4a_convert_fps_to_uint(record_config_delay.camera_fps);
238+
uint32_t timestamp_delta = HZ_TO_PERIOD_US(k4a_convert_fps_to_uint(record_config_delay.camera_fps));
239239
k4a_capture_t capture = NULL;
240240
for (size_t i = 0; i < test_frame_count; i++)
241241
{

‎tests/latency/CMakeLists.txt‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,15 @@
1+
# Copyright (c) Microsoft Corporation. All rights reserved.
2+
# Licensed under the MIT License.
3+
4+
add_executable(latency_perf latency_perf.cpp)
5+
6+
target_compile_definitions(latency_perf PRIVATE _CRT_SECURE_NO_WARNINGS)
7+
8+
target_link_libraries(latency_perf PRIVATE
9+
azure::aziotsharedutil
10+
gtest::gtest
11+
k4a::k4a
12+
k4ainternal::logging
13+
k4ainternal::utcommon)
14+
15+
k4a_add_tests(TARGET latency_perf HARDWARE_REQUIRED TEST_TYPE PERF)

0 commit comments

Comments
 (0)