in src/capturesync/capturesync.c [178:395]
void capturesync_add_capture(capturesync_t capturesync_handle,
k4a_result_t capture_result,
k4a_capture_t capture_raw,
bool color_capture)
{
capturesync_context_t *sync = NULL;
k4a_wait_result_t wresult = K4A_WAIT_RESULT_SUCCEEDED;
k4a_result_t result;
bool locked = false;
uint64_t ts_raw_capture = 0;
result = K4A_RESULT_FROM_BOOL(capturesync_handle != NULL);
if (K4A_SUCCEEDED(result))
{
sync = capturesync_t_get_context(capturesync_handle);
result = K4A_RESULT_FROM_BOOL(sync != NULL);
}
if (K4A_FAILED(capture_result))
{
if (sync->running)
{
LOG_WARNING("Capture Error Detected, %s", color_capture ? "Color " : "Depth ");
}
// Stop queues
queue_stop(sync->sync_queue);
queue_stop(sync->depth_ir.queue);
queue_stop(sync->color.queue);
// Reflect the low level error in the current result
result = capture_result;
}
if (K4A_SUCCEEDED(result))
{
result = K4A_RESULT_FROM_BOOL(capture_raw != NULL);
}
// Read the timestamp of the raw sample
if (K4A_SUCCEEDED(result))
{
k4a_image_t image = NULL;
if (color_capture)
{
image = capture_get_color_image(capture_raw);
}
else
{
image = capture_get_ir_image(capture_raw);
}
result = K4A_RESULT_FROM_BOOL(image != NULL);
if (K4A_SUCCEEDED(result))
{
ts_raw_capture = image_get_device_timestamp_usec(image);
image_dec_ref(image);
image = NULL;
}
}
if (K4A_SUCCEEDED(result))
{
Lock(sync->lock);
locked = true;
result = sync->running == true ? K4A_RESULT_SUCCEEDED : K4A_RESULT_FAILED;
}
if (K4A_SUCCEEDED(result))
{
if (sync->enable_ts_logging)
{
LOG_INFO("capturesync_ts, Arriving capture, TS:%10lld, %s, Color TS:%10lld, Depth TS:%10lld",
ts_raw_capture,
color_capture ? "Color " : "Depth ",
sync->color.ts,
sync->depth_ir.ts);
}
if (sync->sync_captures == false || sync->disable_sync == true)
{
// we are not synchronizing samples, just copy to the queue
queue_push(sync->sync_queue, capture_raw);
result = K4A_RESULT_FAILED; // Not an error, just a graceful exit
}
else if (!color_capture && sync->waiting_for_clean_depth_ts)
{
// Timestamps at the start of streaming are tricky, they will get reset to zero when the color camera is
// started. This code protects against the depth timestamps from being reported before the reset happens.
if (ts_raw_capture / sync->fps_period > 10)
{
sync->depth_captures_dropped++;
result = K4A_RESULT_FAILED; // Not an error, just a graceful exit
}
else
{
// Once we get a good TS we are going to always get a good TS
sync->waiting_for_clean_depth_ts = false;
if (sync->depth_captures_dropped)
{
LOG_INFO("Dropped %d depth captures waiting for time stamps to stabilize",
sync->depth_captures_dropped);
}
}
}
}
if (K4A_SUCCEEDED(result))
{
frame_info_t *frame_info = &sync->depth_ir;
if (color_capture)
{
frame_info = &sync->color;
}
if (frame_info->capture == NULL)
{
assert(frame_info->image == 0); // Both capture and image should be NULL
frame_info->image = frame_info->get_typed_image(capture_raw);
frame_info->ts = image_get_device_timestamp_usec(frame_info->image);
frame_info->capture = capture_raw;
capture_inc_ref(capture_raw);
capture_raw = NULL;
}
else
{
// Store the capture in our internal queue
k4a_capture_t dropped_sample = NULL;
queue_push_w_dropped(frame_info->queue, capture_raw, &dropped_sample);
if (dropped_sample)
{
// If the internal queue is full, then we publish the oldest frame as we can no longer store it.
// The user will interpret this a capture that is either depth or color, but not both
replace_sample(sync, dropped_sample, frame_info);
}
}
while (wresult == K4A_WAIT_RESULT_SUCCEEDED && sync->depth_ir.capture != NULL && sync->color.capture != NULL)
{
if (sync->depth_delay_off_color_usec < 0)
{
// We are using the depth capture TS as the window to find its matching color capture. Its TS
// won't be perfectly aligned so we give it a slush margine of 25% of the FPS
uint64_t begin_sync_window = TS_SUBTRACT(sync->depth_ir.ts, sync->fps_1_quarter_period);
// Offset sync window by the time we have programmed the hardware, in this case
// depth_delay_off_color_usec is negative so we subtract.
begin_sync_window = (uint64_t)TS_SUBTRACT((int64_t)begin_sync_window, sync->depth_delay_off_color_usec);
uint64_t end_sync_window = begin_sync_window + sync->fps_period;
if (sync->color.ts > end_sync_window)
{
// Drop depth_cap because color is beyond 1 period away
drop_sample(sync, &wresult, DEPTH_CAPTURE, true);
continue;
}
else if (sync->color.ts < begin_sync_window)
{
// Drop color sample because it happened before this frame window
drop_sample(sync, &wresult, COLOR_CAPTURE, true);
continue;
}
}
else
{
// We are using the color capture TS as the window to find its matching depth capture. Its TS
// won't be perfectly aligned so we give it a slush margine of 25% of the FPS
uint64_t begin_sync_window = TS_SUBTRACT(sync->color.ts, sync->fps_1_quarter_period);
// Offset sync window by the time we have programmed the hardware, in this case
// depth_delay_off_color_usec is positive.
begin_sync_window = (uint64_t)TS_ADD((int64_t)begin_sync_window, sync->depth_delay_off_color_usec);
uint64_t end_sync_window = begin_sync_window + sync->fps_period;
if (sync->depth_ir.ts > end_sync_window)
{
// Drop color_cap because depth is beyond 1 period away
drop_sample(sync, &wresult, COLOR_CAPTURE, true);
continue;
}
else if (sync->depth_ir.ts < begin_sync_window)
{
// Drop depth sample because it happened before this frame window
drop_sample(sync, &wresult, DEPTH_CAPTURE, true);
continue;
}
}
if (sync->color.capture != NULL && sync->depth_ir.capture != NULL)
{
if (sync->enable_ts_logging)
{
LOG_INFO("capturesync_link,TS_Color, %10lld, TS_Depth, %10lld,", sync->color.ts, sync->depth_ir.ts);
}
k4a_capture_t merged = merge_captures(sync->depth_ir.capture, sync->color.capture);
queue_push(sync->sync_queue, merged);
merged = NULL; // No need to call capture_dec_ref() here.
// Use drop symantic to get another sample from the queue if present. Synchronized sample is
// already in output queue and has its own ref
drop_sample(sync, &wresult, COLOR_CAPTURE, false);
drop_sample(sync, &wresult, DEPTH_CAPTURE, false);
continue;
}
if (wresult == K4A_WAIT_RESULT_FAILED)
{
sync->running = false;
LOG_ERROR("CaptureSync, error encountered access queue", 0);
}
}
}
if (locked)
{
Unlock(sync->lock);
locked = false;
}
}