1 /*
2 * Copyright (C) 2016 Alberto Irurueta Carro (alberto@irurueta.com)
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16 package com.irurueta.ar.calibration.estimators;
17
18 import com.irurueta.algebra.AlgebraException;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.algebra.SingularValueDecomposer;
21 import com.irurueta.ar.calibration.DualAbsoluteQuadric;
22 import com.irurueta.geometry.BaseQuadric;
23 import com.irurueta.geometry.PinholeCamera;
24 import com.irurueta.geometry.estimators.LockedException;
25 import com.irurueta.geometry.estimators.NotReadyException;
26 import com.irurueta.numerical.NumericalException;
27
28 import java.util.List;
29
30 /**
31 * Implementation of a Dual Absolute Quadric estimator using an LMSE (Least Mean
32 * Squared Error) solution for provided pinhole cameras.
33 * This implementation assumes that:
34 * - cameras are arbitrary (usually the initial camera is the identity and must
35 * be discarded) as it creates a numerical degeneracy.
36 * - all provided cameras have the same intrinsic parameters.
37 * - it is assumed that skewness is zero, the principal point is at the center
38 * of the image plane (zero), and both horizontal and vertical focal planes are
39 * equal.
40 */
41 @SuppressWarnings("DuplicatedCode")
42 public class LMSEDualAbsoluteQuadricEstimator extends DualAbsoluteQuadricEstimator {
43
44 /**
45 * Indicates if by default an LMSE (the Least Mean Square Error) solution is
46 * allowed if more pinhole cameras than the minimum are provided.
47 */
48 public static final boolean DEFAULT_ALLOW_LMSE_SOLUTION = false;
49
50 /**
51 * Indicates if by default an LMSE (the Least Mean Square Error) solution is
52 * allowed if more pinhole cameras than the minimum are provided.
53 */
54 private boolean allowLMSESolution;
55
56 /**
57 * Constructor.
58 */
59 public LMSEDualAbsoluteQuadricEstimator() {
60 super();
61 allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
62 }
63
64 /**
65 * Constructor with listener.
66 *
67 * @param listener listener to be notified of events such as when estimation
68 * starts, ends or estimation progress changes.
69 */
70 public LMSEDualAbsoluteQuadricEstimator(final DualAbsoluteQuadricEstimatorListener listener) {
71 super(listener);
72 allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
73 }
74
75 /**
76 * Constructor.
77 *
78 * @param cameras list of cameras used to estimate the Dual Absolute Quadric
79 * (DAQ).
80 * @throws IllegalArgumentException if list of cameras is null or invalid
81 * for default constraints.
82 */
83 public LMSEDualAbsoluteQuadricEstimator(final List<PinholeCamera> cameras) {
84 super(cameras);
85 allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
86 }
87
88 /**
89 * Constructor.
90 *
91 * @param cameras list of cameras used to estimate the Dual Absolute Quadric
92 * (DAQ).
93 * @param listener listener to be notified of events such as when estimation
94 * starts, ends or estimation progress changes.
95 * @throws IllegalArgumentException if list of cameras is null or invalid
96 * for default constraints.
97 */
98 public LMSEDualAbsoluteQuadricEstimator(
99 final List<PinholeCamera> cameras, final DualAbsoluteQuadricEstimatorListener listener) {
100 super(cameras, listener);
101 allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
102 }
103
104 /**
105 * Indicates if an LMSE (the Least Mean Square Error) solution is allowed if
106 * more pinhole cameras than the minimum are provided. If false, the
107 * exceeding correspondences will be ignored and only the 2 first ones
108 * will be used.
109 *
110 * @return true if LMSE solution is allowed, false otherwise.
111 */
112 public boolean isLMSESolutionAllowed() {
113 return allowLMSESolution;
114 }
115
116 /**
117 * Specifies if an LMSE (the Least Mean Square Error) solution is allowed if
118 * more pinhole cameras than the minimum are provided. If false, the
119 * exceeding correspondences will be ignored and only the 2 first ones
120 * will be used.
121 *
122 * @param allowed true if LMSE solution is allowed, false otherwise.
123 * @throws LockedException if estimator is locked.
124 */
125 public void setLMSESolutionAllowed(final boolean allowed) throws LockedException {
126 if (isLocked()) {
127 throw new LockedException();
128 }
129 allowLMSESolution = allowed;
130 }
131
132 /**
133 * Indicates whether current constraints are enough to start the estimation.
134 * In order to obtain a linear solution for the DAQ estimation, we need at
135 * least the principal point at origin constraint.
136 *
137 * @return true if constraints are valid, false otherwise.
138 */
139 @Override
140 public boolean areValidConstraints() {
141 final var valid = super.areValidConstraints();
142 if (!valid) {
143 return false;
144 }
145
146 if (allowLMSESolution) {
147 return !(focalDistanceAspectRatioKnown && singularityEnforced);
148 } else {
149 return true;
150 }
151 }
152
153
154 /**
155 * Estimates the Dual Absolute Quadric using provided cameras.
156 *
157 * @param result instance where estimated Dual Absolute Quadric (DAQ) will
158 * be stored.
159 * @throws LockedException if estimator is locked.
160 * @throws NotReadyException if no valid input data has already been
161 * provided.
162 * @throws DualAbsoluteQuadricEstimatorException if an error occurs during
163 * estimation, usually because input data is not valid or
164 * numerically unstable.
165 */
166 @Override
167 public void estimate(final DualAbsoluteQuadric result) throws LockedException, NotReadyException,
168 DualAbsoluteQuadricEstimatorException {
169
170 if (isLocked()) {
171 throw new LockedException();
172 }
173 if (!isReady()) {
174 throw new NotReadyException();
175 }
176
177 try {
178 locked = true;
179 if (listener != null) {
180 listener.onEstimateStart(this);
181 }
182
183 if (principalPointAtOrigin) {
184 if (zeroSkewness) {
185 if (focalDistanceAspectRatioKnown) {
186 estimateZeroSkewnessPrincipalPointAtOriginAndKnownFocalDistanceAspectRatio(result);
187 } else {
188 estimateZeroSkewnessAndPrincipalPointAtOrigin(result);
189 }
190 } else {
191 estimatePrincipalPointAtOrigin(result);
192 }
193 }
194
195 if (listener != null) {
196 listener.onEstimateEnd(this);
197 }
198 } finally {
199 locked = false;
200 }
201 }
202
203 /**
204 * Returns type of Dual Absolute Quadric estimator.
205 *
206 * @return type of DAQ estimator.
207 */
208 @Override
209 public DualAbsoluteQuadricEstimatorType getType() {
210 return DualAbsoluteQuadricEstimatorType.LMSE_DUAL_ABSOLUTE_QUADRIC_ESTIMATOR;
211 }
212
213 /**
214 * Gets minimum number of equations required to find a solution.
215 *
216 * @return minimum number of equations required to find a solution.
217 */
218 private int getMinRequiredEquations() {
219 return singularityEnforced ? MIN_REQUIRED_EQUATIONS - 1 : MIN_REQUIRED_EQUATIONS;
220 }
221
222 /**
223 * Estimates Dual Absolute Quadric (DAQ) assuming that skewness is zero,
224 * principal point is located at origin of coordinates and that aspect ratio
225 * of focal distances is known.
226 *
227 * @param result instance where resulting estimated Dual Absolute Quadric
228 * will be stored.
229 * @throws DualAbsoluteQuadricEstimatorException if an error occurs during
230 * estimation, usually because repeated cameras are
231 * provided, or cameras corresponding to critical motion
232 * sequences such as pure parallel translations are
233 * provided, where no additional data is really provided.
234 */
235 private void estimateZeroSkewnessPrincipalPointAtOriginAndKnownFocalDistanceAspectRatio(
236 final DualAbsoluteQuadric result) throws DualAbsoluteQuadricEstimatorException {
237 try {
238 final var nCams = cameras.size();
239
240 final Matrix a;
241 if (isLMSESolutionAllowed()) {
242 a = new Matrix(4 * nCams, BaseQuadric.N_PARAMS);
243 } else {
244 a = new Matrix(singularityEnforced ? 8 : 12, BaseQuadric.N_PARAMS);
245 }
246
247 Matrix cameraMatrix;
248 double p11;
249 double p12;
250 double p13;
251 double p14;
252 double p21;
253 double p22;
254 double p23;
255 double p24;
256 double p31;
257 double p32;
258 double p33;
259 double p34;
260 var eqCounter = 0;
261 final var minReqEqs = getMinRequiredEquations();
262 for (final var camera : cameras) {
263
264 // normalize cameras to increase accuracy
265 camera.normalize();
266
267 cameraMatrix = camera.getInternalMatrix();
268
269 p11 = cameraMatrix.getElementAt(0, 0);
270 p21 = cameraMatrix.getElementAt(1, 0);
271 p31 = cameraMatrix.getElementAt(2, 0);
272
273 p12 = cameraMatrix.getElementAt(0, 1);
274 p22 = cameraMatrix.getElementAt(1, 1);
275 p32 = cameraMatrix.getElementAt(2, 1);
276
277 p13 = cameraMatrix.getElementAt(0, 2);
278 p23 = cameraMatrix.getElementAt(1, 2);
279 p33 = cameraMatrix.getElementAt(2, 2);
280
281 p14 = cameraMatrix.getElementAt(0, 3);
282 p24 = cameraMatrix.getElementAt(1, 3);
283 p34 = cameraMatrix.getElementAt(2, 3);
284
285 // 1st row
286 fill2ndRowAnd1stRowEquation(p11, p21, p12, p22, p13, p23, p14, p24, a, eqCounter);
287 eqCounter++;
288
289 // 2nd row
290 fill3rdRowAnd1stRowEquation(p11, p31, p12, p32, p13, p33, p14, p34, a, eqCounter);
291 eqCounter++;
292
293 // 3rd row
294 fill3rdRowAnd2ndRowEquation(p21, p31, p22, p32, p23, p33, p24, p34, a, eqCounter);
295 eqCounter++;
296
297 // 4th row
298 fill1stRowEqualTo2ndRowEquation(p11, p21, p12, p22, p13, p23, p14, p24, a, eqCounter);
299 eqCounter++;
300
301 if (!isLMSESolutionAllowed() && eqCounter >= minReqEqs) {
302 break;
303 }
304 }
305
306 final var decomposer = new SingularValueDecomposer(a);
307 enforceRank3IfNeeded(decomposer, result);
308
309 } catch (final AlgebraException | NumericalException e) {
310 throw new DualAbsoluteQuadricEstimatorException(e);
311 }
312 }
313
314 /**
315 * Estimates Dual Absolute Quadric (DAQ) assuming that skewness is zero,
316 * and principal point is located at origin of coordinates.
317 *
318 * @param result instance where resulting estimated Dual Absolute Quadric
319 * will be stored.
320 * @throws DualAbsoluteQuadricEstimatorException if an error occurs during
321 * estimation, usually because repeated cameras are
322 * provided, or cameras corresponding to critical motion
323 * sequences such as pure parallel translations are
324 * provided, where no additional data is really provided.
325 */
326 private void estimateZeroSkewnessAndPrincipalPointAtOrigin(final DualAbsoluteQuadric result)
327 throws DualAbsoluteQuadricEstimatorException {
328 try {
329 final var nCams = cameras.size();
330
331 final Matrix a;
332 if (isLMSESolutionAllowed()) {
333 a = new Matrix(3 * nCams, BaseQuadric.N_PARAMS);
334 } else {
335 a = new Matrix(9, BaseQuadric.N_PARAMS);
336 }
337
338 Matrix cameraMatrix;
339 double p11;
340 double p12;
341 double p13;
342 double p14;
343 double p21;
344 double p22;
345 double p23;
346 double p24;
347 double p31;
348 double p32;
349 double p33;
350 double p34;
351 var eqCounter = 0;
352 final var minReqEqs = getMinRequiredEquations();
353 for (final var camera : cameras) {
354
355 // normalize cameras to increase accuracy
356 camera.normalize();
357
358 cameraMatrix = camera.getInternalMatrix();
359
360 p11 = cameraMatrix.getElementAt(0, 0);
361 p21 = cameraMatrix.getElementAt(1, 0);
362 p31 = cameraMatrix.getElementAt(2, 0);
363
364 p12 = cameraMatrix.getElementAt(0, 1);
365 p22 = cameraMatrix.getElementAt(1, 1);
366 p32 = cameraMatrix.getElementAt(2, 1);
367
368 p13 = cameraMatrix.getElementAt(0, 2);
369 p23 = cameraMatrix.getElementAt(1, 2);
370 p33 = cameraMatrix.getElementAt(2, 2);
371
372 p14 = cameraMatrix.getElementAt(0, 3);
373 p24 = cameraMatrix.getElementAt(1, 3);
374 p34 = cameraMatrix.getElementAt(2, 3);
375
376 // 1st row
377 fill2ndRowAnd1stRowEquation(p11, p21, p12, p22, p13, p23, p14, p24, a, eqCounter);
378 eqCounter++;
379
380 // 2nd row
381 fill3rdRowAnd1stRowEquation(p11, p31, p12, p32, p13, p33, p14, p34, a, eqCounter);
382 eqCounter++;
383
384 // 3rd row
385 fill3rdRowAnd2ndRowEquation(p21, p31, p22, p32, p23, p33, p24, p34, a, eqCounter);
386 eqCounter++;
387
388 if (!isLMSESolutionAllowed() && eqCounter >= minReqEqs) {
389 break;
390 }
391 }
392
393 final var decomposer = new SingularValueDecomposer(a);
394 enforceRank3IfNeeded(decomposer, result);
395
396 } catch (final AlgebraException | NumericalException e) {
397 throw new DualAbsoluteQuadricEstimatorException(e);
398 }
399 }
400
401 /**
402 * Estimates Dual Absolute Quadric (DAQ) assuming that principal point is
403 * zero.
404 *
405 * @param result instance where resulting estimated Dual Absolute Quadric
406 * will be stored.
407 * @throws DualAbsoluteQuadricEstimatorException if an error occurs during
408 * estimation, usually because repeated cameras are
409 * provided, or cameras corresponding to critical motion
410 * sequences such as pure parallel translations are
411 * provided, where no additional data is really provided.
412 */
413 private void estimatePrincipalPointAtOrigin(final DualAbsoluteQuadric result)
414 throws DualAbsoluteQuadricEstimatorException {
415 try {
416 final var nCams = cameras.size();
417
418 final Matrix a;
419 if (isLMSESolutionAllowed()) {
420 a = new Matrix(2 * nCams, BaseQuadric.N_PARAMS);
421 } else {
422 a = new Matrix(singularityEnforced ? 8 : 10, BaseQuadric.N_PARAMS);
423 }
424
425 Matrix cameraMatrix;
426 double p11;
427 double p12;
428 double p13;
429 double p14;
430 double p21;
431 double p22;
432 double p23;
433 double p24;
434 double p31;
435 double p32;
436 double p33;
437 double p34;
438 var eqCounter = 0;
439 final var minReqEqs = getMinRequiredEquations();
440 for (final var camera : cameras) {
441
442 // normalize cameras to increase accuracy
443 camera.normalize();
444
445 cameraMatrix = camera.getInternalMatrix();
446
447 p11 = cameraMatrix.getElementAt(0, 0);
448 p21 = cameraMatrix.getElementAt(1, 0);
449 p31 = cameraMatrix.getElementAt(2, 0);
450
451 p12 = cameraMatrix.getElementAt(0, 1);
452 p22 = cameraMatrix.getElementAt(1, 1);
453 p32 = cameraMatrix.getElementAt(2, 1);
454
455 p13 = cameraMatrix.getElementAt(0, 2);
456 p23 = cameraMatrix.getElementAt(1, 2);
457 p33 = cameraMatrix.getElementAt(2, 2);
458
459 p14 = cameraMatrix.getElementAt(0, 3);
460 p24 = cameraMatrix.getElementAt(1, 3);
461 p34 = cameraMatrix.getElementAt(2, 3);
462
463 // 1st row
464 fill3rdRowAnd1stRowEquation(p11, p31, p12, p32, p13, p33, p14, p34, a, eqCounter);
465 eqCounter++;
466
467 // 2nd row
468 fill3rdRowAnd2ndRowEquation(p21, p31, p22, p32, p23, p33, p24, p34, a, eqCounter);
469 eqCounter++;
470
471 if (!isLMSESolutionAllowed() && eqCounter >= minReqEqs) {
472 break;
473 }
474 }
475
476 final var decomposer = new SingularValueDecomposer(a);
477 enforceRank3IfNeeded(decomposer, result);
478
479 } catch (final AlgebraException | NumericalException e) {
480 throw new DualAbsoluteQuadricEstimatorException(e);
481 }
482 }
483 }