Introduce the PoseFilter. Use Quaternions to improve the smoothing of rotations

src/geometry/camera-model.ts

@@ -28,6 +28,7 @@ import { SpeedyVector2 } from 'speedy-vision/types/core/speedy-vector';
 import { SpeedyPromise } from 'speedy-vision/types/core/speedy-promise';
 import { Nullable, Utils } from '../utils/utils';
 import { Settings } from '../core/settings';
+import { PoseFilter } from './pose-filter';
 import { IllegalOperationError, IllegalArgumentError } from '../utils/errors';
 
 /** A guess of the horizontal field-of-view of a typical camera, in degrees */
@@ -36,12 +37,6 @@ const HFOV_GUESS = 60; // https://developer.apple.com/library/archive/documentat
 /** Number of iterations used to refine the estimated pose */
 const POSE_ITERATIONS = 30;
 
-/** Number of samples used in the rotation filter */
-const ROTATION_FILTER_SAMPLES = 10;
-
-/** Number of samples used in the translation filter */
-const TRANSLATION_FILTER_SAMPLES = 5;
-
 /** Convert degrees to radians */
 const DEG2RAD = 0.017453292519943295; // pi / 180
 
@@ -83,11 +78,8 @@ export class CameraModel
     /** extrinsics matrix, in column-major format */
     private _extrinsics: number[];
 
-    /** filter: samples of partial rotation matrix [ r1 | r2 ] */
-    private _partialRotationBuffer: number[][];
-
-    /** filter: samples of translation vector t */
-    private _translationBuffer: number[][];
+    /** smoothing filter */
+    private _filter: PoseFilter;
 
 
 
@@ -100,8 +92,7 @@ export class CameraModel
         this._matrix = Speedy.Matrix.Eye(3, 4);
         this._intrinsics = [1,0,0,0,1,0,0,0,1]; // 3x3 identity matrix
         this._extrinsics = [1,0,0,0,1,0,0,0,1,0,0,0]; // 3x4 matrix [ R | t ] = [ I | 0 ] no rotation & no translation
-        this._partialRotationBuffer = [];
-        this._translationBuffer = [];
+        this._filter = new PoseFilter();
     }
 
     /**
@@ -177,12 +168,13 @@ export class CameraModel
 
         // estimate the pose
         const pose = this._estimatePose(homography);
-        this._extrinsics = pose.read();
+        if(this._filter.feed(pose))
+            this._extrinsics = this._filter.run().read();
 
         // compute the camera matrix
         const C = this.denormalizer();
         const K = Speedy.Matrix(3, 3, this._intrinsics);
-        const E = pose; //Speedy.Matrix(3, 4, this._extrinsics);
+        const E = Speedy.Matrix(3, 4, this._extrinsics);
         this._matrix.setToSync(K.times(E).times(C));
         //console.log("intrinsics -----------", K.toString());
         //console.log("matrix ----------------",this._matrix.toString());
@@ -320,9 +312,8 @@ export class CameraModel
         this._extrinsics.fill(0);
         this._extrinsics[0] = this._extrinsics[4] = this._extrinsics[8] = 1;
 
-        // reset filters
-        this._partialRotationBuffer.length = 0;
-        this._translationBuffer.length = 0;
+        // reset filter
+        this._filter.reset();
     }
 
     /**
@@ -755,72 +746,15 @@ export class CameraModel
     }
 
     /**
-     * Apply a smoothing filter to the partial pose
-     * @param partialPose 3x3 [ r1 | r2 | t ]
-     * @returns filtered partial pose
-     */
-    private _filterPartialPose(partialPose: SpeedyMatrix): SpeedyMatrix
-    {
-        const avg: number[] = new Array(9).fill(0);
-        const entries = partialPose.read();
-        const rotationBlock = entries.slice(0, 6);
-        const translationBlock = entries.slice(6, 9);
-
-        // how many samples should we store, at most?
-        const div = (Settings.powerPreference == 'low-power') ? 1.5 : 1; // low-power ~ half the fps
-        const N = Math.ceil(ROTATION_FILTER_SAMPLES / div);
-        const M = Math.ceil(TRANSLATION_FILTER_SAMPLES / div);
-
-        // is it a valid partial pose?
-        if(!Number.isNaN(entries[0])) {
-            // store samples
-            this._partialRotationBuffer.unshift(rotationBlock);
-            if(this._partialRotationBuffer.length > N)
-                this._partialRotationBuffer.length = N;
-
-            this._translationBuffer.unshift(translationBlock);
-            if(this._translationBuffer.length > M)
-                this._translationBuffer.length = M;
-        }
-        else if(this._partialRotationBuffer.length == 0) {
-            // invalid pose, no samples
-            return Speedy.Matrix.Eye(3);
-        }
-
-        // average *nearby* rotations
-        const n = this._partialRotationBuffer.length;
-        for(let i = 0; i < n; i++) {
-            const r = this._partialRotationBuffer[i];
-            for(let j = 0; j < 6; j++)
-                avg[j] += r[j] / n;
-        }
-        const r = this._refineRotation(avg);
-
-        // average translations
-        const m = this._translationBuffer.length;
-        for(let i = 0; i < m; i++) {
-            const t = this._translationBuffer[i];
-            for(let j = 0; j < 3; j++)
-                avg[6 + j] += (m - i) * t[j] / ((m * m + m) / 2);
-                //avg[6 + j] += t[j] / m;
-        }
-        const t = [ avg[6], avg[7], avg[8] ];
-
-        // done!
-        return Speedy.Matrix(3, 3, r.concat(t));
-    }
-
-    /**
-     * Estimate extrinsics [ R | t ] given a partial pose [ r1 | r2 | t ]
-     * @param partialPose
-     * @returns 3x4 matrix
+     * Find a 3x3 rotation matrix R given two orthonormal vectors [ r1 | r2 ]
+     * @param partialRotation partial rotation matrix [ r1 | r2 ] in column-major format
+     * @returns a rotation matrix R in column-major format
      */
-    private _estimateFullPose(partialPose: SpeedyMatrix): SpeedyMatrix
+    private _computeFullRotation(partialRotation: number[]): number[]
     {
-        const p = partialPose.read();
-        const r11 = p[0], r12 = p[3], t1 = p[6];
-        const r21 = p[1], r22 = p[4], t2 = p[7];
-        const r31 = p[2], r32 = p[5], t3 = p[8];
+        const r11 = partialRotation[0], r12 = partialRotation[3];
+        const r21 = partialRotation[1], r22 = partialRotation[4];
+        const r31 = partialRotation[2], r32 = partialRotation[5];
 
         // r3 = +- ( r1 x r2 )
         let r13 = r21 * r32 - r31 * r22;
@@ -836,12 +770,11 @@ export class CameraModel
         }
 
         // done!
-        return Speedy.Matrix(3, 4, [
+        return [
             r11, r21, r31,
             r12, r22, r32,
-            r13, r23, r33,
-            t1, t2, t3,           
-        ]);
+            r13, r23, r33
+        ];
     }
 
     /**
@@ -870,19 +803,16 @@ export class CameraModel
         }
         //console.log('-----------');
 
-        // refine the translation vector
+        // read the partial pose
         const mat = partialPose.read();
-        const r = mat.slice(0, 6);
+        const r0 = mat.slice(0, 6);
         const t0 = mat.slice(6, 9);
-        const t = this._refineTranslation(normalizedHomography, r, t0);
-        const refinedPartialPose = Speedy.Matrix(3, 3, r.concat(t));
 
-        // filter the partial pose
-        const filteredPartialPose = this._filterPartialPose(refinedPartialPose);
+        // refine the translation vector and compute the full rotation matrix
+        const t = this._refineTranslation(normalizedHomography, r0, t0);
+        const r = this._computeFullRotation(r0);
 
-        // estimate the full pose
-        //const finalPartialPose = partialPose;
-        const finalPartialPose = filteredPartialPose;
-        return this._estimateFullPose(finalPartialPose);
+        // done!
+        return Speedy.Matrix(3, 4, r.concat(t));
     }
 }

src/geometry/pose-filter.ts

@@ -0,0 +1,188 @@
+/*
+ * encantar.js
+ * GPU-accelerated Augmented Reality for the web
+ * Copyright (C) 2022-2024 Alexandre Martins <alemartf(at)gmail.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published
+ * by the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ * pose-filter.ts
+ * Smoothing filter for a pose
+ */
+
+import Speedy from 'speedy-vision';
+import { SpeedyMatrix } from 'speedy-vision/types/core/speedy-matrix';
+import { Settings } from '../core/settings';
+import { Quaternion } from './quaternion';
+import { Vector3 } from './vector3';
+import { IllegalArgumentError } from '../utils/errors';
+
+/** Number of translation samples */
+const TRANSLATION_SAMPLES = 5;
+
+/** Number of rotation samples */
+const ROTATION_SAMPLES = 12;
+
+/** A vector representing "no translation" */
+const NO_TRANSLATION = Vector3.Zero();
+
+/** A quaternion representing "no rotation" */
+const NO_ROTATION = Quaternion.Identity();
+
+/** The zero quaternion */
+const ZERO_QUATERNION = new Quaternion(0, 0, 0, 0);
+
+
+
+
+
+/**
+ * Smoothing filter for a pose
+ */
+export class PoseFilter
+{
+    /** smooth rotation */
+    private _smoothRotation: Quaternion;
+
+    /** smooth translation */
+    private _smoothTranslation: Vector3;
+
+    /** samples of rotations */
+    private _rotationSample: Quaternion[];
+
+    /** samples of translations */
+    private _translationSample: Vector3[];
+
+    /** empty buffers? (i.e., no samples collected?) */
+    private _isEmpty: boolean;
+
+
+
+    /**
+     * Constructor
+     */
+    constructor()
+    {
+        this._smoothRotation = Quaternion.Identity();
+        this._smoothTranslation = Vector3.Zero();
+        this._rotationSample = Array.from({ length: ROTATION_SAMPLES }, () => Quaternion.Identity());
+        this._translationSample = Array.from({ length: TRANSLATION_SAMPLES }, () => Vector3.Zero());
+        this._isEmpty = true;
+    }
+
+    /**
+     * Reset the filter
+     */
+    reset(): void
+    {
+        this._rotationSample.forEach(q => q._copyFrom(NO_ROTATION));
+        this._translationSample.forEach(t => t._copyFrom(NO_TRANSLATION));
+        this._isEmpty = true;
+    }
+
+    /**
+     * Feed the filter with a sample
+     * @param sample 3x4 [ R | t ] matrix
+     * @returns true on success
+     */
+    feed(sample: SpeedyMatrix): boolean
+    {
+        const data = sample.read();
+
+        // sanity check
+        if(sample.rows != 3 || sample.columns != 4)
+            throw new IllegalArgumentError();
+
+        // discard invalid samples
+        if(Number.isNaN(data[0] * data[9])) // rotation, translation
+            return false;
+
+        // store sample
+        const q = this._rotationSample[ROTATION_SAMPLES - 1];
+        for(let i = ROTATION_SAMPLES - 1; i > 0; i--)
+            this._rotationSample[i] = this._rotationSample[i-1];
+        this._rotationSample[0] = q._fromRotationMatrix(sample.block(0, 2, 0, 2));
+
+        const t = this._translationSample[TRANSLATION_SAMPLES - 1];
+        for(let i = TRANSLATION_SAMPLES - 1; i > 0; i--)
+            this._translationSample[i] = this._translationSample[i-1];
+        this._translationSample[0] = t._set(data[9], data[10], data[11]);
+
+        // empty buffers?
+        if(this._isEmpty) {
+            this._rotationSample.forEach((q, i) => i > 0 && q._copyFrom(this._rotationSample[0]));
+            this._translationSample.forEach((t, i) => i > 0 && t._copyFrom(this._translationSample[0]));
+            this._isEmpty = false;
+        }
+
+        // done!
+        return true;
+    }
+
+    /**
+     * Run the filter
+     * @returns a 3x4 [ R | t ] matrix
+     */
+    run(): SpeedyMatrix
+    {
+        // how many samples should we use, at most?
+        const div = (Settings.powerPreference == 'low-power') ? 1.5 : 1; // low-power ~ half the fps
+        const T = Math.ceil(TRANSLATION_SAMPLES / div);
+        const R = Math.ceil(ROTATION_SAMPLES / div);
+
+        // clear the output of the filter
+        const t = this._smoothTranslation._copyFrom(NO_TRANSLATION);
+        const q = this._smoothRotation._copyFrom(ZERO_QUATERNION);
+
+        // average translations
+        for(let i = 0, d = 2 / (T * T + T); i < T; i++) {
+            const ti = this._translationSample[i];
+            const w = (T - i) * d;
+
+            // weighted avg: sum from i=0 to T-1 { (T-i) * t[i] } * (2/(T^2+T))
+            t.x += ti.x * w;
+            t.y += ti.y * w;
+            t.z += ti.z * w;
+        }
+
+        // average *nearby* rotations
+        // based on https://web.archive.org/web/20130514122622/http://wiki.unity3d.com/index.php/Averaging_Quaternions_and_Vectors
+        for(let i = 0; i < R; i++) {
+            const qi = this._rotationSample[i];
+            const w = 1 / R; //(R - (i - i%2)) / R;
+
+            // since unit quaternions qi and -qi encode the same rotation
+            // (see quaternion.ts), let's enforce dot(qi, 1) = qi.w >= 0
+            if(qi.w < 0) {
+                // XXX since Quaternion._fromRotationMatrix() computes w >= 0,
+                // this will never happen. Leave this here for extra safety
+                // in case anything changes?
+                qi.x = -qi.x;
+                qi.y = -qi.y;
+                qi.z = -qi.z;
+                qi.w = -qi.w;
+            }
+
+            q.x += qi.x * w;
+            q.y += qi.y * w;
+            q.z += qi.z * w;
+            q.w += qi.w * w;
+        }
+        //q._normalize();
+
+        // convert to matrix form and return
+        const entries = q._toRotationMatrix().read();
+        entries.push(t.x, t.y, t.z);
+        return Speedy.Matrix(3, 4, entries);
+    }
+}