Make find6DofHomography() return a promise that resolves to a refined homography

src/geometry/pnp.ts

 
 import Speedy from 'speedy-vision';
 import { SpeedyMatrix } from 'speedy-vision/types/core/speedy-matrix';
+import { SpeedyPromise } from 'speedy-vision/types/core/speedy-promise';
 import { Utils, Nullable } from '../utils/utils';
 import { IllegalArgumentError } from '../utils/errors';
 
     mask?: Nullable<SpeedyMatrix>;
 }
 
+/** Options for find6DoFHomography() */
+export interface find6DoFHomographyOptions extends solvePlanarPnPRansacOptions
+{
+    /** quality of the refinement: a value in [0,1] */
+    refinementQuality?: number;
+}
+
 /** Default options for the RANSAC scheme */
 const DEFAULT_RANSAC_OPTIONS: Required<solvePlanarPnPRansacOptions> = {
-    numberOfHypotheses: 512,
+    numberOfHypotheses: 100,
     reprojectionError: 3,
     acceptablePercentageOfInliers: Number.POSITIVE_INFINITY, // never exit early
     mask: null,
 };
 
+/** Default options for find6DoFHomography() */
+const DEFAULT_FIND6DOFHOMOGRAPHY_OPTIONS: Required<find6DoFHomographyOptions> = Object.assign(
+    {}, DEFAULT_RANSAC_OPTIONS, {
+        refinementQuality: 1.0
+    }
+);
+
 
 
 
  * @param referencePoints 2 x n matrix with reference points in a plane parallel to the image plane (Z = z0 > 0)
  * @param observedPoints 2 x n matrix with the observed points corresponding to the reference points
  * @param cameraIntrinsics 3x3 matrix
- * @param options RANSAC options
+ * @param options options
  * @returns 3x3 homography matrix or a matrix of NaNs if no suitable homography is found
  */
-export function find6DoFHomography(referencePoints: SpeedyMatrix, observedPoints: SpeedyMatrix, cameraIntrinsics: SpeedyMatrix, options: solvePlanarPnPRansacOptions = {}): SpeedyMatrix
+export function find6DoFHomography(referencePoints: SpeedyMatrix, observedPoints: SpeedyMatrix, cameraIntrinsics: SpeedyMatrix, options: find6DoFHomographyOptions = {}): SpeedyPromise<SpeedyMatrix>
 {
-    const extrinsics = solvePlanarPnPRansac(referencePoints, observedPoints, cameraIntrinsics, options);
-    const homography = buildHomography(cameraIntrinsics, extrinsics);
-    const entries = Array.from<number>(homography);
+    const n = referencePoints.columns;
+    const settings = Object.assign({}, DEFAULT_FIND6DOFHOMOGRAPHY_OPTIONS, options);
+    const refinementQuality = Math.max(0, Math.min(settings.refinementQuality, 1));
+    const reprojectionError = settings.reprojectionError;
+    const mask = settings.mask;
+
+    // validate
+    if(n < 4)
+        return Speedy.Promise.reject(new IllegalArgumentError('find6DofHomography() requires at least 4 points'));
+    else if(referencePoints.columns != observedPoints.columns || referencePoints.rows != 2 || observedPoints.rows != 2)
+        return Speedy.Promise.reject(new IllegalArgumentError('Bad input'));
+    else if(cameraIntrinsics.columns != 3 || cameraIntrinsics.rows != 3)
+        return Speedy.Promise.reject(new IllegalArgumentError('Bad intrinsics'));
+
+    // find a pose. 6DoF: 3 for rotation + 3 for translation
+    const pose = solvePlanarPnPRansac(referencePoints, observedPoints, cameraIntrinsics, options);
 
     /*
     DEBUG && print({
         find6DoFHomography: {
-            R: extrinsics.block(0, 2, 0, 2).toString(),
-            t: extrinsics.block(0, 2, 3, 3).toString(),
-            //q: new Quaternion()._fromRotationMatrix(extrinsics.block(0, 2, 0, 2)).toString(),
+            R: pose.block(0, 2, 0, 2).toString(),
+            t: pose.block(0, 2, 3, 3).toString(),
+            //q: new Quaternion()._fromRotationMatrix(pose.block(0, 2, 0, 2)).toString(),
         }
     });
     */
 
-    return Speedy.Matrix(3, 3, entries);
+    // build an initial homography
+    const hom = buildHomography(cameraIntrinsics, pose);
+    const entries = Array.from<number>(hom);
+    const homography = Speedy.Matrix(3, 3, entries);
+
+    // quit without refinement (for testing purposes)
+    // we can expect a coarse estimate of the camera intrinsics
+    if(refinementQuality == 0 || Number.isNaN(entries[0]))
+        return Speedy.Promise.resolve(homography);
+
+    // refine the homography with DLT + RANSAC
+    const src = referencePoints, dest = observedPoints;
+    const intermediate = Speedy.Matrix.Zeros(2, n);
+
+    return Speedy.Matrix.applyPerspectiveTransform(intermediate, src, homography)
+    .then(intermediate =>
+        Speedy.Matrix.findHomography(
+            Speedy.Matrix.Zeros(3),
+            intermediate,
+            dest,
+            {
+                method: 'pransac',
+                numberOfHypotheses: Math.ceil(512 * refinementQuality), // XXX we can reduce this number without compromising quality
+                bundleSize: Math.ceil(128 * refinementQuality),
+                reprojectionError: reprojectionError,
+                mask,
+            }
+        )
+    )
+    .then(adjustment =>
+        adjustment.setTo(adjustment.times(homography))
+    );
 }
 
 /*

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

 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';
+import { find6DoFHomography, find6DoFHomographyOptions } from '../../geometry/pnp';
 
 /*
 
      * 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
+     * @param options to be passed to find6DofHomography
      * @returns a pair [ 3x3 transformation matrix, quality score ]
      */
-    static find6DoFHomographyNDC(cameraIntrinsics: SpeedyMatrix, points: SpeedyMatrix, options: solvePlanarPnPRansacOptions): SpeedyPromise<[SpeedyMatrix,number]>
+    static find6DoFHomographyNDC(cameraIntrinsics: SpeedyMatrix, points: SpeedyMatrix, options: find6DoFHomographyOptions): SpeedyPromise<[SpeedyMatrix,number]>
     {
         // too few data points?
         const n = points.columns / 2;
         // 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 => {
+        const mask = options.mask || Speedy.Matrix.Zeros(1, n);
+
+        return find6DoFHomography(
+            src,
+            dest,
+            cameraIntrinsics,
+            Object.assign({ mask }, options)
+        ).then(homography => {
+
+            // check if this is a valid homography
+            const a00 = homography.at(0,0);
+            if(Number.isNaN(a00))
+                throw new NumericalError(`Can't compute a perspective warp: bad keypoints`);
 
             // count inliers
             let m = 0;
             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];
+            return [homography, score];
 
         });
     }
         // compute a homography
         const src = points.block(0, 1, 0, n-1);
         const dest = points.block(0, 1, n, 2*n-1);
-        const mask = Speedy.Matrix.Zeros(1, n);
+        const mask = ((options as any).mask as SpeedyMatrix | null | undefined) || Speedy.Matrix.Zeros(1, n);
 
         return Speedy.Matrix.findHomography(
             Speedy.Matrix.Zeros(3),
             if(Number.isNaN(a00))
                 throw new NumericalError(`Can't compute a perspective warp: bad keypoints`);
 
-            // count the number of inliers
+            // count inliers
+            let m = 0;
             const inliers = mask.read();
-            let inlierCount = 0;
-            for(let i = inliers.length - 1; i >= 0; i--)
-                inlierCount += inliers[i];
-            const score = inlierCount / inliers.length;
+            for(let i = 0; i < n; i++)
+                m += inliers[i];
 
             // done!
-            return [ homography, score ];
+            const score = m / n;
+            return [homography, score];
 
         });
     }
         const model = Speedy.Matrix.Eye(3);
         const src = points.block(0, 1, 0, n-1);
         const dest = points.block(0, 1, n, 2*n-1);
-        const mask = Speedy.Matrix.Zeros(1, n);
+        const mask = ((options as any).mask as SpeedyMatrix | null | undefined) || Speedy.Matrix.Zeros(1, n);
 
         return Speedy.Matrix.findAffineTransform(
             model.block(0, 1, 0, 2), // 2x3 submatrix
             if(Number.isNaN(a00))
                 throw new NumericalError(`Can't compute an affine warp: bad keypoints`);
 
-            // count the number of inliers
+            // count inliers
+            let m = 0;
             const inliers = mask.read();
-            let inlierCount = 0;
-            for(let i = inliers.length - 1; i >= 0; i--)
-                inlierCount += inliers[i];
-            const score = inlierCount / inliers.length;
+            for(let i = 0; i < n; i++)
+                m += inliers[i];
 
             // done!
-            return [ model, score ];
+            const score = m / n;
+            return [model, score];
 
         });
     }