From 0531dca1d9b3ccb0b92577a6dde321639fa406fe Mon Sep 17 00:00:00 2001
From: kalwalt <github@kalwaltart.it>
Date: Sat, 20 Apr 2024 20:11:54 +0200
Subject: [PATCH] further improves to Optical Flow routine

---
 .../WebARKitTracker.cpp                       | 25 ++++++++++++-------
 1 file changed, 16 insertions(+), 9 deletions(-)

diff --git a/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/WebARKitTracker.cpp b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/WebARKitTracker.cpp
index c77f217..cd7f32a 100644
--- a/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/WebARKitTracker.cpp
+++ b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/WebARKitTracker.cpp
@@ -192,27 +192,34 @@ class WebARKitTracker::WebARKitTrackerImpl {
 
         // use optical flow to track keypoints
         std::vector<float> err;
-        std::vector<uchar> status;
+        //std::vector<uchar> status;
+        std::vector<uchar> statusFirstPass, statusSecondPass;
         std::vector<cv::Point2f> currPts, goodPtsCurr, goodPtsPrev;
         bool valid;
-        calcOpticalFlowPyrLK(_prevPyramid, _pyramid, framePts, currPts, status, err, winSize, maxLevel, termcrit, 0,
+        calcOpticalFlowPyrLK(_prevPyramid, _pyramid, framePts, currPts, statusFirstPass, err, winSize, maxLevel, termcrit, 0,
                              0.001);
-        calcOpticalFlowPyrLK(_pyramid, _prevPyramid, currPts, framePts, status, err, winSize, maxLevel, termcrit, 0,
+        calcOpticalFlowPyrLK(_pyramid, _prevPyramid, currPts, framePts, statusSecondPass, err, winSize, maxLevel, termcrit, 0,
                              0.001);
         // calculate average variance
         double mean, avg_variance = 0.0;
         double sum = 0.0;
         double diff;
         std::vector<double> diffs;
-        for (size_t i = 0; i < framePts.size(); ++i) {
-            if (status[i]) {
-                goodPtsCurr.push_back(currPts[i]);
-                goodPtsPrev.push_back(framePts[i]);
+        int killed1 = 0;
 
-                diff = sqrt(pow(currPts[i].x - framePts[i].x, 2.0) + pow(currPts[i].y - framePts[i].y, 2.0));
+        for (auto j = 0; j != currPts.size(); ++j) {
+            if (!statusFirstPass[j] || !statusSecondPass[j]) {
+                statusFirstPass[j] = (uchar)0;
+                killed1++;
+                continue;
+            }
+
+            goodPtsCurr.push_back(currPts[j]);
+            goodPtsPrev.push_back(framePts[j]);
+
+            diff = sqrt(pow(currPts[j].x - framePts[j].x, 2.0) + pow(currPts[j].y - framePts[j].y, 2.0));
                 sum += diff;
                 diffs.push_back(diff);
-            }
         }
 
         mean = sum / diffs.size();