octopus, pt, aruco: optimize image copying

Perform less operations in inner loop where pixels are accessed.

7 files changed, 109 insertions, 63 deletions

diff --git a/ftnoir_tracker_aruco/ar_video_widget.cpp b/ftnoir_tracker_aruco/ar_video_widget.cpp
index 61a611ea..2d9e3052 100644
--- a/ftnoir_tracker_aruco/ar_video_widget.cpp
+++ b/ftnoir_tracker_aruco/ar_video_widget.cpp
@@ -14,30 +14,47 @@ using namespace std;
 void ArucoVideoWidget::update_image(const cv::Mat& frame)
 {
     QMutexLocker foo(&mtx);
-    _frame = frame.clone();
+    if (!fresh)
+    {
+        _frame = frame.clone();
+        fresh = true;
+    }
 }
 
 void ArucoVideoWidget::update_and_repaint()
 {
-    QMutexLocker foo(&mtx);
-    if (_frame.cols*_frame.rows <= 0)
-        return;
-    QImage qframe = QImage(_frame.cols, _frame.rows, QImage::Format_RGB888);
-    uchar* data = qframe.bits();
-    const int pitch = qframe.bytesPerLine();
-    for (int y = 0; y < _frame.rows; y++)
+    QImage qframe;
+    
     {
-        for (int x = 0; x < _frame.cols; x++)
+        QMutexLocker foo(&mtx);
+        if (_frame.cols*_frame.rows <= 0 || !fresh)
+            return;
+        fresh = false;
+        qframe = QImage(_frame.cols, _frame.rows, QImage::Format_RGB888);
+        uchar* data = qframe.bits();
+        const int pitch = qframe.bytesPerLine();
+        unsigned char *input = (unsigned char*)(_frame.data);
+        const int chans = _frame.channels();
+        for (int y = 0; y < _frame.rows; y++)
         {
-            const auto& elt = _frame.at<cv::Vec3b>(y, x);
-            const cv::Scalar elt2 = static_cast<cv::Scalar>(elt);
-            data[y * pitch + x * 3 + 0] = elt2.val[2];
-            data[y * pitch + x * 3 + 1] = elt2.val[1];
-            data[y * pitch + x * 3 + 2] = elt2.val[0];
+            const int step = y * _frame.step;
+            const int pitch_ = y * pitch;
+            for (int x = 0; x < _frame.cols; x++)
+            {
+                data[pitch_ + x * 3 + 0] = input[step + x * chans + 2];
+                data[pitch_ + x * 3 + 1] = input[step + x * chans + 1];
+                data[pitch_ + x * 3 + 2] = input[step + x * chans + 0];
+            }
         }
     }
-    auto qframe2 = qframe.scaled(size(), Qt::IgnoreAspectRatio, Qt::FastTransformation);
-    texture = qframe2;
+    
+    qframe = qframe.scaled(size(), Qt::IgnoreAspectRatio, Qt::FastTransformation);
+    
+    {
+        QMutexLocker foo(&mtx);
+        texture = qframe;
+    }
+    
     update();
 }
 
@@ -47,8 +64,8 @@ void ArucoVideoWidget::paintEvent(QPaintEvent* e)
     QPainter(this).drawImage(e->rect(), texture);
 }
 
-ArucoVideoWidget::ArucoVideoWidget(QWidget* parent): QWidget(parent)
+ArucoVideoWidget::ArucoVideoWidget(QWidget* parent): QWidget(parent), fresh(false)
 {
     connect(&timer, SIGNAL(timeout()), this, SLOT(update_and_repaint()));
     timer.start(60);
-}
\ No newline at end of file
+}
diff --git a/ftnoir_tracker_aruco/ar_video_widget.h b/ftnoir_tracker_aruco/ar_video_widget.h
index 820ba7d0..8c51382b 100644
--- a/ftnoir_tracker_aruco/ar_video_widget.h
+++ b/ftnoir_tracker_aruco/ar_video_widget.h
@@ -28,6 +28,7 @@ private:
     QImage texture;
     QTimer timer;
     cv::Mat _frame;
+    bool fresh;
 private slots:
     void update_and_repaint(); 
 public:
diff --git a/ftnoir_tracker_ht/ht_video_widget.cpp b/ftnoir_tracker_ht/ht_video_widget.cpp
index c6d59b34..1c5f565c 100644
--- a/ftnoir_tracker_ht/ht_video_widget.cpp
+++ b/ftnoir_tracker_ht/ht_video_widget.cpp
@@ -14,31 +14,44 @@ using namespace std;
 void HTVideoWidget::update_image(unsigned char *frame, int width, int height)
 {
     QMutexLocker foo(&mtx);
-    memcpy(fb, frame, width * height * 3);
-    this->width = width;
-    this->height = height;
+    if (!fresh)
+    {
+        memcpy(fb, frame, width * height * 3);
+        this->width = width;
+        this->height = height;
+        fresh = true;
+    }
 }
 
 void HTVideoWidget::update_and_repaint()
 {
-    QMutexLocker foo(&mtx);
-    if (width*height <= 0)
-        return;
-    QImage qframe = QImage(width, height, QImage::Format_RGB888);
-    uchar* data = qframe.bits();
-    const int pitch = qframe.bytesPerLine();
-    for (int y = 0; y < height; y++)
+    QImage qframe;
     {
-        const int part = y*width;
-        for (int x = 0; x < width; x++)
+        QMutexLocker foo(&mtx);
+        if (width*height <= 0 || !fresh)
+            return;
+        fresh = false;
+        qframe = QImage(width, height, QImage::Format_RGB888);
+        uchar* data = qframe.bits();
+        const int pitch = qframe.bytesPerLine();
+        for (int y = 0; y < height; y++)
         {
-            const int pos = 3 * (part + x);
-            data[y * pitch + x * 3 + 0] = fb[pos + 2];
-            data[y * pitch + x * 3 + 1] = fb[pos + 1];
-            data[y * pitch + x * 3 + 2] = fb[pos + 0];
+            const int part = y*width;
+            for (int x = 0; x < width; x++)
+            {
+                const int pos = 3 * (part + x);
+                const int x_ = x * 3;
+                data[x_ + 0] = fb[pos + 2];
+                data[x_ + 1] = fb[pos + 1];
+                data[x_ + 2] = fb[pos + 0];
+            }
+            data += pitch;
         }
     }
-    auto qframe2 = qframe.scaled(size(), Qt::IgnoreAspectRatio, Qt::FastTransformation);
-    texture = qframe2;
+    qframe = qframe.scaled(size(), Qt::IgnoreAspectRatio, Qt::FastTransformation);
+    {
+        QMutexLocker foo(&mtx);
+        texture = qframe;
+    }
     update();
 }
diff --git a/ftnoir_tracker_ht/ht_video_widget.h b/ftnoir_tracker_ht/ht_video_widget.h
index cbfe6ddc..be4aee44 100644
--- a/ftnoir_tracker_ht/ht_video_widget.h
+++ b/ftnoir_tracker_ht/ht_video_widget.h
@@ -23,7 +23,7 @@ class HTVideoWidget : public QWidget
     Q_OBJECT
 
 public:
-    HTVideoWidget(QWidget *parent) : QWidget(parent), fb(), width(0), height(0) {
+    HTVideoWidget(QWidget *parent) : QWidget(parent), fb(), width(0), height(0), fresh(false) {
         connect(&timer, SIGNAL(timeout()), this, SLOT(update_and_repaint()));
         timer.start(60);
     }
@@ -40,8 +40,9 @@ private:
     QMutex mtx;
     QImage texture;
     QTimer timer;
-    char fb[2048*2048*3];
+    unsigned char fb[2048*2048*3];
     int width,height;
+    bool fresh;
 };
 
 #endif // VIDEOWIDGET_H
diff --git a/ftnoir_tracker_pt/pt_video_widget.cpp b/ftnoir_tracker_pt/pt_video_widget.cpp
index aefb8199..12f01413 100644
--- a/ftnoir_tracker_pt/pt_video_widget.cpp
+++ b/ftnoir_tracker_pt/pt_video_widget.cpp
@@ -18,29 +18,43 @@ using namespace std;
 void PTVideoWidget::update_image(const cv::Mat& frame)
 {
     QMutexLocker foo(&mtx);
-    _frame = frame.clone();
-    freshp = true;
+    
+    if (!freshp)
+    {
+        _frame = frame.clone();
+        freshp = true;
+    }
 }
 
 void PTVideoWidget::update_and_repaint()
 {
-    QMutexLocker foo(&mtx);
-    if (_frame.empty() || !freshp)
-        return;
-    freshp = false;
-    QImage qframe = QImage(_frame.cols, _frame.rows, QImage::Format_RGB888);
-    uchar* data = qframe.bits();
-    const int pitch = qframe.bytesPerLine();
-    for (int y = 0; y < _frame.rows; y++)
-        for (int x = 0; x < _frame.cols; x++)
+    QImage qframe;
+    {
+        QMutexLocker foo(&mtx);
+        if (_frame.empty() || !freshp)
+            return;
+        qframe = QImage(_frame.cols, _frame.rows, QImage::Format_RGB888);
+        freshp = false;
+        uchar* data = qframe.bits();
+        const int pitch = qframe.bytesPerLine();
+        unsigned char *input = (unsigned char*) _frame.data;
+        const int chans = _frame.channels();
+        for (int y = 0; y < _frame.rows; y++)
         {
-            const auto& elt = _frame.at<Vec3b>(y, x);
-            const cv::Scalar elt2 = static_cast<cv::Scalar>(elt);
-            data[y * pitch + x * 3 + 0] = elt2.val[2];
-            data[y * pitch + x * 3 + 1] = elt2.val[1];
-            data[y * pitch + x * 3 + 2] = elt2.val[0];
+            const int step = y * _frame.step;
+            const int pitch_ = y * pitch;
+            for (int x = 0; x < _frame.cols; x++)
+            {
+                data[pitch_ + x * 3 + 0] = input[step + x * chans + 2];
+                data[pitch_ + x * 3 + 1] = input[step + x * chans + 1];
+                data[pitch_ + x * 3 + 2] = input[step + x * chans + 0];
+            }
         }
+    }
     qframe = qframe.scaled(size(), Qt::IgnoreAspectRatio, Qt::FastTransformation);
-    texture = qframe;
+    {
+        QMutexLocker foo(&mtx);
+        texture = qframe;
+    }
     update();
 }
diff --git a/ftnoir_tracker_pt/pt_video_widget.h b/ftnoir_tracker_pt/pt_video_widget.h
index f2b41d63..07b1ee2a 100644
--- a/ftnoir_tracker_pt/pt_video_widget.h
+++ b/ftnoir_tracker_pt/pt_video_widget.h
@@ -39,7 +39,6 @@ public:
         timer.start(40);
     }
     void update_image(const cv::Mat &frame);
-    void update_frame_and_points() {}
 protected slots:
     void paintEvent( QPaintEvent* e ) {
         QMutexLocker foo(&mtx);
@@ -52,5 +51,5 @@ private:
     QImage texture;
     QTimer timer;
     cv::Mat _frame;
-    volatile bool freshp;
+    bool freshp;
 };
diff --git a/pose-widget/glwidget.cpp b/pose-widget/glwidget.cpp
index 0322077b..b444e87e 100644
--- a/pose-widget/glwidget.cpp
+++ b/pose-widget/glwidget.cpp
@@ -145,14 +145,14 @@ void GLWidget::project_quad_texture() {
         Triangle(projected[1][0], projected[1][1], projected[1][2])
     };
   
-    int orig_pitch = tex.bytesPerLine();
-    int dest_pitch = texture.bytesPerLine();
+    const int orig_pitch = tex.bytesPerLine();
+    const int dest_pitch = texture.bytesPerLine();
     
     const unsigned char* orig = tex.bits();
     unsigned char* dest = texture.bits();
     
-    int orig_depth = tex.depth() / 8;
-    int dest_depth = texture.depth() / 8;
+    const int orig_depth = tex.depth() / 8;
+    const int dest_depth = texture.depth() / 8;
     
     /* image breakage? */
     if (orig_depth < 3)
@@ -165,12 +165,13 @@ void GLWidget::project_quad_texture() {
                 vec2 coords;
                 if (triangles[i].barycentric_coords(pos, coords))
                 {
-                    int px = origs[i][0].x()
+                    const int px = origs[i][0].x()
                             + coords.x() * (origs[i][2].x() - origs[i][0].x())
                             + coords.y() * (origs[i][1].x() - origs[i][0].x());
-                    int py = origs[i][0].y()
+                    const int py = origs[i][0].y()
                             + coords.x() * (origs[i][2].y() - origs[i][0].y())
                             + coords.y() * (origs[i][1].y() - origs[i][0].y());
+                    
                     int r = orig[py * orig_pitch + px * orig_depth + 2];
                     int g = orig[py * orig_pitch + px * orig_depth + 1];
                     int b = orig[py * orig_pitch + px * orig_depth + 0];