|
@@ -29,11 +29,14 @@ import { SpeedyPromise } from 'speedy-vision/types/core/speedy-promise';
|
29
|
29
|
import { Nullable, Utils } from '../utils/utils';
|
30
|
30
|
import { Settings } from '../core/settings';
|
31
|
31
|
import { PoseFilter } from './pose-filter';
|
32
|
|
-import { IllegalOperationError, IllegalArgumentError } from '../utils/errors';
|
|
32
|
+import { NumericalError } from '../utils/errors';
|
33
|
33
|
|
34
|
34
|
/** A guess of the horizontal field-of-view of a typical camera, in degrees */
|
35
|
35
|
const HFOV_GUESS = 60; // https://developer.apple.com/library/archive/documentation/DeviceInformation/Reference/iOSDeviceCompatibility/Cameras/Cameras.html
|
36
|
36
|
|
|
37
|
+/** The default scale of the image plane. The scale affects the focal length */
|
|
38
|
+const DEFAULT_SCALE = 2; // the length of the [-1,+1] interval
|
|
39
|
+
|
37
|
40
|
/** Convert degrees to radians */
|
38
|
41
|
const DEG2RAD = 0.017453292519943295; // pi / 180
|
39
|
42
|
|
|
@@ -56,14 +59,16 @@ const U0 = 6;
|
56
|
59
|
const V0 = 7;
|
57
|
60
|
|
58
|
61
|
/** Number of iterations used to refine the estimated pose */
|
59
|
|
-const POSE_ITERATIONS = 30;
|
|
62
|
+const POSE_REFINEMENT_ITERATIONS = 30;
|
60
|
63
|
|
61
|
64
|
/** Maximum number of iterations used when refining the translation vector */
|
62
|
|
-const REFINE_TRANSLATION_ITERATIONS = 15;
|
|
65
|
+const TRANSLATION_REFINEMENT_ITERATIONS = 15;
|
63
|
66
|
|
64
|
67
|
/** Tolerance used to exit early when refining the translation vector */
|
65
|
|
-const REFINE_TRANSLATION_TOLERANCE = 1; // in units compatible with the size of the image sensor
|
66
|
|
-//FIXME make it a percentage?
|
|
68
|
+const TRANSLATION_REFINEMENT_TOLERANCE = DEFAULT_SCALE * 0.01;
|
|
69
|
+
|
|
70
|
+/** Size of the grid used to refine the translation vector */
|
|
71
|
+const TRANSLATION_REFINEMENT_GRIDSIZE = 5; //3;
|
67
|
72
|
|
68
|
73
|
|
69
|
74
|
|
|
@@ -72,16 +77,19 @@ const REFINE_TRANSLATION_TOLERANCE = 1; // in units compatible with the size of
|
72
|
77
|
*/
|
73
|
78
|
export class CameraModel
|
74
|
79
|
{
|
75
|
|
- /** size of the image */
|
|
80
|
+ /** size of the image plane */
|
76
|
81
|
private _imageSize: SpeedySize;
|
77
|
82
|
|
78
|
83
|
/** 3x4 camera matrix */
|
79
|
84
|
private _matrix: SpeedyMatrix;
|
80
|
85
|
|
81
|
|
- /** intrinsics matrix, in column-major format */
|
|
86
|
+ /** a helper to switch the handedness of a coordinate system */
|
|
87
|
+ private _flipZ: SpeedyMatrix;
|
|
88
|
+
|
|
89
|
+ /** entries of the intrinsics matrix in column-major format */
|
82
|
90
|
private _intrinsics: number[];
|
83
|
91
|
|
84
|
|
- /** extrinsics matrix, in column-major format */
|
|
92
|
+ /** entries of the extrinsics matrix in column-major format */
|
85
|
93
|
private _extrinsics: number[];
|
86
|
94
|
|
87
|
95
|
/** smoothing filter */
|
|
@@ -99,20 +107,35 @@ export class CameraModel
|
99
|
107
|
this._intrinsics = [1,0,0,0,1,0,0,0,1]; // 3x3 identity matrix
|
100
|
108
|
this._extrinsics = [1,0,0,0,1,0,0,0,1,0,0,0]; // 3x4 matrix [ R | t ] = [ I | 0 ] no rotation & no translation
|
101
|
109
|
this._filter = new PoseFilter();
|
|
110
|
+ this._flipZ = Speedy.Matrix(4, 4, [
|
|
111
|
+ 1, 0, 0, 0,
|
|
112
|
+ 0, 1, 0, 0,
|
|
113
|
+ 0, 0,-1, 0,
|
|
114
|
+ 0, 0, 0, 1
|
|
115
|
+ ]);
|
102
|
116
|
}
|
103
|
117
|
|
104
|
118
|
/**
|
105
|
119
|
* Initialize the model
|
106
|
|
- * @param imageSize
|
|
120
|
+ * @param aspectRatio aspect ratio of the image plane
|
|
121
|
+ * @param scale optional scale factor of the image plane
|
107
|
122
|
*/
|
108
|
|
- init(imageSize: SpeedySize): void
|
|
123
|
+ init(aspectRatio: number, scale: number = DEFAULT_SCALE): void
|
109
|
124
|
{
|
110
|
125
|
// log
|
111
|
126
|
Utils.log(`Initializing the camera model...`);
|
|
127
|
+ Utils.assert(aspectRatio > 0 && scale > 1e-5);
|
112
|
128
|
|
113
|
|
- // set the imageSize
|
114
|
|
- this._imageSize.width = imageSize.width;
|
115
|
|
- this._imageSize.height = imageSize.height;
|
|
129
|
+ // set the size of the image plane
|
|
130
|
+ // this rule is conceived so that min(w,h) = s and w/h = a
|
|
131
|
+ if(aspectRatio >= 1) {
|
|
132
|
+ this._imageSize.width = aspectRatio * scale;
|
|
133
|
+ this._imageSize.height = scale;
|
|
134
|
+ }
|
|
135
|
+ else {
|
|
136
|
+ this._imageSize.width = scale;
|
|
137
|
+ this._imageSize.height = scale / aspectRatio;
|
|
138
|
+ }
|
116
|
139
|
|
117
|
140
|
// reset the model
|
118
|
141
|
this.reset();
|
|
@@ -129,14 +152,15 @@ export class CameraModel
|
129
|
152
|
|
130
|
153
|
/**
|
131
|
154
|
* Update the camera model
|
132
|
|
- * @param homography 3x3 perspective transform
|
133
|
|
- * @returns promise that resolves to a camera matrix
|
|
155
|
+ * @param homographyNDC 3x3 perspective transform
|
|
156
|
+ * @returns a promise that resolves to a camera matrix
|
134
|
157
|
*/
|
135
|
|
- update(homography: SpeedyMatrix): SpeedyPromise<SpeedyMatrix>
|
|
158
|
+ update(homographyNDC: SpeedyMatrix): SpeedyPromise<SpeedyMatrix>
|
136
|
159
|
{
|
137
|
|
- // validate the shape of the homography
|
138
|
|
- if(homography.rows != 3 || homography.columns != 3)
|
139
|
|
- throw new IllegalArgumentError(`Camera model: provide a homography matrix`);
|
|
160
|
+ Utils.assert(homographyNDC.rows == 3 && homographyNDC.columns == 3);
|
|
161
|
+
|
|
162
|
+ // convert to image space
|
|
163
|
+ const homography = this._convertToImageSpace(homographyNDC);
|
140
|
164
|
|
141
|
165
|
// read the entries of the homography
|
142
|
166
|
const h = homography.read();
|
|
@@ -146,10 +170,8 @@ export class CameraModel
|
146
|
170
|
|
147
|
171
|
// validate the homography (homography matrices aren't singular)
|
148
|
172
|
const det = h13 * (h21 * h32 - h22 * h31) - h23 * (h11 * h32 - h12 * h31) + h33 * (h11 * h22 - h12 * h21);
|
149
|
|
- if(Math.abs(det) < EPSILON) {
|
150
|
|
- Utils.warning(`Can't update the camera model using an invalid homography matrix`);
|
151
|
|
- return Speedy.Promise.resolve(this._matrix);
|
152
|
|
- }
|
|
173
|
+ if(Math.abs(det) < EPSILON || Number.isNaN(det))
|
|
174
|
+ return Speedy.Promise.reject(new NumericalError(`Can't update the camera model using an invalid homography matrix`));
|
153
|
175
|
|
154
|
176
|
// estimate the pose
|
155
|
177
|
const pose = this._estimatePose(homography);
|
|
@@ -157,12 +179,22 @@ export class CameraModel
|
157
|
179
|
this._extrinsics = this._filter.output().read();
|
158
|
180
|
|
159
|
181
|
// compute the camera matrix
|
160
|
|
- const C = this.denormalizer();
|
|
182
|
+ const Z = this._flipZ; // switch to a right handed system
|
161
|
183
|
const K = Speedy.Matrix(3, 3, this._intrinsics);
|
162
|
184
|
const E = Speedy.Matrix(3, 4, this._extrinsics);
|
163
|
|
- this._matrix.setToSync(K.times(E).times(C));
|
164
|
|
- //console.log("intrinsics -----------", K.toString());
|
165
|
|
- //console.log("matrix ----------------",this._matrix.toString());
|
|
185
|
+ this._matrix.setToSync(K.times(E).times(Z));
|
|
186
|
+
|
|
187
|
+ /*
|
|
188
|
+ // test
|
|
189
|
+ console.log("homography ------------", homography.toString());
|
|
190
|
+ console.log("intrinsics ------------", K.toString());
|
|
191
|
+ console.log("extrinsics ------------", E.toString());
|
|
192
|
+ console.log("extrinsicsINV ---------", Speedy.Matrix(this.computeViewMatrix().inverse()).toString());
|
|
193
|
+ console.log("matrix ----------------", this._matrix.toString());
|
|
194
|
+ console.log("projectionMatrix ----- ", this.computeProjectionMatrix(0.1,100).toString());
|
|
195
|
+ */
|
|
196
|
+
|
|
197
|
+ // done!
|
166
|
198
|
return Speedy.Promise.resolve(this._matrix);
|
167
|
199
|
}
|
168
|
200
|
|
|
@@ -184,6 +216,14 @@ export class CameraModel
|
184
|
216
|
}
|
185
|
217
|
|
186
|
218
|
/**
|
|
219
|
+ * The size of the image plane
|
|
220
|
+ */
|
|
221
|
+ get imageSize(): SpeedySize
|
|
222
|
+ {
|
|
223
|
+ return this._imageSize;
|
|
224
|
+ }
|
|
225
|
+
|
|
226
|
+ /**
|
187
|
227
|
* The aspect ratio of the image
|
188
|
228
|
*/
|
189
|
229
|
get aspectRatio(): number
|
|
@@ -192,8 +232,9 @@ export class CameraModel
|
192
|
232
|
}
|
193
|
233
|
|
194
|
234
|
/**
|
195
|
|
- * Focal length in pixels (projection distance in the pinhole camera model)
|
196
|
|
- * same as (focal length in mm) * (number of pixels per world unit in pixels/mm)
|
|
235
|
+ * Focal length in "pixels" (projection distance in the pinhole camera model)
|
|
236
|
+ * same as (focal length in mm) * (number of "pixels" per world unit in "pixels"/mm)
|
|
237
|
+ * "pixels" means image plane units
|
197
|
238
|
*/
|
198
|
239
|
get focalLength(): number
|
199
|
240
|
{
|
|
@@ -205,7 +246,8 @@ export class CameraModel
|
205
|
246
|
*/
|
206
|
247
|
get fovx(): number
|
207
|
248
|
{
|
208
|
|
- return 2 * Math.atan(this._intrinsics[U0] / this._intrinsics[FX]);
|
|
249
|
+ const halfWidth = this._imageSize.width / 2;
|
|
250
|
+ return 2 * Math.atan(halfWidth / this._intrinsics[FX]);
|
209
|
251
|
}
|
210
|
252
|
|
211
|
253
|
/**
|
|
@@ -213,102 +255,30 @@ export class CameraModel
|
213
|
255
|
*/
|
214
|
256
|
get fovy(): number
|
215
|
257
|
{
|
216
|
|
- return 2 * Math.atan(this._intrinsics[V0] / this._intrinsics[FY]);
|
217
|
|
- }
|
218
|
|
-
|
219
|
|
- /**
|
220
|
|
- * Principal point
|
221
|
|
- * @returns principal point
|
222
|
|
- */
|
223
|
|
- /*
|
224
|
|
- principalPoint(): SpeedyPoint2
|
225
|
|
- {
|
226
|
|
- return Speedy.Point2(this._intrinsics[U0], this._intrinsics[V0]);
|
227
|
|
- }
|
228
|
|
- */
|
229
|
|
-
|
230
|
|
- /**
|
231
|
|
- * Convert coordinates from normalized space [-1,1]^3 to a
|
232
|
|
- * "3D pixel space" based on the dimensions of the image sensor.
|
233
|
|
- *
|
234
|
|
- * We perform a 180-degrees rotation around the x-axis so that
|
235
|
|
- * it looks nicer (the y-axis grows downwards in image space).
|
236
|
|
- *
|
237
|
|
- * The final camera matrix is P = K * [ R | t ] * C, where
|
238
|
|
- * C is this conversion matrix. The intent behind this is to
|
239
|
|
- * make tracking independent of target and screen sizes.
|
240
|
|
- *
|
241
|
|
- * Reminder: we use a right-handed coordinate system in 3D!
|
242
|
|
- * In 2D image space the coordinate system is left-handed.
|
243
|
|
- *
|
244
|
|
- * @returns 4x4 conversion matrix C
|
245
|
|
- */
|
246
|
|
- denormalizer(): SpeedyMatrix
|
247
|
|
- {
|
248
|
|
- const w = this._imageSize.width / 2; // half width, in pixels
|
249
|
|
- const h = this._imageSize.height / 2; // half height, in pixels
|
250
|
|
- const d = Math.min(w, h); // virtual unit length, in pixels
|
251
|
|
-
|
252
|
|
- /*
|
253
|
|
- return Speedy.Matrix(4, 4, [
|
254
|
|
- 1, 0, 0, 0,
|
255
|
|
- 0,-1, 0, 0,
|
256
|
|
- 0, 0,-1, 0,
|
257
|
|
- w/d, h/d, 0, 1/d
|
258
|
|
- ]);
|
259
|
|
- */
|
260
|
|
-
|
261
|
|
- return Speedy.Matrix(4, 4, [
|
262
|
|
- d, 0, 0, 0,
|
263
|
|
- 0,-d, 0, 0,
|
264
|
|
- 0, 0,-d, 0,
|
265
|
|
- w, h, 0, 1,
|
266
|
|
- ]);
|
|
258
|
+ const halfHeight = this._imageSize.height / 2;
|
|
259
|
+ return 2 * Math.atan(halfHeight / this._intrinsics[FY]);
|
267
|
260
|
}
|
268
|
261
|
|
269
|
262
|
/**
|
270
|
|
- * Compute the view matrix in AR screen space, measured in pixels.
|
271
|
|
- * This 4x4 matrix moves 3D points from world space to view space.
|
272
|
|
- * We assume that the camera is looking in the direction of the
|
273
|
|
- * negative z-axis (WebGL-friendly)
|
274
|
|
- * @param camera
|
275
|
|
- * @returns a 4x4 matrix describing a rotation and a translation
|
|
263
|
+ * Compute the view matrix. This 4x4 matrix moves 3D points from
|
|
264
|
+ * world space to view space. We want the camera looking in the
|
|
265
|
+ * direction of the negative z-axis (WebGL-friendly)
|
|
266
|
+ * @returns a view matrix
|
276
|
267
|
*/
|
277
|
268
|
computeViewMatrix(): SpeedyMatrix
|
278
|
269
|
{
|
279
|
270
|
const E = this._extrinsics;
|
280
|
271
|
|
281
|
|
- /*
|
282
|
|
-
|
283
|
|
- // this is the view matrix in AR screen space, measured in pixels
|
284
|
|
- // we augment the extrinsics matrix, making it 4x4 by adding a
|
285
|
|
- // [ 0 0 0 1 ] row. Below, E is a 3x4 extrinsics matrix
|
286
|
|
- const V = Speedy.Matrix(4, 4, [
|
287
|
|
- E[0], E[1], E[2], 0,
|
288
|
|
- E[3], E[4], E[5], 0,
|
289
|
|
- E[6], E[7], E[8], 0,
|
290
|
|
- E[9], E[10], E[11], 1
|
291
|
|
- ]);
|
292
|
|
-
|
293
|
|
- // we premultiply V by F, which performs a rotation around the
|
294
|
|
- // x-axis by 180 degrees, so that we get the 3D objects in front
|
295
|
|
- // of the camera pointing in the direction of the negative z-axis
|
296
|
|
- const F = Speedy.Matrix(4, 4, [
|
297
|
|
- 1, 0, 0, 0,
|
298
|
|
- 0,-1, 0, 0,
|
299
|
|
- 0, 0,-1, 0,
|
300
|
|
- 0, 0, 0, 1
|
301
|
|
- ]);
|
302
|
|
-
|
303
|
|
- Matrix F * V is matrix V with the second and third rows negated
|
304
|
|
-
|
305
|
|
- */
|
306
|
|
-
|
|
272
|
+ // We augment the 3x4 extrinsics matrix E with the [ 0 0 0 1 ] row
|
|
273
|
+ // and get E+. Let Z be 4x4 flipZ, the identity matrix with the third
|
|
274
|
+ // column negated. The following matrix is View = Z * E+ * Z. We get
|
|
275
|
+ // the camera looking in the direction of the negative z-axis in a
|
|
276
|
+ // right handed system!
|
307
|
277
|
return Speedy.Matrix(4, 4, [
|
308
|
|
- E[0],-E[1],-E[2], 0,
|
309
|
|
- E[3],-E[4],-E[5], 0,
|
310
|
|
- E[6],-E[7],-E[8], 0,
|
311
|
|
- E[9],-E[10],-E[11], 1
|
|
278
|
+ E[0], E[1],-E[2], 0, // r1
|
|
279
|
+ E[3], E[4],-E[5], 0, // r2
|
|
280
|
+ -E[6],-E[7],+E[8], 0, // r3
|
|
281
|
+ E[9], E[10],-E[11], 1 // t
|
312
|
282
|
]);
|
313
|
283
|
}
|
314
|
284
|
|
|
@@ -319,11 +289,15 @@ export class CameraModel
|
319
|
289
|
*/
|
320
|
290
|
computeProjectionMatrix(near: number, far: number): SpeedyMatrix
|
321
|
291
|
{
|
322
|
|
- const K = this._intrinsics;
|
|
292
|
+ const fx = this._intrinsics[FX];
|
|
293
|
+ const fy = this._intrinsics[FY];
|
|
294
|
+ const halfWidth = this._imageSize.width / 2;
|
|
295
|
+ const halfHeight = this._imageSize.height / 2;
|
323
|
296
|
|
324
|
|
- // we assume that the principal point is at the center of the image
|
325
|
|
- const top = near * (K[V0] / K[FY]);
|
326
|
|
- const right = near * (K[U0] / K[FX]);
|
|
297
|
+ // we assume that the principal point is at the center of the image plane
|
|
298
|
+ const right = near * (halfWidth / fx);
|
|
299
|
+ const top = near * (halfHeight / fy);
|
|
300
|
+ //const top = right * (halfHeight / halfWidth); // same thing
|
327
|
301
|
const bottom = -top, left = -right; // symmetric frustum
|
328
|
302
|
|
329
|
303
|
// a derivation of this projection matrix can be found at
|
|
@@ -357,8 +331,8 @@ export class CameraModel
|
357
|
331
|
{
|
358
|
332
|
const cameraWidth = Math.max(this._imageSize.width, this._imageSize.height); // portrait or landscape?
|
359
|
333
|
|
360
|
|
- const u0 = this._imageSize.width / 2;
|
361
|
|
- const v0 = this._imageSize.height / 2;
|
|
334
|
+ const u0 = 0; // principal point at the center of the image plane
|
|
335
|
+ const v0 = 0;
|
362
|
336
|
const fx = (cameraWidth / 2) / Math.tan(DEG2RAD * HFOV_GUESS / 2);
|
363
|
337
|
const fy = fx;
|
364
|
338
|
|
|
@@ -369,6 +343,34 @@ export class CameraModel
|
369
|
343
|
}
|
370
|
344
|
|
371
|
345
|
/**
|
|
346
|
+ * Convert a homography from NDC to image space
|
|
347
|
+ * @param homographyNDC
|
|
348
|
+ * @returns a new homography
|
|
349
|
+ */
|
|
350
|
+ private _convertToImageSpace(homographyNDC: SpeedyMatrix): SpeedyMatrix
|
|
351
|
+ {
|
|
352
|
+ const w = this._imageSize.width / 2;
|
|
353
|
+ const h = this._imageSize.height / 2;
|
|
354
|
+
|
|
355
|
+ // fromNDC converts points from NDC to image space
|
|
356
|
+ const fromNDC = Speedy.Matrix(3, 3, [
|
|
357
|
+ w, 0, 0,
|
|
358
|
+ 0, h, 0,
|
|
359
|
+ 0, 0, 1
|
|
360
|
+ ]);
|
|
361
|
+
|
|
362
|
+ /*
|
|
363
|
+ // make h33 = 1 (wanted?)
|
|
364
|
+ const data = homographyNDC.read();
|
|
365
|
+ const h33 = data[8];
|
|
366
|
+ const hom = homographyNDC.times(1/h33);
|
|
367
|
+ */
|
|
368
|
+
|
|
369
|
+ // convert homography
|
|
370
|
+ return Speedy.Matrix(fromNDC.times(homographyNDC));
|
|
371
|
+ }
|
|
372
|
+
|
|
373
|
+ /**
|
372
|
374
|
* Compute a normalized homography H^ = K^(-1) * H for an
|
373
|
375
|
* ideal pinhole with f = 1 and principal point = (0,0)
|
374
|
376
|
* @param homography homography H to be normalized
|
|
@@ -642,22 +644,30 @@ export class CameraModel
|
642
|
644
|
const r21 = rot[1], r22 = rot[4];
|
643
|
645
|
const r31 = rot[2], r32 = rot[5];
|
644
|
646
|
|
645
|
|
- // sample points [ xi yi ]' in screen space
|
646
|
|
- //const x = [ 0.5, 0.0, 1.0, 1.0, 0.0, 0.5, 1.0, 0.5, 0.0 ];
|
647
|
|
- //const y = [ 0.5, 0.0, 0.0, 1.0, 1.0, 0.0, 0.5, 1.0, 0.5 ];
|
648
|
|
- const x = [ 0.5, 0.0, 1.0, 1.0, 0.0 ];
|
649
|
|
- const y = [ 0.5, 0.0, 0.0, 1.0, 1.0 ];
|
650
|
|
- const n = x.length;
|
651
|
|
- const n3 = 3*n;
|
652
|
|
-
|
653
|
|
- const width = this._imageSize.width;
|
654
|
|
- const height = this._imageSize.height;
|
655
|
|
- for(let i = 0; i < n; i++) {
|
656
|
|
- x[i] *= width;
|
657
|
|
- y[i] *= height;
|
|
647
|
+ // generate a grid of sample points [ xi yi ]' in the image
|
|
648
|
+ //const x = [ 0, -1, +1, +1, -1 ];
|
|
649
|
+ //const y = [ 0, -1, -1, +1, +1 ];
|
|
650
|
+ const g = TRANSLATION_REFINEMENT_GRIDSIZE;
|
|
651
|
+ const x = new Array<number>(g*g);
|
|
652
|
+ const y = new Array<number>(g*g);
|
|
653
|
+ const halfWidth = this._imageSize.width / 2;
|
|
654
|
+ const halfHeight = this._imageSize.height / 2;
|
|
655
|
+
|
|
656
|
+ for(let k = 0, i = 0; i < g; i++) {
|
|
657
|
+ for(let j = 0; j < g; j++, k++) {
|
|
658
|
+ // in [-1,+1]
|
|
659
|
+ x[k] = (i/(g-1)) * 2 - 1;
|
|
660
|
+ y[k] = (j/(g-1)) * 2 - 1;
|
|
661
|
+
|
|
662
|
+ // in [-s/2,+s/2], where s = w,h
|
|
663
|
+ x[k] *= halfWidth;
|
|
664
|
+ y[k] *= halfHeight;
|
|
665
|
+ }
|
658
|
666
|
}
|
|
667
|
+ //console.log(x.toString(), y.toString());
|
659
|
668
|
|
660
|
669
|
// set auxiliary values: ai = H [ xi yi 1 ]'
|
|
670
|
+ const n = x.length;
|
661
|
671
|
const a1 = new Array<number>(n);
|
662
|
672
|
const a2 = new Array<number>(n);
|
663
|
673
|
const a3 = new Array<number>(n);
|
|
@@ -669,8 +679,9 @@ export class CameraModel
|
669
|
679
|
|
670
|
680
|
// we'll solve M t = v for t with linear least squares
|
671
|
681
|
// M: 3n x 3, v: 3n x 1, t: 3 x 1
|
672
|
|
- const m = new Array<number>(3*n * 3);
|
673
|
|
- const v = new Array<number>(3*n);
|
|
682
|
+ const n3 = 3*n;
|
|
683
|
+ const m = new Array<number>(n3 * 3);
|
|
684
|
+ const v = new Array<number>(n3);
|
674
|
685
|
for(let i = 0, k = 0; k < n; i += 3, k++) {
|
675
|
686
|
m[i] = m[i+n3+1] = m[i+n3+n3+2] = 0;
|
676
|
687
|
m[i+n3] = -(m[i+1] = a3[k]);
|
|
@@ -742,7 +753,7 @@ export class CameraModel
|
742
|
753
|
t[2] = t0[2];
|
743
|
754
|
|
744
|
755
|
// iterate
|
745
|
|
- for(let it = 0; it < REFINE_TRANSLATION_ITERATIONS; it++) {
|
|
756
|
+ for(let it = 0; it < TRANSLATION_REFINEMENT_ITERATIONS; it++) {
|
746
|
757
|
//console.log("it",it+1);
|
747
|
758
|
|
748
|
759
|
// compute residual r = Mt - v
|
|
@@ -771,8 +782,8 @@ export class CameraModel
|
771
|
782
|
let num = 0;
|
772
|
783
|
for(let i = 0; i < 3; i++)
|
773
|
784
|
num += c[i] * c[i];
|
774
|
|
- //console.log("c'c=",num);
|
775
|
|
- if(num < REFINE_TRANSLATION_TOLERANCE)
|
|
785
|
+ //console.log("c'c=",num," at #",it+1);
|
|
786
|
+ if(num < TRANSLATION_REFINEMENT_TOLERANCE)
|
776
|
787
|
break;
|
777
|
788
|
|
778
|
789
|
// compute (Mc)'(Mc)
|
|
@@ -846,7 +857,7 @@ export class CameraModel
|
846
|
857
|
// it won't be a perfect equality due to noise in the homography.
|
847
|
858
|
// remark: composition of homographies
|
848
|
859
|
const residual = Speedy.Matrix(normalizedHomography);
|
849
|
|
- for(let k = 0; k < POSE_ITERATIONS; k++) {
|
|
860
|
+ for(let k = 0; k < POSE_REFINEMENT_ITERATIONS; k++) {
|
850
|
861
|
// incrementally improve the partial pose
|
851
|
862
|
const rt = this._estimatePartialPose(residual); // rt should converge to the identity matrix
|
852
|
863
|
partialPose.setToSync(rt.times(partialPose));
|