Updates frame rate estimation

EyeRecToo/src/Camera.cpp

@@ -133,7 +133,6 @@ void Camera::setCamera(const QCameraInfo& cameraInfo, QCameraViewfinderSettings
 
         qInfo() << id << "Opening" << cameraInfo.description();
         camera = new QCamera(cameraInfo.deviceName().toUtf8());
-        frameGrabber = new FrameGrabber(id, colorCode);
 
         camera->load();
         if (camera->state() == QCamera::UnloadedState) {
@@ -152,6 +151,7 @@ void Camera::setCamera(const QCameraInfo& cameraInfo, QCameraViewfinderSettings
         settingsList = camera->supportedViewfinderSettings();
         QMetaObject::invokeMethod(ui, "updateSettings", Q_ARG(QList<QCameraViewfinderSettings>, settingsList), Q_ARG(QCameraViewfinderSettings, currentViewfinderSettings));
 
+        frameGrabber = new FrameGrabber(id, colorCode, settings.maximumFrameRate());
         camera->setViewfinderSettings(settings);
         camera->setViewfinder(frameGrabber);
         camera->start();

EyeRecToo/src/CameraWidget.cpp

@@ -8,6 +8,9 @@ CameraWidget::CameraWidget(QString id, ImageProcessor::Type type, QWidget* paren
     : ERWidget(id, parent)
     , type(type)
     , ui(new Ui::CameraWidget)
+    , dtEstimator(30)
+    , lastTimestamp(0)
+    , lastFrameRateUpdate(0)
     , sROI(QPoint(0, 0))
     , eROI(QPoint(0, 0))
     , userMaxPupilRatio(0)
@@ -237,25 +240,11 @@ void CameraWidget::preview(const DataTuple& data)
 
 void CameraWidget::updateFrameRate(Timestamp t)
 {
-    // Simpler version since the status bar update showed some overhead
-    // during tests with OpenGL
-    if (tq.size() == 0) {
-        ui->statusbar->showMessage(
-            QString("%1 @ N/A FPS").arg(camera->currentCameraInfo.description()));
-        tq.push_back(t);
-        lastFrameRateUpdate = t;
-        return;
-    }
-
-    tq.push_back(t);
-    if (tq.size() > 30) {
-        tq.pop_front();
-        if (tq.back() - lastFrameRateUpdate > 100) { // update every 100 ms
-            double fps = (tq.size() - 1) / (1.0e-3 * (tq.back() - tq.front()));
-            lastFrameRateUpdate = tq.back();
-            ui->statusbar->showMessage(
-                QString("%1 @ %2 FPS").arg(camera->currentCameraInfo.description()).arg(fps, 0, 'f', 2));
-        }
+    dtEstimator.update(t - lastTimestamp);
+    lastTimestamp = t;
+    if (t - lastFrameRateUpdate > 100) { // update every 100 ms
+        auto fps = 1e3 / dtEstimator.average();
+        ui->statusbar->showMessage(QString("%1 @ %2 FPS").arg(camera->currentCameraInfo.description()).arg(fps, 0, 'f', 2));
     }
 }
 

EyeRecToo/src/CameraWidget.h

@@ -94,7 +94,8 @@ private:
     QActionGroup* optionsGroup;
     QAction* optionAction;
 
-    std::deque<Timestamp> tq;
+    MovingAverage<Timestamp> dtEstimator;
+    Timestamp lastTimestamp;
     Timestamp lastFrameRateUpdate;
     void updateFrameRate(Timestamp t);
 

EyeRecToo/src/FrameGrabber.cpp

@@ -5,14 +5,16 @@
 using namespace std;
 using namespace cv;
 
-FrameGrabber::FrameGrabber(QString id, int code, QObject* parent)
+FrameGrabber::FrameGrabber(const QString id, const int code, const double fps, QObject* parent)
     : QAbstractVideoSurface(parent)
     , id(id)
     , code(code)
+    , fps(fps)
     , yuvBuffer(nullptr)
     , yuvBufferSize(0)
     , watchdog(new QTimer(this))
     , timeoutMs(2e3)
+    , drift(fps)
 {
 #ifdef TURBOJPEG
     tjh = tjInitDecompress();
@@ -113,12 +115,11 @@ bool FrameGrabber::present(const QVideoFrame& frame)
      */
     using namespace std::chrono;
 
-    // Get SW timestamp asap
-    auto t = getTimestamp(frame);
-
     if (!frame.isValid())
         return false;
 
+    auto t = getTimestamp(frame);
+
     QVideoFrame copy(frame);
     Mat cvFrame;
 

EyeRecToo/src/FrameGrabber.h

@@ -21,38 +21,129 @@ Q_DECLARE_METATYPE(SteadyTimePoint)
 
 class Drift {
 public:
-    Drift(const Timestamp tolerance = 10)
-        : tolerance(tolerance)
+    Drift(const double fps)
+        : fps(fps)
+        , maxTolerableDrift(0.5 * 1.0e3 / fps) // i.e., half a frame
+        , maxDriftSamples(1 * fps) // N seconds
+        , outOfSyncThrehsold(100)
+        , unstabilityPeriod(5e3)
+        , state(IDLE)
+        , dbg(false)
     {
     }
-    void setTolerance(const Timestamp newTolerance) { tolerance = newTolerance; }
-    Timestamp correct(const Timestamp ref, const Timestamp other)
+
+    void updateEstimate(const Timestamp src)
+    {
+        dtSrc = src - prevSrc;
+        prevSrc = src;
+        estimate += dtSrc;
+    }
+    bool isSynchronized(const Timestamp delta) const
+    {
+        using std::abs;
+        return abs(delta) < outOfSyncThrehsold;
+    }
+    bool isDriftTolerable() const
+    {
+        if (driftSamples.size() < maxDriftSamples)
+            return false;
+        return isSynchronized(dtSrc) && isSynchronized(driftSamples.back()) && abs(mean(driftSamples)) < maxTolerableDrift;
+    }
+    void synchronize(const Timestamp reference)
+    {
+        if (dbg)
+            qDebug() << "resyncing" << state << "->" << REFERENCE << driftSamples.size() << dtSrc << mean(driftSamples);
+        state = REFERENCE;
+        estimate = reference;
+        driftSamples.clear();
+    }
+    Timestamp correct(const Timestamp reference, const Timestamp src)
     {
+        using std::isnan;
         using std::max;
 
-        auto dt = other - prev;
-        auto drift = ref - (cur + dt);
-
-        // if drift is too large, we discard dt and update cur monotonically
-        if (abs(drift) > tolerance)
-            cur = max(ref, cur);
-        else
-            cur += dt;
-
-        prev = other;
-        return cur;
+        updateEstimate(src);
+        driftSamples.emplace_back(reference - estimate);
+        while (driftSamples.size() > maxDriftSamples)
+            driftSamples.pop_front();
+
+        // by default, we use the reference
+        Timestamp selected = reference;
+
+        switch (state) {
+        case IDLE:
+            prevSelected = selected;
+            firstReference = reference;
+            if (!isnan(firstReference))
+                state = INITIALIZING;
+            break;
+        case INITIALIZING:
+            if (reference - firstReference > unstabilityPeriod)
+                synchronize(reference);
+            break;
+        case REFERENCE:
+            if (isDriftTolerable())
+                state = ESTIMATE;
+            else if (driftSamples.size() >= maxDriftSamples)
+                synchronize(reference);
+            break;
+        case ESTIMATE:
+            if (isDriftTolerable())
+                selected = estimate;
+            else
+                synchronize(reference);
+            break;
+        }
+
+        selected = max(selected, prevSelected + 1);
+        prevSelected = selected;
+
+        if (dbg) {
+            auto tmp = driftSamples.size() > 0 ? driftSamples.back() : 0;
+            if (abs(selected - reference) > outOfSyncThrehsold) {
+                qDebug() << "WARNING" << state << format(tmp) << mean(driftSamples) << format(selected - reference) << format(reference) << format(estimate);
+            }
+            if (reference - lastReport > 60e3) {
+                lastReport = reference;
+                qDebug() << "report" << state << format(tmp) << mean(driftSamples) << format(selected - reference);
+            }
+        }
+        return selected;
     }
 
 private:
-    Timestamp tolerance;
-    Timestamp prev;
-    Timestamp cur;
+    const double fps;
+    const Timestamp maxTolerableDrift;
+
+    Timestamp prevSrc;
+    Timestamp dtSrc;
+
+    Timestamp estimate;
+    Timestamp prevSelected;
+
+    Timestamp firstReference;
+
+    std::deque<Timestamp> driftSamples;
+    const size_t maxDriftSamples;
+    const Timestamp outOfSyncThrehsold;
+
+    enum State {
+        IDLE = 0,
+        INITIALIZING = 1,
+        REFERENCE = 2,
+        ESTIMATE = 3,
+    };
+    State state;
+    const Timestamp unstabilityPeriod;
+
+    const bool dbg;
+    Timestamp lastReport = 0;
 };
 
 class FrameGrabber : public QAbstractVideoSurface {
     Q_OBJECT
 public:
-    explicit FrameGrabber(QString id, int code, QObject* parent = 0);
+    FrameGrabber(const QString id, const int code, const double fps, QObject* parent = 0);
     ~FrameGrabber();
     QList<QVideoFrame::PixelFormat> supportedPixelFormats(QAbstractVideoBuffer::HandleType handleType) const;
 
@@ -72,6 +163,7 @@ private:
 #endif
     QString id;
     int code;
+    double fps;
     unsigned char* yuvBuffer;
     long unsigned int yuvBufferSize;
     bool jpeg2bmp(const QVideoFrame& in, cv::Mat& cvFrame);

EyeRecToo/src/MainWindow.cpp

@@ -668,7 +668,7 @@ void MainWindow::storeMetaDataHead()
 
     QDateTime utc = QDateTime::currentDateTimeUtc();
     out << "start_utc" << Token::Delimiter << utc.toString(metaDateFormat) << Token::Newline;
-    out << "start_timer" << Token::Delimiter << gTimer.elapsed() << Token::Newline;
+    out << "start_timer" << Token::Delimiter << format(gTimer.elapsed()) << Token::Newline;
     out << "version" << Token::Delimiter << VERSION << Token::Newline;
     out << "build" << Token::Delimiter << QString("%1 %2").arg(GIT_BRANCH).arg(GIT_COMMIT_HASH) << Token::Newline;
 
@@ -689,7 +689,7 @@ void MainWindow::storeMetaDataTail()
 
     QDateTime utc = QDateTime::currentDateTimeUtc();
     out << "end_utc" << Token::Delimiter << utc.toString(metaDateFormat) << Token::Newline;
-    out << "end_timer" << Token::Delimiter << gTimer.elapsed() << Token::Newline;
+    out << "end_timer" << Token::Delimiter << format(gTimer.elapsed()) << Token::Newline;
 
     file.close();
 }

EyeRecToo/src/utils.cpp

@@ -72,10 +72,10 @@ void logMessages(QtMsgType type, const QMessageLogContext& context, const QStrin
     if (gLogWidget) {
         if (logBuffer.size() > 0) {
             for (auto s = logBuffer.begin(); s != logBuffer.end(); s++)
-                QMetaObject::invokeMethod(gLogWidget, "appendMessage", Q_ARG(const QString&, *s));
+                QMetaObject::invokeMethod(gLogWidget, "appendMessage", Q_ARG(QString, *s));
             logBuffer.clear();
         }
-        QMetaObject::invokeMethod(gLogWidget, "appendMessage", Q_ARG(const QString&, QString(str.c_str())));
+        QMetaObject::invokeMethod(gLogWidget, "appendMessage", Q_ARG(QString, QString(str.c_str())));
     } else
         logBuffer.push_back(QString(str.c_str()));
 
@@ -112,6 +112,11 @@ QString toQString(QCameraViewfinderSettings setting)
         + QString::number(static_cast<int>(setting.maximumFrameRate())) + " FPS";
 }
 
+QString format(const double v)
+{
+    return QString("%1").arg(v, 0, 'f', Token::Precision);
+}
+
 QString iniStr(QString str)
 {
     return str.remove(QRegExp("[^a-zA-Z\\d\\s]"));

EyeRecToo/src/utils.h

@@ -2,6 +2,7 @@
 #define UTILS_H
 
 #include <atomic>
+#include <deque>
 #include <iostream>
 #include <typeinfo>
 
@@ -39,6 +40,8 @@ public:
     static const int Precision;
 };
 
+QString format(const double v);
+
 template <typename T>
 inline QString journalField(const T& v)
 {
@@ -47,7 +50,7 @@ inline QString journalField(const T& v)
 template <>
 inline QString journalField<double>(const double& v)
 {
-    return QString("%1%2").arg(v, 0, 'f', Token::Precision).arg(Token::Delimiter);
+    return QString("%1%2").arg(format(v)).arg(Token::Delimiter);
 }
 template <>
 inline QString journalField<float>(const float& v)
@@ -110,7 +113,7 @@ double median(T v)
 template <typename T>
 double mean(T v)
 {
-    return std::accumulate(v.begin(), v.end(), 0.0) / v.size();
+    return v.size() > 0 ? std::accumulate(v.begin(), v.end(), 0.0) / static_cast<double>(v.size()) : std::nanl("");
 }
 
 template <typename T>
@@ -119,4 +122,30 @@ T normalize(const T value, const T mn, const T mx)
     return (value - mn) / (mx - mn);
 }
 
+template <typename T>
+class MovingAverage {
+public:
+    MovingAverage(const size_t windowSize)
+        : windowSize(windowSize)
+    {
+        total = 0;
+    }
+    double average() const { return mean(values); }
+    void update(const T t)
+    {
+        values.emplace_back(t);
+        total += values.back();
+        while (values.size() > windowSize) {
+            total -= values.front();
+            values.pop_front();
+        }
+    }
+    void setWindowSize(const size_t w) { windowSize = w; }
+
+private:
+    size_t windowSize;
+    std::deque<T> values;
+    double total;
+};
+
 #endif // UTILS_H