Introduce a new PnP method

src/trackers/image-tracker/image-tracker-utils.ts

@@ -32,6 +32,7 @@ import { ReferenceImageWithMedia } from './reference-image';
 import { Utils } from '../../utils/utils';
 import { IllegalOperationError, IllegalArgumentError, NumericalError } from '../../utils/errors';
 import { NIS_SIZE, TRACK_GRID_GRANULARITY } from './settings';
+import { find6DoFHomography, solvePlanarPnPRansacOptions } from '../../geometry/pnp';
 
 /*
 
@@ -227,6 +228,93 @@ export class ImageTrackerUtils
 
     /**
      * Given n > 0 pairs of keypoints in NDC as a 2 x 2n [ src | dest ] matrix,
+     * find a 6 DoF perspective warp (homography) from src to dest in NDC
+     * @param cameraIntrinsics 3x3 camera intrinsics
+     * @param points compiled pairs of keypoints in NDC
+     * @param options to be passed to pnp
+     * @returns a pair [ 3x3 transformation matrix, quality score ]
+     */
+    static find6DoFHomographyNDC(cameraIntrinsics: SpeedyMatrix, points: SpeedyMatrix, options: solvePlanarPnPRansacOptions): SpeedyPromise<[SpeedyMatrix,number]>
+    {
+        // too few data points?
+        const n = points.columns / 2;
+        if(n < 4) {
+            return Speedy.Promise.reject(
+                new IllegalArgumentError(`Too few data points to compute a perspective warp`)
+            );
+        }
+
+        // compute a homography
+        const src = points.block(0, 1, 0, n-1);
+        const dest = points.block(0, 1, n, 2*n-1);
+        const homography = find6DoFHomography(src, dest, cameraIntrinsics, options);
+        //console.log('homography',homography.toString(), src.toString(), dest.toString());
+
+        // quit without refinement (test)
+        // we use a coarse estimate of the camera intrinsics
+        //return Speedy.Promise.resolve([homography, 0]);
+
+        const mask = Speedy.Matrix.Zeros(1, n);
+        const intermediate = Speedy.Matrix.Zeros(2, n);
+
+        // refine the result of find6DoFHomography() with DLT + RANSAC
+        return Speedy.Matrix.applyPerspectiveTransform(intermediate, src, homography)
+        .then(intermediate =>
+            Speedy.Matrix.findHomography(
+                Speedy.Matrix.Zeros(3),
+                intermediate,
+                dest,
+                {
+                    method: 'pransac',
+                    numberOfHypotheses: 512, // XXX we can reduce this number without compromising quality
+                    bundleSize: 128, // maybe make it a parameter in case we need an extra performance boost?
+                    reprojectionError: options.reprojectionError,
+                    mask,
+                }
+            )
+        )
+        .then(adjustment => adjustment.setTo(adjustment.times(homography)))
+        .then(newHomography => {
+
+            // count inliers
+            let m = 0;
+            const inliers = mask.read();
+            for(let i = 0; i < n; i++)
+                m += inliers[i];
+
+            /*
+            // count and collect inliers
+            let m = 0;
+            const _mask = mask.read(), _src = src.read(), _dest = dest.read();
+            const _isrc = new Array<number>(2*n), _idest = new Array<number>(2*n);
+            for(let i = 0; i < n; i++) {
+                if(_mask[i]) {
+                    const j = m++;
+                    _isrc[2*j] = _src[2*i];
+                    _isrc[2*j+1] = _src[2*i+1];
+                    _idest[2*j] = _dest[2*i];
+                    _idest[2*j+1] = _dest[2*i+1];
+                }
+            }
+            _isrc.length = _idest.length = 2*m;
+
+            // refine homography
+            if(m > 0) {
+                const isrc = Speedy.Matrix(2, m, _isrc);
+                const idest = Speedy.Matrix(2, m, _idest);
+                newHomography = refineHomography(newHomography, isrc, idest);
+            }
+            */
+
+            // done!
+            const score = m / n;
+            return [newHomography, score];
+
+        });
+    }
+
+    /**
+     * Given n > 0 pairs of keypoints in NDC as a 2 x 2n [ src | dest ] matrix,
      * find a perspective warp (homography) from src to dest in NDC
      * @param points compiled pairs of keypoints in NDC
      * @param options to be passed to speedy-vision

src/trackers/image-tracker/settings.ts

@@ -138,7 +138,7 @@ export const TRACK_RANSAC_REPROJECTIONERROR_NDC = TRACK_RANSAC_REPROJECTIONERROR
 export const TRACK_GRID_GRANULARITY = 10; //20; // the value of N
 
 /** Used to identify the best maches */
-export const TRACK_MATCH_RATIO = 0.75; // usually a value in [0.6, 0.8] - low values => strict tracking
+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;

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

@@ -67,6 +67,9 @@ const USE_TURBO = true;
 /** Number of PBOs; meaningful only when using turbo */
 const NUMBER_OF_PBOS = 2;
 
+/** Motion model */
+const NO_MOTION = Speedy.Matrix.Eye(3);
+
 
 
 /**
@@ -298,22 +301,25 @@ export class ImageTrackerTrackingState extends ImageTrackerState
             // find motion models
             const points = ImageTrackerUtils.compilePairsOfKeypointsNDC(pairs);
             return Speedy.Promise.all<SpeedyMatrixExpr>([
-                this._findAffineMotionNDC(points),
-                this._findPerspectiveMotionNDC(points)
+                //this._findAffineMotionNDC(points),
+                //this._findPerspectiveMotionNDC(points),
+                this._find6DoFPerspectiveMotionNDC(points),
+                Speedy.Promise.resolve(NO_MOTION),
             ]);
 
         })
-        .then(([affineMotion, perspectiveMotion]) => {
+        .then(([warpMotion, poseMotion]) => {
 
             const lowPower = (Settings.powerPreference == 'low-power');
             const delay = NUMBER_OF_PBOS * (!lowPower ? 2 : 1);
 
             // update warp homography
             if(!USE_TURBO || this._counter % delay == 1) // skip the first frame (PBOs)
-                this._warpHomography.setToSync(affineMotion.times(this._warpHomography));
+                this._warpHomography.setToSync(warpMotion.times(this._warpHomography));
 
             // update pose homography
-            this._poseHomography.setToSync(perspectiveMotion.times(this._warpHomography));
+            //poseMotion = warpMotion; // commented: reduce jitter, increase delay
+            this._poseHomography.setToSync(poseMotion.times(this._warpHomography));
             if(Number.isNaN(this._poseHomography.at(0,0)))
                 throw new NumericalError('Bad homography'); // normalize? 1 / h33
 
@@ -324,8 +330,8 @@ export class ImageTrackerTrackingState extends ImageTrackerState
             // test
             console.log("POSE ", this._poseHomography.toString());
             console.log("WARP ", this._warpHomography.toString());
-            console.log("AMOT ", Speedy.Matrix(affineMotion).toString());
-            console.log("PMOT ", Speedy.Matrix(perspectiveMotion).toString());
+            console.log("AMOT ", Speedy.Matrix(warpMotion).toString());
+            console.log("PMOT ", Speedy.Matrix(poseMotion).toString());
             */
 
             // We transform the keypoints of the reference image to NDC as a
@@ -455,6 +461,8 @@ export class ImageTrackerTrackingState extends ImageTrackerState
 
         Note: work with a 6 DoF perspective transform instead of 8.
 
+        Edit: now we have find6DoFHomography()
+
         */
 
         return ImageTrackerUtils.findAffineWarpNDC(points, {
@@ -498,7 +506,8 @@ export class ImageTrackerTrackingState extends ImageTrackerState
             numberOfHypotheses: 512*2,
             bundleSize: 128,//128*4,
             mask: undefined // score is not needed
-        }).then(([ warp, score ]) => {
+        })
+        .then(([ warp, score ]) => {
 
             const scale = TRACK_RECTIFIED_SCALE;
             const aspectRatio = ImageTrackerUtils.bestFitAspectRatioNDC(this.screenSize, this._referenceImage!);
@@ -520,6 +529,43 @@ export class ImageTrackerTrackingState extends ImageTrackerState
     }
 
     /**
+     * Find a 6 DoF perspective motion model in NDC between pairs of keypoints in NDC
+     * given as a 2 x 2n [ src | dest ] matrix
+     * @param points compiled pairs of keypoints in NDC
+     * @returns a promise that resolves to a 3x3 warp in NDC that maps source to destination
+     */
+    private _find6DoFPerspectiveMotionNDC(points: SpeedyMatrix): SpeedyPromise<SpeedyMatrixExpr>
+    {
+        const K = this._camera.intrinsicsMatrix();
+
+        return ImageTrackerUtils.find6DoFHomographyNDC(K, points, {
+            reprojectionError: TRACK_RANSAC_REPROJECTIONERROR_NDC,
+            numberOfHypotheses: 100,
+            //mask: undefined // score is not needed
+        })
+        .then(([ warp, score ]) => {
+
+            const scale = TRACK_RECTIFIED_SCALE;
+            const aspectRatio = ImageTrackerUtils.bestFitAspectRatioNDC(this.screenSize, this._referenceImage!);
+            const shrink = ImageTrackerUtils.bestFitScaleNDC(aspectRatio, scale);
+            const grow = ImageTrackerUtils.inverseBestFitScaleNDC(aspectRatio, scale);
+            const scaledWarp = grow.times(warp).times(shrink);
+
+            //const distort = this._poseHomography;
+            const distort = this._warpHomography;
+            const undistort = distort.inverse();
+            const correctedWarp = distort.times(scaledWarp).times(undistort);
+
+            return correctedWarp;
+
+        }).catch(err => {
+
+            throw new TrackingError(`Can't find a perspective motion`, err);
+
+        });
+    }
+
+    /**
      * Find matching pairs of two sets of keypoints matched via brute force
      * @param srcKeypoints source (database)
      * @param destKeypoints destination