Synchronizer.cpp 3.35 KB
Newer Older
Thiago Santini's avatar
Thiago Santini committed
1 2 3 4
#include "Synchronizer.h"

using namespace std;

5 6 7 8 9 10
Synchronizer::Synchronizer(QObject* parent)
    : QObject(parent)
    , maxAgeMs(100)
    , safeGuardMs(5)
    , updated(false)
    , lastSynchronization(0)
Thiago Santini's avatar
Thiago Santini committed
11 12 13 14 15 16 17 18 19 20
{

    // TODO: allow user to pick sampling rate
    bool userSelected = false;
    if (userSelected) {
        // This may be dangerous and is in practice much less efficient than
        // synchronizing one of the cameras as trigger; e.g., due to timer imprecisions
        int userSynchronizeMs = 8;
        timer = new QTimer(this);
        connect(timer, SIGNAL(timeout()),
21
            this, SLOT(synchronize()));
Thiago Santini's avatar
Thiago Santini committed
22 23
        timer->start(userSynchronizeMs);
    }
24 25
    emptyEyeData.timestamp = emptyEyeData.processingTimestamp = -1;
    emptyFieldData.timestamp = emptyFieldData.processingTimestamp = -1;
Thiago Santini's avatar
Thiago Santini committed
26 27
}

28
template <class T>
Thiago Santini's avatar
Thiago Santini committed
29
void Synchronizer::updateDeque(std::deque<T>& dataDeque, T& data)
Thiago Santini's avatar
Thiago Santini committed
30
{
Thiago Santini's avatar
Thiago Santini committed
31 32 33
    dataDeque.emplace_back(data);
    while (dataDeque.back().timestamp - dataDeque.front().timestamp > maxAgeMs)
        dataDeque.pop_front();
Thiago Santini's avatar
Thiago Santini committed
34
    updated = true;
35 36 37 38
}

void Synchronizer::newRightEyeData(EyeData eyeData)
{
Thiago Santini's avatar
Thiago Santini committed
39 40
    updateDeque(rEyeDeque, eyeData);
    synchronize(rEyeDeque.back().timestamp, Trigger::RIGHT_EYE);
Thiago Santini's avatar
Thiago Santini committed
41 42 43 44
}

void Synchronizer::newLeftEyeData(EyeData eyeData)
{
Thiago Santini's avatar
Thiago Santini committed
45 46
    updateDeque(lEyeDeque, eyeData);
    synchronize(lEyeDeque.back().timestamp, Trigger::LEFT_EYE);
Thiago Santini's avatar
Thiago Santini committed
47 48 49 50
}

void Synchronizer::newFieldData(FieldData fieldData)
{
Thiago Santini's avatar
Thiago Santini committed
51 52
    updateDeque(fieldDeque, fieldData);
    synchronize(fieldDeque.back().timestamp, Trigger::FIELD_EYE);
Thiago Santini's avatar
Thiago Santini committed
53 54
}

55
template <class T>
Thiago Santini's avatar
Thiago Santini committed
56
T* Synchronizer::getClosestInTime(Timestamp timestamp, deque<T>& dataDeque)
Thiago Santini's avatar
Thiago Santini committed
57
{
58
    T* closest = nullptr;
Thiago Santini's avatar
Thiago Santini committed
59
    if (dataDeque.empty())
Thiago Santini's avatar
Thiago Santini committed
60 61
        return closest;

Thiago Santini's avatar
Thiago Santini committed
62 63 64
    Timestamp minimum = std::numeric_limits<Timestamp>::max();
    for (auto& data : dataDeque) {
        unsigned int diff = abs(timestamp - data.timestamp);
Thiago Santini's avatar
Thiago Santini committed
65 66
        if (diff < minimum) {
            minimum = diff;
Thiago Santini's avatar
Thiago Santini committed
67
            closest = &data;
Thiago Santini's avatar
Thiago Santini committed
68 69 70 71
        }
    }

    if (minimum > maxAgeMs)
Thiago Santini's avatar
Thiago Santini committed
72
        return nullptr;
Thiago Santini's avatar
Thiago Santini committed
73 74 75 76

    return closest;
}

Thiago Santini's avatar
Thiago Santini committed
77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94
bool Synchronizer::shouldSynchronize(Timestamp timestamp, Trigger trigger)
{
    switch (trigger) {
    case Trigger::FIELD_EYE:
        if (!lEyeDeque.empty())
            if (timestamp - lEyeDeque.back().timestamp < maxAgeMs)
                return false;
    case Trigger::LEFT_EYE:
        if (!rEyeDeque.empty())
            if (timestamp - rEyeDeque.back().timestamp < maxAgeMs)
                return false;
    case Trigger::RIGHT_EYE:
    default:
        return true;
    }
}

void Synchronizer::synchronize(Timestamp timestamp, Trigger trigger)
Thiago Santini's avatar
Thiago Santini committed
95 96 97 98 99
{
    if (!updated)
        return;
    updated = false;

Thiago Santini's avatar
Thiago Santini committed
100 101
    if (!shouldSynchronize(timestamp, trigger))
        return;
Thiago Santini's avatar
Thiago Santini committed
102 103 104 105 106

    if (timestamp == maxTimestamp) // nothing recent was available
        return;

    // Make sure we don't overload the thread event loop
107
    if (timestamp - lastSynchronization < safeGuardMs && lastSynchronization != 0)
Thiago Santini's avatar
Thiago Santini committed
108 109 110 111
        return;

    lastSynchronization = timestamp;

Thiago Santini's avatar
Thiago Santini committed
112 113 114
    FieldData* field = getClosestInTime<FieldData>(timestamp, fieldDeque);
    EyeData* lEye = getClosestInTime<EyeData>(timestamp, lEyeDeque);
    EyeData* rEye = getClosestInTime<EyeData>(timestamp, rEyeDeque);
Thiago Santini's avatar
Thiago Santini committed
115

116 117
    DataTuple dataTuple(
        timestamp,
Thiago Santini's avatar
Thiago Santini committed
118 119 120
        (lEye ? *lEye : emptyEyeData),
        (rEye ? *rEye : emptyEyeData),
        (field ? *field : emptyFieldData));
Thiago Santini's avatar
Thiago Santini committed
121 122 123

    emit newData(dataTuple);
}