diff --git a/EyeRecToo/src/Camera.cpp b/EyeRecToo/src/Camera.cpp index 3bf776385e172826b691812eebb7f769baf0daad..b204282a1962bca25c74eba960173dae2f48758d 100644 --- a/EyeRecToo/src/Camera.cpp +++ b/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, settingsList), Q_ARG(QCameraViewfinderSettings, currentViewfinderSettings)); + frameGrabber = new FrameGrabber(id, colorCode, settings.maximumFrameRate()); camera->setViewfinderSettings(settings); camera->setViewfinder(frameGrabber); camera->start(); diff --git a/EyeRecToo/src/CameraWidget.cpp b/EyeRecToo/src/CameraWidget.cpp index fb0b7df6a792048c33ae1e1fce2c348f09c182c7..4cd21c88d257794865cd0ce82040536bea3849af 100644 --- a/EyeRecToo/src/CameraWidget.cpp +++ b/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)); } } diff --git a/EyeRecToo/src/CameraWidget.h b/EyeRecToo/src/CameraWidget.h index a403d3fddc2bc5e39c505cbd7188922cee8dc3f6..831609fdd602328d614b4c401a7b01eedef09e89 100644 --- a/EyeRecToo/src/CameraWidget.h +++ b/EyeRecToo/src/CameraWidget.h @@ -94,7 +94,8 @@ private: QActionGroup* optionsGroup; QAction* optionAction; - std::deque tq; + MovingAverage dtEstimator; + Timestamp lastTimestamp; Timestamp lastFrameRateUpdate; void updateFrameRate(Timestamp t); diff --git a/EyeRecToo/src/FrameGrabber.cpp b/EyeRecToo/src/FrameGrabber.cpp index eb70a7b4d82806afa32bd04c4f23950db30eaf6a..38bc799e0655ed0cfe86fb22328a692987d6fd36 100644 --- a/EyeRecToo/src/FrameGrabber.cpp +++ b/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; diff --git a/EyeRecToo/src/FrameGrabber.h b/EyeRecToo/src/FrameGrabber.h index a5a2362edf74caecb05e5f45dc871fca452074ae..c0cde4d62996e239e9ffa2ac2ceff7a3ccc4866c 100644 --- a/EyeRecToo/src/FrameGrabber.h +++ b/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 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 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); diff --git a/EyeRecToo/src/MainWindow.cpp b/EyeRecToo/src/MainWindow.cpp index dccd50ee47be03e63e8c45e655af1c6f68232df6..a3ecbeec3c6d10310972c52e025550c8121ffc8f 100644 --- a/EyeRecToo/src/MainWindow.cpp +++ b/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(); } diff --git a/EyeRecToo/src/utils.cpp b/EyeRecToo/src/utils.cpp index 2a171a8525dde07b805252b78c09e72bc9550127..73761b29f0c1b67563afd38de63170a9f91dece6 100644 --- a/EyeRecToo/src/utils.cpp +++ b/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(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]")); diff --git a/EyeRecToo/src/utils.h b/EyeRecToo/src/utils.h index 03b99ade252c297e1654bc090bb9660de866bb63..dc394a73484f4448307da9ebeff9f4ceea97de17 100644 --- a/EyeRecToo/src/utils.h +++ b/EyeRecToo/src/utils.h @@ -2,6 +2,7 @@ #define UTILS_H #include +#include #include #include @@ -39,6 +40,8 @@ public: static const int Precision; }; +QString format(const double v); + template inline QString journalField(const T& v) { @@ -47,7 +50,7 @@ inline QString journalField(const T& v) template <> inline QString journalField(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(const float& v) @@ -110,7 +113,7 @@ double median(T v) template 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(v.size()) : std::nanl(""); } template @@ -119,4 +122,30 @@ T normalize(const T value, const T mn, const T mx) return (value - mn) / (mx - mn); } +template +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 values; + double total; +}; + #endif // UTILS_H