Easy Tracker: Moving things around and theoretical data structure usage optimization.

4 files changed, 173 insertions, 158 deletions

diff --git a/tracker-easy/point-extractor.cpp b/tracker-easy/point-extractor.cpp
index c1f525c9..cc58c70c 100644
--- a/tracker-easy/point-extractor.cpp
+++ b/tracker-easy/point-extractor.cpp
@@ -78,20 +78,20 @@ namespace EasyTracker
         }
 
         // Contours detection
-        std::vector<std::vector<cv::Point> > contours;
-        cv::findContours(iFrameGray, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
+        iContours.clear();
+        cv::findContours(iFrameGray, iContours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
     
         // Workout which countours are valid points
-        for (size_t i = 0; i < contours.size(); i++)
+        for (size_t i = 0; i < iContours.size(); i++)
         {
             if (aPreview)
             {
-                cv::drawContours(*aPreview, contours, (int)i, CV_RGB(255, 0, 0), 2);
+                cv::drawContours(*aPreview, iContours, (int)i, CV_RGB(255, 0, 0), 2);
             }
         
 
             cv::Rect bBox;
-            bBox = cv::boundingRect(contours[i]);
+            bBox = cv::boundingRect(iContours[i]);
        
             // Make sure bounding box matches our criteria
             if (bBox.width >= s.min_point_size
@@ -116,7 +116,7 @@ namespace EasyTracker
         // Typically noise comming from zippers and metal parts on your clothing.
         // With a cap tracker it also successfully discards noise from glasses.
         // However it may not work as good with a clip user wearing glasses.
-        while (aPoints.size() > 3) // Until we have no more than three points
+        while (aPoints.size() > KPointCount) // Until we have no more than three points
         {
             int maxY = 0;
             size_t index = -1;
diff --git a/tracker-easy/point-extractor.h b/tracker-easy/point-extractor.h
index 7827bb30..3c65b193 100644
--- a/tracker-easy/point-extractor.h
+++ b/tracker-easy/point-extractor.h
@@ -34,6 +34,9 @@ namespace EasyTracker
         cv::Mat iFrameChannelSizeOne;
         // Our frame with a single 8 bits channel
         cv::Mat iFrameGray;
+        //
+        std::vector<std::vector<cv::Point> > iContours;
+
     };
 
 }
diff --git a/tracker-easy/tracker-easy.cpp b/tracker-easy/tracker-easy.cpp
index 4afb928a..b15db44e 100644
--- a/tracker-easy/tracker-easy.cpp
+++ b/tracker-easy/tracker-easy.cpp
@@ -122,197 +122,208 @@ namespace EasyTracker
         iDistCoeffsMatrix.at<double>(7, 0) = iCameraInfo.radialDistortionSixthOrder; // Radial sixth order
     }
 
-
-    void Tracker::run()
+    ///
+    ///
+    ///
+    void Tracker::ProcessFrame()
     {
-        maybe_reopen_camera();
+        // Create OpenCV matrix from our frame
+        // TODO: Assert channel size is one or two
+        iMatFrame = cv::Mat(iFrame.height, iFrame.width, CV_MAKETYPE((iFrame.channelSize == 2 ? CV_16U : CV_8U), iFrame.channels), iFrame.data, iFrame.stride);
+        iFrameCount++;
 
-        iFpsTimer.start();
+        bool doPreview = check_is_visible();
+        if (doPreview)
+        {
+            iPreview = iMatFrame;
+        }
+
+        iPoints.clear();
+        iPointExtractor.ExtractPoints(iMatFrame, (doPreview ? &iPreview.iFrameRgb : nullptr), iPoints);
+
+        const bool success = iPoints.size() >= KPointCount;
+
+        int topPointIndex = -1;
 
-        while (!isInterruptionRequested())
         {
-            iTimer.start();
+            QMutexLocker l(&center_lock);
 
-            bool new_frame = false;
+            if (success)
             {
-                QMutexLocker l(&camera_mtx);
+                ever_success.store(true, std::memory_order_relaxed);
 
-                if (camera)
+                // Solve P3P problem with OpenCV
+
+                //Bitmap origin is top left
+                iTrackedPoints.clear();
+                // Tracked points must match the order of the object model points.
+                // Find top most point, that's the one with min Y as we assume our guy's head is not up side down
+                int minY = std::numeric_limits<int>::max();
+                for (int i = 0; i < 3; i++)
                 {
-                    std::tie(iFrame, new_frame) = camera->get_frame();
+                    if (iPoints[i].y < minY)
+                    {
+                        minY = iPoints[i].y;
+                        topPointIndex = i;
+                    }
                 }
-                    
-            }
 
-            if (new_frame)
-            {
-                // Create OpenCV matrix from our frame
-                // TODO: Assert channel size is one or two
-                iMatFrame = cv::Mat(iFrame.height, iFrame.width, CV_MAKETYPE((iFrame.channelSize == 2 ? CV_16U : CV_8U), iFrame.channels), iFrame.data, iFrame.stride);
-                iFrameCount++;
+                int rightPointIndex = -1;
+                int maxX = 0;
+
+                // Find right most point 
+                for (int i = 0; i < 3; i++)
+                {
+                    // Excluding top most point
+                    if (i != topPointIndex && iPoints[i].x > maxX)
+                    {
+                        maxX = iPoints[i].x;
+                        rightPointIndex = i;
+                    }
+                }
 
-                const bool preview_visible = check_is_visible();
-                if (preview_visible)
+                // Find left most point
+                int leftPointIndex = -1;
+                for (int i = 0; i < 3; i++)
                 {
-                    iPreview = iMatFrame;
+                    // Excluding top most point
+                    if (i != topPointIndex && i != rightPointIndex)
+                    {
+                        leftPointIndex = i;
+                        break;
+                    }
                 }
 
-                iPoints.clear();
-                iPointExtractor.ExtractPoints(iMatFrame, (preview_visible ? &iPreview.iFrameRgb : nullptr), iPoints);                
+                //
+                iTrackedPoints.push_back(iPoints[rightPointIndex]);
+                iTrackedPoints.push_back(iPoints[leftPointIndex]);
+                iTrackedPoints.push_back(iPoints[topPointIndex]);
 
-                const bool success = iPoints.size() >= KPointCount;
+                dbgout << "Object: " << iModel << "\n";
+                dbgout << "Points: " << iTrackedPoints << "\n";
 
-                int topPointIndex = -1;
 
-                {
-                    QMutexLocker l(&center_lock);
+                // TODO: try SOLVEPNP_AP3P too, make it a settings option?
+                iAngles.clear();
+                iBestSolutionIndex = -1;
+                int solutionCount = cv::solveP3P(iModel, iTrackedPoints, iCameraMatrix, iDistCoeffsMatrix, iRotations, iTranslations, cv::SOLVEPNP_P3P);
 
-                    if (success)
+                if (solutionCount > 0)
+                {
+                    dbgout << "Solution count: " << solutionCount << "\n";
+                    int minPitch = std::numeric_limits<int>::max();
+                    // Find the solution we want amongst all possible ones
+                    for (int i = 0; i < solutionCount; i++)
                     {
-                        ever_success.store(true, std::memory_order_relaxed);
-
-                        // Solve P3P problem with OpenCV
-                    
-                        //Bitmap origin is top left
-                        iTrackedPoints.clear();
-                        // Tracked points must match the order of the object model points.
-                        // Find top most point, that's the one with min Y as we assume our guy's head is not up side down
-                        int minY = std::numeric_limits<int>::max();
-                        for (int i = 0; i < 3; i++)
+                        dbgout << "Translation:\n";
+                        dbgout << iTranslations.at(i);
+                        dbgout << "\n";
+                        dbgout << "Rotation:\n";
+                        //dbgout << rvecs.at(i);
+                        cv::Mat rotationCameraMatrix;
+                        cv::Rodrigues(iRotations[i], rotationCameraMatrix);
+                        cv::Vec3d angles;
+                        getEulerAngles(rotationCameraMatrix, angles);
+                        iAngles.push_back(angles);
+
+                        // Check if pitch is closest to zero
+                        int absolutePitch = std::abs(angles[0]);
+                        if (minPitch > absolutePitch)
                         {
-                            if (iPoints[i].y < minY)
-                            {
-                                minY = iPoints[i].y;
-                                topPointIndex = i;
-                            }
+                            // The solution with pitch closest to zero is the one we want
+                            minPitch = absolutePitch;
+                            iBestSolutionIndex = i;
                         }
 
-                        int rightPointIndex = -1;
-                        int maxX = 0;
+                        dbgout << angles;
+                        dbgout << "\n";
+                    }
 
-                        // Find right most point 
-                        for (int i = 0; i < 3; i++)
-                        {
-                            // Excluding top most point
-                            if (i != topPointIndex && iPoints[i].x > maxX)
-                            {
-                                maxX = iPoints[i].x;
-                                rightPointIndex = i;
-                            }
-                        }
+                    dbgout << "\n";
+                }
+            }
 
-                        // Find left most point
-                        int leftPointIndex = -1;
-                        for (int i = 0; i < 3; i++)
-                        {
-                            // Excluding top most point
-                            if (i != topPointIndex && i != rightPointIndex)
-                            {
-                                leftPointIndex = i;
-                                break;
-                            }
-                        }
+            // Send solution data back to main thread
+            QMutexLocker l2(&data_lock);
+            if (iBestSolutionIndex != -1)
+            {
+                iBestAngles = iAngles[iBestSolutionIndex];
+                iBestTranslation = iTranslations[iBestSolutionIndex];
+            }
+
+        }
 
-                        //
-                        iTrackedPoints.push_back(iPoints[rightPointIndex]);
-                        iTrackedPoints.push_back(iPoints[leftPointIndex]);
-                        iTrackedPoints.push_back(iPoints[topPointIndex]);
+        if (doPreview)
+        {
+            std::ostringstream ss;
+            ss << "FPS: " << iFps << "/" << iSkippedFps;
+            iPreview.DrawInfo(ss.str());
 
-                        dbgout << "Object: " << iModel << "\n";
-                        dbgout << "Points: " << iTrackedPoints << "\n";
+            //
+            if (topPointIndex != -1)
+            {
+                // Render a cross to indicate which point is the head
+                iPreview.DrawCross(iPoints[topPointIndex]);
+            }
 
+            // Show full size preview pop-up
+            if (iSettings.debug)
+            {
+                cv::imshow("Preview", iPreview.iFrameRgb);
+                cv::waitKey(1);
+            }
 
-                        // TODO: try SOLVEPNP_AP3P too, make it a settings option?
-                        iAngles.clear();
-                        iBestSolutionIndex = -1;
-                        int solutionCount = cv::solveP3P(iModel, iTrackedPoints, iCameraMatrix, iDistCoeffsMatrix, iRotations, iTranslations, cv::SOLVEPNP_P3P);
+            // Update preview widget
+            widget->update_image(iPreview.get_bitmap());
 
-                        if (solutionCount > 0)
-                        {
-                            dbgout << "Solution count: " << solutionCount << "\n";
-                            int minPitch = std::numeric_limits<int>::max();
-                            // Find the solution we want amongst all possible ones
-                            for (int i = 0; i < solutionCount; i++)
-                            {
-                                dbgout << "Translation:\n";
-                                dbgout << iTranslations.at(i);
-                                dbgout << "\n";
-                                dbgout << "Rotation:\n";
-                                //dbgout << rvecs.at(i);
-                                cv::Mat rotationCameraMatrix;
-                                cv::Rodrigues(iRotations[i], rotationCameraMatrix);
-                                cv::Vec3d angles;
-                                getEulerAngles(rotationCameraMatrix, angles);
-                                iAngles.push_back(angles);
-
-                                // Check if pitch is closest to zero
-                                int absolutePitch = std::abs(angles[0]);
-                                if (minPitch > absolutePitch)
-                                {
-                                    // The solution with pitch closest to zero is the one we want
-                                    minPitch = absolutePitch;
-                                    iBestSolutionIndex = i;
-                                }
-
-                                dbgout << angles;
-                                dbgout << "\n";
-                            }
-
-                            dbgout << "\n";
-                        }
-                    }
+            auto[w, h] = widget->preview_size();
+            if (w != preview_width || h != preview_height)
+            {
+                // Resize preivew if widget size has changed
+                preview_width = w; preview_height = h;
+                iPreview = Preview(w, h);
+            }
+        }
+        else
+        {
+            // No preview, destroy preview pop-up
+            if (iSettings.debug)
+            {
+                cv::destroyWindow("Preview");
+            }
+        }
 
-                    // Send solution data back to main thread
-                    QMutexLocker l2(&data_lock);
-                    if (iBestSolutionIndex != -1)
-                    {
-                        iBestAngles = iAngles[iBestSolutionIndex];
-                        iBestTranslation = iTranslations[iBestSolutionIndex];
-                    }
+        dbgout << "Frame time:" << iTimer.elapsed_seconds() << "\n";
 
-                }
+    }
 
-                if (preview_visible)
-                {
-                    std::ostringstream ss;
-                    ss << "FPS: " << iFps << "/" << iSkippedFps;
-                    iPreview.DrawInfo(ss.str());
+    ///
+    ///
+    ///
+    void Tracker::run()
+    {
+        maybe_reopen_camera();
 
-                    //
-                    if (topPointIndex != -1)
-                    {
-                        // Render a cross to indicate which point is the head
-                        iPreview.DrawCross(iPoints[topPointIndex]);
-                    }
+        iFpsTimer.start();
 
-                    // Show full size preview pop-up
-                    if (iSettings.debug)
-                    {
-                        cv::imshow("Preview", iPreview.iFrameRgb);
-                        cv::waitKey(1);
-                    }
+        while (!isInterruptionRequested())
+        {
+            iTimer.start();
 
-                    // Update preview widget
-                    widget->update_image(iPreview.get_bitmap());
+            bool new_frame = false;
+            {
+                QMutexLocker l(&camera_mtx);
 
-                    auto[w, h] = widget->preview_size();
-                    if (w != preview_width || h != preview_height)
-                    {
-                        // Resize preivew if widget size has changed
-                        preview_width = w; preview_height = h;
-                        iPreview = Preview(w, h);
-                    }
-                }
-                else
+                if (camera)
                 {
-                    // No preview, destroy preview pop-up
-                    if (iSettings.debug)
-                    {
-                        cv::destroyWindow("Preview");
-                    }                    
+                    std::tie(iFrame, new_frame) = camera->get_frame();
                 }
+                    
+            }
 
-                dbgout << "Frame time:" << iTimer.elapsed_seconds() << "\n";
+            if (new_frame)
+            {
+                ProcessFrame();
             }
             else
             {
diff --git a/tracker-easy/tracker-easy.h b/tracker-easy/tracker-easy.h
index 06551533..a8c8e6be 100644
--- a/tracker-easy/tracker-easy.h
+++ b/tracker-easy/tracker-easy.h
@@ -56,6 +56,7 @@ namespace EasyTracker
     private:
         void CreateModelFromSettings();
         void CreateCameraIntrinsicsMatrices();
+        void ProcessFrame();
 
         // From QThread
         void run() override;