Commit 0d254e0b authored by Thiago Santini's avatar Thiago Santini

Updates frame rate estimation

parent 1faa5a76
......@@ -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();
......
......@@ -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));
}
}
......
......@@ -94,7 +94,8 @@ private:
QActionGroup* optionsGroup;
QAction* optionAction;
std::deque<Timestamp> tq;
MovingAverage<Timestamp> dtEstimator;
Timestamp lastTimestamp;
Timestamp lastFrameRateUpdate;
void updateFrameRate(Timestamp t);
......
......@@ -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;
......
......@@ -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);
......
......@@ -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();
}
......@@ -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]"));
......
......@@ -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
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment