Tune the algorithm to increase the stability of the tracking

src/geometry/pnp.ts

@@ -902,6 +902,7 @@ export function solvePlanarPnPRansac(referencePoints: SpeedyMatrix, observedPoin
         // We're just using n = 4. We'll refine the homography later.
         const newPose = pose;
         const newError = error;
+        //const newError = n - count;
 
         // Not the best model? Discard it
         if(newError > bestError)

src/trackers/image-tracker/settings.ts

@@ -129,16 +129,25 @@ export const TRACK_CLIPPING_BORDER = TRACK_RECTIFIED_BORDER * 1.20; //1.25; //1.
 export const TRACK_RECTIFIED_SCALE = 1 - 2 * TRACK_RECTIFIED_BORDER;
 
 /** Reprojection error, in NIS pixels, used when estimating a motion model (tracking state) */
-export const TRACK_RANSAC_REPROJECTIONERROR_NIS = (NIS_SIZE * 0.0125) | 0;
+export const TRACK_RANSAC_REPROJECTIONERROR_NIS = (NIS_SIZE * 0.0125 * 0.5) | 0;
 
 /** Reprojection error, in NDC, used when estimating a motion model (tracking state) */
 export const TRACK_RANSAC_REPROJECTIONERROR_NDC = TRACK_RANSAC_REPROJECTIONERROR_NIS / (NIS_SIZE / 2);
 
 /** We use a N x N grid to spatially distribute the keypoints in order to compute a better homography */
-export const TRACK_GRID_GRANULARITY = 10; //20; // the value of N
+export const TRACK_GRID_GRANULARITY = 15; //20; //10; // the value of N
 
 /** Used to identify the best maches */
 export const TRACK_MATCH_RATIO = 0.7; // usually a value in [0.6, 0.8] - low values => strict tracking
 
 /** Number of consecutive frames in which we tolerate a  "target lost" situation */
-export const TRACK_LOST_TOLERANCE = 15;
+export const TRACK_LOST_TOLERANCE = 15;
+
+/** Interpolation filter: interpolation factor */
+export const TRACK_FILTER_ALPHA = 0.125;
+
+/** Interpolation filter: correction strength for noisy corners */
+export const TRACK_FILTER_BETA = 1;
+
+/** Interpolation filter: translation factor */
+export const TRACK_FILTER_TAU = 0.25;

src/trackers/image-tracker/states/tracking.ts

@@ -57,6 +57,7 @@ import {
     SUBPIXEL_GAUSSIAN_KSIZE, SUBPIXEL_GAUSSIAN_SIGMA,
     TRACK_HARRIS_QUALITY, TRACK_DETECTOR_CAPACITY, TRACK_MAX_KEYPOINTS,
     TRACK_RANSAC_REPROJECTIONERROR_NDC, TRACK_MATCH_RATIO,
+    TRACK_FILTER_ALPHA, TRACK_FILTER_BETA, TRACK_FILTER_TAU,
     NIGHTVISION_QUALITY, SUBPIXEL_METHOD,
 } from '../settings';
 import { Settings } from '../../../core/settings';
@@ -298,17 +299,12 @@ export class ImageTrackerTrackingState extends ImageTrackerState
             if(pairs.length < TRACK_MIN_MATCHES)
                 throw new TrackingError('Not enough data points to continue the tracking');
 
-            // find motion models
+            // find motion model
             const points = ImageTrackerUtils.compilePairsOfKeypointsNDC(pairs);
-            return Speedy.Promise.all<SpeedyMatrixExpr>([
-                //this._findAffineMotionNDC(points),
-                //this._findPerspectiveMotionNDC(points),
-                this._find6DoFPerspectiveMotionNDC(points),
-                Speedy.Promise.resolve(NO_MOTION),
-            ]);
+            return this._find6DoFPerspectiveMotionNDC(points);
 
         })
-        .then(([warpMotion, poseMotion]) => {
+        .then(warpMotion => {
 
             const lowPower = (Settings.powerPreference == 'low-power');
             const delay = NUMBER_OF_PBOS * (!lowPower ? 2 : 1);
@@ -317,15 +313,26 @@ export class ImageTrackerTrackingState extends ImageTrackerState
             if(!USE_TURBO || this._counter % delay == 1) // skip the first frame (PBOs)
                 this._warpHomography.setToSync(warpMotion.times(this._warpHomography));
 
+            // update counter
+            this._counter = (this._counter + 1) % delay;
+
+            // apply filter
+            return ImageTrackerUtils.interpolateHomographies(
+                this._poseHomography,
+                Speedy.Matrix(warpMotion.times(this._warpHomography)),
+                TRACK_FILTER_ALPHA,
+                TRACK_FILTER_BETA,
+                TRACK_FILTER_TAU
+            );
+
+        })
+        .then(filteredHomography => {
+
             // update pose homography
-            //poseMotion = warpMotion; // commented: reduce jitter, increase delay
-            this._poseHomography.setToSync(poseMotion.times(this._warpHomography));
+            this._poseHomography.setToSync(filteredHomography);
             if(Number.isNaN(this._poseHomography.at(0,0)))
                 throw new NumericalError('Bad homography'); // normalize? 1 / h33
 
-            // update counter
-            this._counter = (this._counter + 1) % delay;
-
             /*
             // test
             console.log("POSE ", this._poseHomography.toString());
@@ -344,6 +351,7 @@ export class ImageTrackerTrackingState extends ImageTrackerState
 
             // update camera model
             return this._camera.update(homography);
+
         })
         .then(() => {