diff --git a/stereo_viewer/inc/focus.hpp b/stereo_viewer/inc/focus.hpp
index ea93039..dd4d2ac 100644
--- a/stereo_viewer/inc/focus.hpp
+++ b/stereo_viewer/inc/focus.hpp
@@ -25,6 +25,30 @@
 #include <QImage>
 #include <QPixmap>
 
-double focusValue(const QPixmap &pixmap);
+/**
+ * Focus class. This class is used to compute the focus value of an image.
+ */
+class Focus {
+public:
+  /**
+   * Constructor
+   * @param maxValues Maximum number of focus values to display in image stream.
+   * @param lineColor Line color of focus annotation to use.
+   * @param lineWidth Line with of focus annotation to use.
+   */
+  explicit Focus(size_t maxValues = 100, QColor lineColor = Qt::green, size_t lineWidth = 3);
+  ~Focus() = default;
+  void enable(bool enable);
+  void process(QPixmap &pixmap);
+private:
+  static cv::Mat to_mat(const QImage &image);
+  static double focusValue(const cv::Mat &img);
+  void paint(QPixmap &img);
+  size_t m_maxValues;
+  QColor m_lineColor;
+  size_t m_lineWidth;
+  bool m_enabled;
+  std::vector<double> m_last_values;
+};
 
 #endif // __FOCUS_HPP__
\ No newline at end of file
diff --git a/stereo_viewer/inc/ui/MainWindow.hpp b/stereo_viewer/inc/ui/MainWindow.hpp
index 52d0410..8a3dba3 100644
--- a/stereo_viewer/inc/ui/MainWindow.hpp
+++ b/stereo_viewer/inc/ui/MainWindow.hpp
@@ -57,6 +57,7 @@ public Q_SLOTS:
   void onFolderSelect();
   void handleSave();
   void handleColormap();
+  void handleFocus();
   void handleDeviceControl();
 
 protected:
diff --git a/stereo_viewer/inc/ui/cameraview.hpp b/stereo_viewer/inc/ui/cameraview.hpp
index 29f4db2..3d25277 100644
--- a/stereo_viewer/inc/ui/cameraview.hpp
+++ b/stereo_viewer/inc/ui/cameraview.hpp
@@ -24,6 +24,7 @@
 #include <QtWidgets/QLabel>
 #include <QtWidgets/QRubberBand>
 #include <QMouseEvent>
+#include "focus.hpp"
 
 namespace labforge::ui {
 
@@ -42,10 +43,12 @@ class CameraView : public QLabel {
   void addFeature(QPoint &pos, QColor &color, int width);
   void reset();
   void redrawPixmap();
+  void enableFocus(bool value);
 
 private:
   QPoint m_origin;
   QRect m_crop;
+  Focus m_focus;
   std::unique_ptr<QRubberBand> m_rubberband;
   std::unique_ptr<QPixmap> m_last_frame;
   bool m_scaled;
diff --git a/stereo_viewer/src/focus.cc b/stereo_viewer/src/focus.cc
index f2e6251..a3dd9d3 100644
--- a/stereo_viewer/src/focus.cc
+++ b/stereo_viewer/src/focus.cc
@@ -20,25 +20,88 @@
 #include "focus.hpp"
 
 #include <iostream>
+#include <utility>
+#include <QPainter>
 
 using namespace cv;
 using namespace std;
 
-double focusValue(const QPixmap &pixmap) {
-  // Convert QPixmap to QImage
-  QImage qimage = pixmap.toImage();
+Focus::Focus(size_t maxValues, QColor lineColor, size_t lineWidth)
+: m_maxValues(maxValues), m_lineColor(std::move(lineColor)), m_lineWidth(lineWidth), m_enabled(false) {
+  m_last_values.reserve(maxValues);
+}
 
-  // Convert QImage to Mat
+void Focus::enable(bool enable) {
+  m_enabled = enable;
+  // Reset values every time this gets touched
+  m_last_values.clear();
+}
+
+void Focus::process(QPixmap &pixmap) {
+  if(m_enabled) {
+    QImage image = pixmap.toImage();
+    Mat mat = to_mat(image);
+    if(!mat.empty()) {
+      double value = focusValue(mat);
+      m_last_values.push_back(value);
+      // Overflow
+      if(m_last_values.size() > m_maxValues) {
+        m_last_values.erase(m_last_values.begin());
+      }
+      // Front fill
+      if(m_last_values.size() < m_maxValues) {
+        m_last_values.insert(m_last_values.begin(), m_maxValues - m_last_values.size(), value);
+      }
+      // Draw focus helper
+      paint(pixmap);
+    }
+  }
+}
+
+cv::Mat Focus::to_mat(const QImage &image) {
   Mat mat;
-  switch (qimage.format()) {
+  switch (image.format()) {
     case QImage::Format_RGB32:
     case QImage::Format_ARGB32:
     case QImage::Format_ARGB32_Premultiplied:
-      mat = Mat(qimage.height(), qimage.width(), CV_8UC4, const_cast<uchar*>(qimage.bits()), qimage.bytesPerLine());
+      mat = Mat(image.height(), image.width(), CV_8UC4, const_cast<uchar*>(image.bits()), image.bytesPerLine());
       break;
     default:
-      return -1.0;
+      return mat;
   }
+  return mat;
+}
+
+double Focus::focusValue(const cv::Mat &img) {
+  Mat gray;
+  cvtColor(img, gray, COLOR_BGR2GRAY);
+  Mat lap;
+  Laplacian(gray, lap, CV_64F);
+  Scalar mu, sigma;
+  meanStdDev(lap, mu, sigma);
+  return sigma.val[0] * sigma.val[0];
+}
 
-  return 0.0;
+void Focus::paint(QPixmap &img) {
+  QPainter paint(&img);
+  paint.setPen(QPen(m_lineColor, m_lineWidth));
+
+  // Determine the range of the data
+  double minValue = *std::min_element(m_last_values.begin(), m_last_values.end());
+  double maxValue = *std::max_element(m_last_values.begin(), m_last_values.end());
+  double range = maxValue - minValue;
+  int width = img.width();
+  int height = img.height();
+
+  // Plot each point
+  for (std::size_t i = 0; i < m_last_values.size() - 1; ++i) {
+    double normalizedStart = (m_last_values[i] - minValue) / range;
+    double normalizedEnd = (m_last_values[i + 1] - minValue) / range;
+    int x1 = (i * width) / m_last_values.size();
+    int y1 = height - (normalizedStart * height);
+    int x2 = ((i + 1) * width) / m_last_values.size();
+    int y2 = height - (normalizedEnd * height);
+
+    paint.drawLine(x1, y1, x2, y2);
+  }
 }
\ No newline at end of file
diff --git a/stereo_viewer/src/ui/cameraview.cc b/stereo_viewer/src/ui/cameraview.cc
index 65e88cc..ac1e18f 100644
--- a/stereo_viewer/src/ui/cameraview.cc
+++ b/stereo_viewer/src/ui/cameraview.cc
@@ -21,13 +21,11 @@
 #include <QPen>
 
 #include "ui/cameraview.hpp"
-#include "focus.hpp"
 
 using namespace std;
 using namespace labforge::ui;
 
 CameraView::CameraView(QWidget *parent) : QLabel(parent), m_scaled(false) {
-
 }
 
 void CameraView::resizeEvent(QResizeEvent *event) {
@@ -85,11 +83,9 @@ void CameraView::mouseReleaseEvent(QMouseEvent *event) {
                                   scaled_pixmap_rect.width(),
                                   scaled_pixmap_rect.height());
       QRectF selection = m_rubberband->geometry();
-//      cout << "Input: " << selection.x() << ", " << selection.y() << ", " << selection.width() << ", " << selection.height() << endl;
 
       // Make sure we do not cross boundaries
       selection = scaled_pixmap_rect.intersected(selection);
-//      cout << "InputS: " << selection.x() << ", " << selection.y() << ", " << selection.width() << ", " << selection.height() << endl;
 
       // Find origin coordinate in scaled pixmap
       selection = QRectF(selection.x() - scaled_pixmap_rect.x(),
@@ -98,19 +94,11 @@ void CameraView::mouseReleaseEvent(QMouseEvent *event) {
                          selection.height());
 
       double scale = m_last_frame->width() / scaled_pixmap_rect.width();
-//      cout << "Scale: " << scale << endl;
       m_crop = QRect(selection.x()*scale,
                      selection.y()*scale,
                      selection.width()*scale,
                      selection.height()*scale);
-//      if(m_crop.width()*m_crop.height() <= 0) {
-//        QLabel::mouseReleaseEvent(event);
-//        return;
-//      }
-//
-//      cout << "Result" << m_crop.x() << ", " << m_crop.y()
-//      << ", " << m_crop.width()
-//      << ", " << m_crop.height() << endl;
+
       m_scaled = true;
       redrawPixmap();
     }
@@ -160,6 +148,10 @@ void CameraView::redrawPixmap() {
   } else if(m_last_frame) {
     m = m_last_frame->scaled(size(), Qt::KeepAspectRatio, Qt::FastTransformation);
   }
-  focusValue(m);
+  m_focus.process(m);
   setPixmap(m);
+}
+
+void CameraView::enableFocus(bool value) {
+  m_focus.enable(value);
 }
\ No newline at end of file
diff --git a/stereo_viewer/src/viewer.cc b/stereo_viewer/src/viewer.cc
index 8725a60..454ebd6 100644
--- a/stereo_viewer/src/viewer.cc
+++ b/stereo_viewer/src/viewer.cc
@@ -188,6 +188,7 @@ MainWindow::MainWindow(QWidget *parent) : QMainWindow(parent) {
   cfg.chkCalibrate->setVisible(true);
   cfg.chkCalibrate->setChecked(false);
   cfg.chkCalibrate->setEnabled(true);
+
   s_load_colormap(cfg.cbxColormap, COLORMAP_JET);
   s_load_format(cfg.cbxFormat, false);
   cfg.lblFormat->setVisible(false);
@@ -207,6 +208,7 @@ MainWindow::MainWindow(QWidget *parent) : QMainWindow(parent) {
   connect(cfg.btnSave, &QPushButton::released, this, &MainWindow::handleSave);
   connect(cfg.chkColormap, &QCheckBox::stateChanged, this, &MainWindow::handleColormap);
   connect(cfg.btnDeviceControl, &QPushButton::released, this, &MainWindow::handleDeviceControl);
+  connect(cfg.cbxFocus,&QCheckBox::stateChanged, this, &MainWindow::handleFocus);
 
   // Force disconnected state
   OnDisconnected();
@@ -407,6 +409,11 @@ void MainWindow::handleColormap(){
   cfg.cbxColormap->setEnabled(cfg.chkColormap->isChecked());
 }
 
+void MainWindow::handleFocus() {
+  cfg.widgetLeftSensor->enableFocus(cfg.cbxFocus->isChecked());
+  cfg.widgetRightSensor->enableFocus(cfg.cbxFocus->isChecked());
+}
+
 bool isWinVisible(PvGenBrowserWnd *aWnd){
   #ifdef _AFXDLL
     PvString wTitle = aWnd->GetTitle();
diff --git a/stereo_viewer/stereo_viewer.ui b/stereo_viewer/stereo_viewer.ui
index ff92381..a934e97 100644
--- a/stereo_viewer/stereo_viewer.ui
+++ b/stereo_viewer/stereo_viewer.ui
@@ -466,7 +466,7 @@
              <item>
               <widget class="QCheckBox" name="cbxFocus">
                <property name="toolTip">
-                <string>Visualize focus in the camera views ...</string>
+                <string>Visualize quantitative focus in the camera views ...</string>
                </property>
                <property name="text">
                 <string>Focus Aid</string>