1 /*
2 * Copyright (C) 2015 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.ImageOfAbsoluteConic;
22 import com.irurueta.geometry.ProjectiveTransformation2D;
23 import com.irurueta.geometry.Transformation2D;
24 import com.irurueta.geometry.estimators.LockedException;
25 import com.irurueta.geometry.estimators.NotReadyException;
26
27 import java.util.List;
28
29 /**
30 * This class defines an LMSE (the Least Mean Square Error) estimator of Image
31 * of Absolute Conic (IAC).
32 * Aside from enabling constraints whenever possible to obtain more stable and
33 * accurate results, it is also discouraged to enable LMSE solutions, or at
34 * least if LMSE must be used, the minimum possible number of homographies
35 * should be provided in order to introduce the least amount of rounding errors
36 * possible.
37 * If a large number of homographies is available (assuming constant IAC),
38 * instead a robust estimation method should be chosen to discard outliers and
39 * obtain the most accurate and stable solution possible.
40 */
41 @SuppressWarnings("DuplicatedCode")
42 public class LMSEImageOfAbsoluteConicEstimator extends ImageOfAbsoluteConicEstimator {
43
44 /**
45 * Indicates if by default an LMSE (the Least Mean Square Error) solution is
46 * allowed if more homographies than the minimum are provided.
47 */
48 public static final boolean DEFAULT_ALLOW_LMSE_SOLUTION = false;
49
50 /**
51 * Indicates if an LMSE (the Least Mean Square Error) solution is allowed if
52 * more homographies than the minimum are provided. If false, the
53 * exceeding homographies will be ignored and only the first required
54 * homographies will be used.
55 */
56 private boolean allowLMSESolution;
57
58 /**
59 * Constructor.
60 */
61 public LMSEImageOfAbsoluteConicEstimator() {
62 super();
63 allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
64 }
65
66 /**
67 * Constructor with listener.
68 *
69 * @param listener listener to be notified of events such as when estimation
70 * starts, ends or estimation progress changes.
71 */
72 public LMSEImageOfAbsoluteConicEstimator(final ImageOfAbsoluteConicEstimatorListener listener) {
73 super(listener);
74 allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
75 }
76
77 /**
78 * Constructor
79 *
80 * @param homographies list of homographies (2D transformations) used to
81 * estimate the image of absolute conic (IAC), which can be used to obtain
82 * pinhole camera intrinsic parameters.
83 * @throws IllegalArgumentException if not enough homographies are provided
84 * for default IAC estimation constraints.
85 */
86 public LMSEImageOfAbsoluteConicEstimator(final List<Transformation2D> homographies) {
87 super(homographies);
88 allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
89 }
90
91 /**
92 * Constructor
93 *
94 * @param homographies list of homographies (2D transformations) used to
95 * estimate the image of absolute conic (IAC), which can be used to obtain
96 * pinhole camera intrinsic parameters.
97 * @param listener listener to be notified of events such as when estimation
98 * starts, ends or estimation progress changes.
99 * @throws IllegalArgumentException if not enough homographies are provided
100 * for default IAC estimation constraints.
101 */
102 public LMSEImageOfAbsoluteConicEstimator(
103 final List<Transformation2D> homographies, final ImageOfAbsoluteConicEstimatorListener listener) {
104 super(homographies, listener);
105 allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
106 }
107
108 /**
109 * Indicates if an LMSE (the Least Mean Square Error) solution is allowed if
110 * more homographies than the minimum are provided. If false, the
111 * exceeding homographies will be ignored and only the first required
112 * correspondences will be used.
113 *
114 * @return true if LMSE solution is allowed, false otherwise.
115 */
116 public boolean isLMSESolutionAllowed() {
117 return allowLMSESolution;
118 }
119
120 /**
121 * Specifies if an LMSE (the Least Mean Square Error) solution is allowed if
122 * more homographies than the minimum are provided. If false, the
123 * exceeding homographies will be ignored and only the first required ones
124 * will be used.
125 *
126 * @param allowed true if LMSE solution is allowed, false otherwise.
127 * @throws LockedException if estimator is locked.
128 */
129 public void setLMSESolutionAllowed(final boolean allowed) throws LockedException {
130 if (isLocked()) {
131 throw new LockedException();
132 }
133 allowLMSESolution = allowed;
134 }
135
136 /**
137 * Estimates Image of Absolute Conic (IAC).
138 *
139 * @return estimated IAC.
140 * @throws LockedException if estimator is locked.
141 * @throws NotReadyException if input has not yet been provided.
142 * @throws ImageOfAbsoluteConicEstimatorException if an error occurs during
143 * estimation, usually because repeated homographies are
144 * provided, or homographies corresponding to degenerate
145 * camera movements such as pure parallel translations
146 * where no additional data is really provided. Indeed,
147 * if provided homographies belong to the group of affine
148 * transformations (or other groups contained within
149 * such as metric or Euclidean ones), this exception will
150 * raise because camera movements will be degenerate. To
151 * avoid this exception, homographies must be purely
152 * projective.
153 */
154 @Override
155 public ImageOfAbsoluteConic estimate() throws LockedException, NotReadyException,
156 ImageOfAbsoluteConicEstimatorException {
157 if (isLocked()) {
158 throw new LockedException();
159 }
160 if (!isReady()) {
161 throw new NotReadyException();
162 }
163
164 try {
165 locked = true;
166 if (listener != null) {
167 listener.onEstimateStart(this);
168 }
169
170 final ImageOfAbsoluteConic iac;
171 if (zeroSkewness && principalPointAtOrigin) {
172 if (focalDistanceAspectRatioKnown) {
173 iac = estimateZeroSkewnessPrincipalPointAtOriginAndKnownFocalDistanceAspectRatio();
174 } else {
175 iac = estimateZeroSkewnessAndPrincipalPointAtOrigin();
176 }
177 } else if (zeroSkewness) { // && !mPrincipalPointAtOrigin
178 if (focalDistanceAspectRatioKnown) {
179 iac = estimateZeroSkewnessAndKnownFocalDistanceAspectRatio();
180 } else {
181 iac = estimateZeroSkewness();
182 }
183 } else if (principalPointAtOrigin) { // && !mZeroSkewness
184 iac = estimatePrincipalPointAtOrigin();
185 } else {
186 iac = estimateNoConstraints();
187 }
188
189 if (listener != null) {
190 listener.onEstimateEnd(this);
191 }
192
193 return iac;
194 } finally {
195 locked = false;
196 }
197 }
198
199 /**
200 * Returns type of IAC estimator.
201 *
202 * @return type of IAC estimator.
203 */
204 @Override
205 public ImageOfAbsoluteConicEstimatorType getType() {
206 return ImageOfAbsoluteConicEstimatorType.LMSE_IAC_ESTIMATOR;
207 }
208
209 /**
210 * Estimates Image of Absolute Conic (IAC) without constraints.
211 *
212 * @return estimated IAC.
213 * @throws ImageOfAbsoluteConicEstimatorException if an error occurs during
214 * estimation, usually because repeated homographies are
215 * provided, or homographies corresponding to degenerate
216 * camera movements such as pure parallel translations
217 * where no additional data is really provided.
218 */
219 private ImageOfAbsoluteConic estimateNoConstraints() throws ImageOfAbsoluteConicEstimatorException {
220
221 try {
222 final var nHomographies = homographies.size();
223
224 final Matrix a;
225 if (isLMSESolutionAllowed()) {
226 // initialize new matrix to zero when LMSE is enabled
227 a = new Matrix(2 * nHomographies, 6);
228 } else {
229 // When LMSE is disabled, initialize new matrix to zero only with
230 // 5 equations
231 a = new Matrix(MIN_REQUIRED_EQUATIONS, 6);
232 }
233
234 var counter = 0;
235 ProjectiveTransformation2D t = null;
236 final var h = new Matrix(ProjectiveTransformation2D.HOM_COORDS, ProjectiveTransformation2D.HOM_COORDS);
237 // elements ij of homography (last column is not required)
238 double h11;
239 double h12;
240 double h21;
241 double h22;
242 double h31;
243 double h32;
244 double rowNorm;
245 for (final var homography : homographies) {
246 // convert homography into projective so it can be normalized
247 homography.asMatrix(h);
248 if (t == null) {
249 t = new ProjectiveTransformation2D(h);
250 } else {
251 t.setT(h);
252 }
253
254 // normalize
255 t.normalize();
256
257 // obtain elements of projective transformation matrix
258 // there is no need to retrieve internal matrix h, as we already
259 // hold a reference
260 h11 = h.getElementAt(0, 0);
261 h12 = h.getElementAt(0, 1);
262
263 h21 = h.getElementAt(1, 0);
264 h22 = h.getElementAt(1, 1);
265
266 h31 = h.getElementAt(2, 0);
267 h32 = h.getElementAt(2, 1);
268
269 // fill first equation
270 a.setElementAt(counter, 0, h11 * h12);
271 a.setElementAt(counter, 1, h11 * h22 + h21 * h12);
272 a.setElementAt(counter, 2, h21 * h22);
273 a.setElementAt(counter, 3, h11 * h32 + h31 * h12);
274 a.setElementAt(counter, 4, h21 * h32 + h31 * h22);
275 a.setElementAt(counter, 5, h31 * h32);
276
277 // normalize row
278 rowNorm = Math.sqrt(Math.pow(a.getElementAt(counter, 0), 2.0)
279 + Math.pow(a.getElementAt(counter, 1), 2.0)
280 + Math.pow(a.getElementAt(counter, 2), 2.0)
281 + Math.pow(a.getElementAt(counter, 3), 2.0)
282 + Math.pow(a.getElementAt(counter, 4), 2.0)
283 + Math.pow(a.getElementAt(counter, 5), 2.0));
284
285 a.setElementAt(counter, 0, a.getElementAt(counter, 0) / rowNorm);
286 a.setElementAt(counter, 1, a.getElementAt(counter, 1) / rowNorm);
287 a.setElementAt(counter, 2, a.getElementAt(counter, 2) / rowNorm);
288 a.setElementAt(counter, 3, a.getElementAt(counter, 3) / rowNorm);
289 a.setElementAt(counter, 4, a.getElementAt(counter, 4) / rowNorm);
290 a.setElementAt(counter, 5, a.getElementAt(counter, 5) / rowNorm);
291
292 counter++;
293
294 // in case we want an exact solution (up to scale) when LMSE is
295 // disabled, we stop after 5 equations
296 if (!isLMSESolutionAllowed() && (counter >= MIN_REQUIRED_EQUATIONS)) {
297 break;
298 }
299
300 // fill second equation
301 a.setElementAt(counter, 0, Math.pow(h11, 2.0) - Math.pow(h12, 2.0));
302 a.setElementAt(counter, 1, 2.0 * (h11 * h21 - h12 * h22));
303 a.setElementAt(counter, 2, Math.pow(h21, 2.0) - Math.pow(h22, 2.0));
304 a.setElementAt(counter, 3, 2.0 * (h11 * h31 - h12 * h32));
305 a.setElementAt(counter, 4, 2.0 * (h21 * h31 - h22 * h32));
306 a.setElementAt(counter, 5, Math.pow(h31, 2.0) - Math.pow(h32, 2.0));
307
308 // normalize row
309 rowNorm = Math.sqrt(Math.pow(a.getElementAt(counter, 0), 2.0)
310 + Math.pow(a.getElementAt(counter, 1), 2.0)
311 + Math.pow(a.getElementAt(counter, 2), 2.0)
312 + Math.pow(a.getElementAt(counter, 3), 2.0)
313 + Math.pow(a.getElementAt(counter, 4), 2.0)
314 + Math.pow(a.getElementAt(counter, 5), 2.0));
315
316 a.setElementAt(counter, 0, a.getElementAt(counter, 0) / rowNorm);
317 a.setElementAt(counter, 1, a.getElementAt(counter, 1) / rowNorm);
318 a.setElementAt(counter, 2, a.getElementAt(counter, 2) / rowNorm);
319 a.setElementAt(counter, 3, a.getElementAt(counter, 3) / rowNorm);
320 a.setElementAt(counter, 4, a.getElementAt(counter, 4) / rowNorm);
321 a.setElementAt(counter, 5, a.getElementAt(counter, 5) / rowNorm);
322
323 counter++;
324 }
325
326 final var decomposer = new SingularValueDecomposer(a);
327 decomposer.decompose();
328
329 if (decomposer.getNullity() > 1) {
330 // homographies constitute a degenerate camera movement.
331 // A linear combination of possible IAC's exist (i.e. solution is
332 // not unique up to scale)
333 throw new ImageOfAbsoluteConicEstimatorException();
334 }
335
336 final var v = decomposer.getV();
337
338 // use last column of V as IAC vector
339
340 // the last column of V contains IAC matrix (B), which is symmetric
341 // and positive definite, ordered as follows: B11, B12, B22, B13,
342 // B23, B33
343 final var b11 = v.getElementAt(0, 5);
344 final var b12 = v.getElementAt(1, 5);
345 final var b22 = v.getElementAt(2, 5);
346 final var b13 = v.getElementAt(3, 5);
347 final var b23 = v.getElementAt(4, 5);
348 final var b33 = v.getElementAt(5, 5);
349
350 // A conic is defined as [A B D]
351 // [B C E]
352 // [D E F]
353 return new ImageOfAbsoluteConic(b11, b12, b22, b13, b23, b33);
354 } catch (final AlgebraException e) {
355 throw new ImageOfAbsoluteConicEstimatorException(e);
356 }
357 }
358
359 /**
360 * Estimates Image of Absolute Conic (IAC) assuming that skewness is zero.
361 *
362 * @return estimated IAC
363 * @throws ImageOfAbsoluteConicEstimatorException if an error occurs during
364 * estimation, usually because repeated homographies are
365 * provided, or homographies corresponding to degenerate
366 * camera movements such as pure parallel translations
367 * where no additional data is really provided
368 */
369 private ImageOfAbsoluteConic estimateZeroSkewness() throws ImageOfAbsoluteConicEstimatorException {
370 try {
371 final var nHomographies = homographies.size();
372
373 final Matrix a;
374 if (isLMSESolutionAllowed()) {
375 // initialize new matrix to zero when LMSE is enabled
376 a = new Matrix(2 * nHomographies, 5);
377 } else {
378 // When LMSE is disabled, initialize new matrix to zero only with
379 // 4 equations
380 a = new Matrix(MIN_REQUIRED_EQUATIONS - 1, 5);
381 }
382
383 var counter = 0;
384 ProjectiveTransformation2D t = null;
385 final var h = new Matrix(ProjectiveTransformation2D.HOM_COORDS, ProjectiveTransformation2D.HOM_COORDS);
386 // elements ij of homography (last column is not required)
387 double h11;
388 double h12;
389 double h21;
390 double h22;
391 double h31;
392 double h32;
393 double rowNorm;
394 for (final var homography : homographies) {
395 // convert homography into projective so it can be normalized
396 homography.asMatrix(h);
397 if (t == null) {
398 t = new ProjectiveTransformation2D(h);
399 } else {
400 t.setT(h);
401 }
402
403 // normalize
404 t.normalize();
405
406 // obtain elements of projective transformation matrix
407 // there is no need to retrieve internal matrix h, as we already
408 // hold a reference
409 h11 = h.getElementAt(0, 0);
410 h12 = h.getElementAt(0, 1);
411
412 h21 = h.getElementAt(1, 0);
413 h22 = h.getElementAt(1, 1);
414
415 h31 = h.getElementAt(2, 0);
416 h32 = h.getElementAt(2, 1);
417
418 // fill first equation
419 a.setElementAt(counter, 0, h11 * h12);
420 a.setElementAt(counter, 1, h21 * h22);
421 a.setElementAt(counter, 2, h11 * h32 + h31 * h12);
422 a.setElementAt(counter, 3, h21 * h32 + h31 * h22);
423 a.setElementAt(counter, 4, h31 * h32);
424
425 // normalize row
426 rowNorm = Math.sqrt(Math.pow(a.getElementAt(counter, 0), 2.0)
427 + Math.pow(a.getElementAt(counter, 1), 2.0)
428 + Math.pow(a.getElementAt(counter, 2), 2.0)
429 + Math.pow(a.getElementAt(counter, 3), 2.0)
430 + Math.pow(a.getElementAt(counter, 4), 2.0));
431
432 a.setElementAt(counter, 0, a.getElementAt(counter, 0) / rowNorm);
433 a.setElementAt(counter, 1, a.getElementAt(counter, 1) / rowNorm);
434 a.setElementAt(counter, 2, a.getElementAt(counter, 2) / rowNorm);
435 a.setElementAt(counter, 3, a.getElementAt(counter, 3) / rowNorm);
436 a.setElementAt(counter, 4, a.getElementAt(counter, 4) / rowNorm);
437
438 counter++;
439
440 // fill second equation
441 a.setElementAt(counter, 0, Math.pow(h11, 2.0) - Math.pow(h12, 2.0));
442 a.setElementAt(counter, 1, Math.pow(h21, 2.0) - Math.pow(h22, 2.0));
443 a.setElementAt(counter, 2, 2.0 * (h11 * h31 - h12 * h32));
444 a.setElementAt(counter, 3, 2.0 * (h21 * h31 - h22 * h32));
445 a.setElementAt(counter, 4, Math.pow(h31, 2.0) - Math.pow(h32, 2.0));
446
447 // normalize row
448 rowNorm = Math.sqrt(Math.pow(a.getElementAt(counter, 0), 2.0)
449 + Math.pow(a.getElementAt(counter, 1), 2.0)
450 + Math.pow(a.getElementAt(counter, 2), 2.0)
451 + Math.pow(a.getElementAt(counter, 3), 2.0)
452 + Math.pow(a.getElementAt(counter, 4), 2.0));
453
454 a.setElementAt(counter, 0, a.getElementAt(counter, 0) / rowNorm);
455 a.setElementAt(counter, 1, a.getElementAt(counter, 1) / rowNorm);
456 a.setElementAt(counter, 2, a.getElementAt(counter, 2) / rowNorm);
457 a.setElementAt(counter, 3, a.getElementAt(counter, 3) / rowNorm);
458 a.setElementAt(counter, 4, a.getElementAt(counter, 4) / rowNorm);
459
460 counter++;
461
462 // in case we want an exact solution (up to scale) when LMSE is
463 // disabled, we stop after 4 equations
464 if (!isLMSESolutionAllowed() && (counter >= (MIN_REQUIRED_EQUATIONS - 1))) {
465 break;
466 }
467 }
468
469 final var decomposer = new SingularValueDecomposer(a);
470 decomposer.decompose();
471
472 if (decomposer.getNullity() > 1) {
473 // homographies constitute a degenerate camera movement.
474 // A linear combination of possible IAC's exist (i.e. solution is
475 // not unique up to scale)
476 throw new ImageOfAbsoluteConicEstimatorException();
477 }
478
479 final var v = decomposer.getV();
480
481 // use last column of V as IAC vector
482
483 // the last column of V contains IAC matrix (B), which is symmetric
484 // and positive definite, ordered as follows: B11, B12, B22, B13,
485 // B23, B33
486 final var b11 = v.getElementAt(0, 4);
487 final var b22 = v.getElementAt(1, 4);
488 final var b13 = v.getElementAt(2, 4);
489 final var b23 = v.getElementAt(3, 4);
490 final var b33 = v.getElementAt(4, 4);
491
492 // A conic is defined as [A B D]
493 // [B C E]
494 // [D E F]
495 // Since skewness is zero b12 = B = 0.0
496 return new ImageOfAbsoluteConic(b11, 0.0, b22, b13, b23, b33);
497 } catch (final AlgebraException e) {
498 throw new ImageOfAbsoluteConicEstimatorException(e);
499 }
500 }
501
502 /**
503 * Estimates Image of Absolute Conic (IAC) assuming that principal point is
504 * located at origin of coordinates.
505 *
506 * @return estimated IAC
507 * @throws ImageOfAbsoluteConicEstimatorException if an error occurs during
508 * estimation, usually because repeated homographies are
509 * provided, or homographies corresponding to degenerate
510 * camera movements such as pure parallel translations
511 * where no additional data is really provided
512 */
513 private ImageOfAbsoluteConic estimatePrincipalPointAtOrigin() throws ImageOfAbsoluteConicEstimatorException {
514
515 try {
516 final var nHomographies = homographies.size();
517
518 final Matrix a;
519 if (isLMSESolutionAllowed()) {
520 // initialize new matrix to zero when LMSE is enabled
521 a = new Matrix(2 * nHomographies, 4);
522 } else {
523 // When LMSE is disabled, initialize new matrix to zero only with
524 // 2 equations
525 a = new Matrix(MIN_REQUIRED_EQUATIONS - 2, 4);
526 }
527
528 var counter = 0;
529 ProjectiveTransformation2D t = null;
530 final var h = new Matrix(ProjectiveTransformation2D.HOM_COORDS, ProjectiveTransformation2D.HOM_COORDS);
531 // elements ij of homography (last column is not required)
532 double h11;
533 double h12;
534 double h21;
535 double h22;
536 double h31;
537 double h32;
538 double rowNorm;
539 for (final var homography : homographies) {
540 // convert homography into projective so it can be normalized
541 homography.asMatrix(h);
542 if (t == null) {
543 t = new ProjectiveTransformation2D(h);
544 } else {
545 t.setT(h);
546 }
547
548 // normalize
549 t.normalize();
550
551 // obtain elements of projective transformation matrix
552 // there is no need to retrieve internal matrix h, as we already
553 // hold a reference
554 h11 = h.getElementAt(0, 0);
555 h12 = h.getElementAt(0, 1);
556
557 h21 = h.getElementAt(1, 0);
558 h22 = h.getElementAt(1, 1);
559
560 h31 = h.getElementAt(2, 0);
561 h32 = h.getElementAt(2, 1);
562
563 // fill first equation
564 a.setElementAt(counter, 0, h11 * h12);
565 a.setElementAt(counter, 1, h11 * h22 + h21 * h12);
566 a.setElementAt(counter, 2, h21 * h22);
567 a.setElementAt(counter, 3, h31 * h32);
568
569 // normalize row
570 rowNorm = Math.sqrt(Math.pow(a.getElementAt(counter, 0), 2.0)
571 + Math.pow(a.getElementAt(counter, 1), 2.0)
572 + Math.pow(a.getElementAt(counter, 2), 2.0)
573 + Math.pow(a.getElementAt(counter, 3), 2.0));
574
575 a.setElementAt(counter, 0, a.getElementAt(counter, 0) / rowNorm);
576 a.setElementAt(counter, 1, a.getElementAt(counter, 1) / rowNorm);
577 a.setElementAt(counter, 2, a.getElementAt(counter, 2) / rowNorm);
578 a.setElementAt(counter, 3, a.getElementAt(counter, 3) / rowNorm);
579
580 counter++;
581
582 // in case we want an exact solution (up to scale) when LMSE is
583 // disabled, we stop after 2 equations
584 if (!isLMSESolutionAllowed() && (counter >= MIN_REQUIRED_EQUATIONS - 2)) {
585 break;
586 }
587
588 // fill second equation
589 a.setElementAt(counter, 0, Math.pow(h11, 2.0) - Math.pow(h12, 2.0));
590 a.setElementAt(counter, 1, 2.0 * (h11 * h21 - h12 * h22));
591 a.setElementAt(counter, 2, Math.pow(h21, 2.0) - Math.pow(h22, 2.0));
592 a.setElementAt(counter, 3, Math.pow(h31, 2.0) - Math.pow(h32, 2.0));
593
594 // normalize row
595 rowNorm = Math.sqrt(Math.pow(a.getElementAt(counter, 0), 2.0)
596 + Math.pow(a.getElementAt(counter, 1), 2.0)
597 + Math.pow(a.getElementAt(counter, 2), 2.0)
598 + Math.pow(a.getElementAt(counter, 3), 2.0));
599
600 a.setElementAt(counter, 0, a.getElementAt(counter, 0) / rowNorm);
601 a.setElementAt(counter, 1, a.getElementAt(counter, 1) / rowNorm);
602 a.setElementAt(counter, 2, a.getElementAt(counter, 2) / rowNorm);
603 a.setElementAt(counter, 3, a.getElementAt(counter, 3) / rowNorm);
604
605 counter++;
606 }
607
608 final var decomposer = new SingularValueDecomposer(a);
609 decomposer.decompose();
610
611 if (decomposer.getNullity() > 1) {
612 // homographies constitute a degenerate camera movement.
613 // A linear combination of possible IAC's exist (i.e. solution is
614 // not unique up to scale)
615 throw new ImageOfAbsoluteConicEstimatorException();
616 }
617
618 final var v = decomposer.getV();
619
620 // use last column of V as IAC vector
621
622 // the last column of V contains IAC matrix (B), which is symmetric
623 // and positive definite, ordered as follows: B11, B12, B22, B13,
624 // B23, B33
625 final var b11 = v.getElementAt(0, 3);
626 final var b12 = v.getElementAt(1, 3);
627 final var b22 = v.getElementAt(2, 3);
628 final var b33 = v.getElementAt(3, 3);
629
630 // A conic is defined as [A B D]
631 // [B C E]
632 // [D E F]
633 // Since principal point is at origin of coordinates
634 // b13 = D = 0.0, b23 = E = 0.0
635 return new ImageOfAbsoluteConic(b11, b12, b22, 0.0, 0.0, b33);
636 } catch (final AlgebraException e) {
637 throw new ImageOfAbsoluteConicEstimatorException(e);
638 }
639 }
640
641 /**
642 * Estimates Image of Absolute Conic (IAC) assuming that skewness is zero
643 * and that principal point is located at origin of coordinates.
644 *
645 * @return estimated IAC
646 * @throws ImageOfAbsoluteConicEstimatorException if an error occurs during
647 * estimation, usually because repeated homographies are
648 * provided, or homographies corresponding to degenerate
649 * camera movements such as pure parallel translations
650 * where no additional data is really provided
651 */
652 private ImageOfAbsoluteConic estimateZeroSkewnessAndPrincipalPointAtOrigin()
653 throws ImageOfAbsoluteConicEstimatorException {
654
655 try {
656 final var nHomographies = homographies.size();
657
658 final Matrix a;
659 if (isLMSESolutionAllowed()) {
660 // initialize new matrix to zero when LMSE is enabled
661 a = new Matrix(2 * nHomographies, 3);
662 } else {
663 // When LMSE is disabled, initialize new matrix to zero only with
664 // 2 equations
665 a = new Matrix(MIN_REQUIRED_EQUATIONS - 3, 3);
666 }
667
668 var counter = 0;
669 ProjectiveTransformation2D t = null;
670 final var h = new Matrix(ProjectiveTransformation2D.HOM_COORDS, ProjectiveTransformation2D.HOM_COORDS);
671 // elements ij of homography (last column is not required)
672 double h11;
673 double h12;
674 double h21;
675 double h22;
676 double h31;
677 double h32;
678 double rowNorm;
679 for (final var homography : homographies) {
680 // convert homography into projective so it can be normalized
681 homography.asMatrix(h);
682 if (t == null) {
683 t = new ProjectiveTransformation2D(h);
684 } else {
685 t.setT(h);
686 }
687
688 // normalize
689 t.normalize();
690
691 // obtain elements of projective transformation matrix
692 // there is no need to retrieve internal matrix h, as we already
693 // hold a reference
694 h11 = h.getElementAt(0, 0);
695 h12 = h.getElementAt(0, 1);
696
697 h21 = h.getElementAt(1, 0);
698 h22 = h.getElementAt(1, 1);
699
700 h31 = h.getElementAt(2, 0);
701 h32 = h.getElementAt(2, 1);
702
703 // fill first equation
704 a.setElementAt(counter, 0, h11 * h12);
705 a.setElementAt(counter, 1, h21 * h22);
706 a.setElementAt(counter, 2, h31 * h32);
707
708 // normalize row
709 rowNorm = Math.sqrt(Math.pow(a.getElementAt(counter, 0), 2.0)
710 + Math.pow(a.getElementAt(counter, 1), 2.0)
711 + Math.pow(a.getElementAt(counter, 2), 2.0));
712
713 a.setElementAt(counter, 0, a.getElementAt(counter, 0) / rowNorm);
714 a.setElementAt(counter, 1, a.getElementAt(counter, 1) / rowNorm);
715 a.setElementAt(counter, 2, a.getElementAt(counter, 2) / rowNorm);
716
717 counter++;
718
719 // fill second equation
720 a.setElementAt(counter, 0, Math.pow(h11, 2.0) - Math.pow(h12, 2.0));
721 a.setElementAt(counter, 1, Math.pow(h21, 2.0) - Math.pow(h22, 2.0));
722 a.setElementAt(counter, 2, Math.pow(h31, 2.0) - Math.pow(h32, 2.0));
723
724 // normalize row
725 rowNorm = Math.sqrt(Math.pow(a.getElementAt(counter, 0), 2.0)
726 + Math.pow(a.getElementAt(counter, 1), 2.0)
727 + Math.pow(a.getElementAt(counter, 2), 2.0));
728
729 a.setElementAt(counter, 0, a.getElementAt(counter, 0) / rowNorm);
730 a.setElementAt(counter, 1, a.getElementAt(counter, 1) / rowNorm);
731 a.setElementAt(counter, 2, a.getElementAt(counter, 2) / rowNorm);
732
733 counter++;
734
735 // in case we want an exact solution (up to scale) when LMSE is
736 // disabled, we stop after 2 equations
737 if (!isLMSESolutionAllowed() && (counter >= MIN_REQUIRED_EQUATIONS - 3)) {
738 break;
739 }
740 }
741
742 final var decomposer = new SingularValueDecomposer(a);
743 decomposer.decompose();
744
745 if (decomposer.getNullity() > 1) {
746 // homographies constitute a degenerate camera movement.
747 // A linear combination of possible IAC's exist (i.e. solution is
748 // not unique up to scale)
749 throw new ImageOfAbsoluteConicEstimatorException();
750 }
751
752 final var v = decomposer.getV();
753
754 // use last column of V as IAC vector
755
756 // the last column of V contains IAC matrix (B), which is symmetric
757 // and positive definite, ordered as follows: B11, B12, B22, B13,
758 // B23, B33
759 final var b11 = v.getElementAt(0, 2);
760 final var b22 = v.getElementAt(1, 2);
761 final var b33 = v.getElementAt(2, 2);
762
763 // A conic is defined as [A B D]
764 // [B C E]
765 // [D E F]
766 // Since principal point is at origin of coordinates
767 // b12 = B = 0, b13 = D = 0.0, b23 = E = 0.0
768 return new ImageOfAbsoluteConic(b11, 0.0, b22, 0.0, 0.0, b33);
769 } catch (final AlgebraException e) {
770 throw new ImageOfAbsoluteConicEstimatorException(e);
771 }
772 }
773
774 /**
775 * Estimates Image of Absolute Conic (IAC) assuming that skewness is zero
776 * and that aspect ratio of focal distances is known.
777 *
778 * @return estimated IAC
779 * @throws ImageOfAbsoluteConicEstimatorException if an error occurs during
780 * estimation, usually because repeated homographies are
781 * provided, or homographies corresponding to degenerate
782 * camera movements such as pure parallel translations
783 * where no additional data is really provided
784 */
785 private ImageOfAbsoluteConic estimateZeroSkewnessAndKnownFocalDistanceAspectRatio()
786 throws ImageOfAbsoluteConicEstimatorException {
787 try {
788 final var nHomographies = homographies.size();
789
790 final Matrix a;
791 if (isLMSESolutionAllowed()) {
792 // initialize new matrix to zero when LMSE is enabled
793 a = new Matrix(2 * nHomographies, 4);
794 } else {
795 // When LMSE is disabled, initialize new matrix to zero only with
796 // 4 equations
797 a = new Matrix(MIN_REQUIRED_EQUATIONS - 2, 4);
798 }
799
800 final var sqrAspectRatio = Math.pow(focalDistanceAspectRatio, 2.0);
801
802 var counter = 0;
803 ProjectiveTransformation2D t = null;
804 final var h = new Matrix(ProjectiveTransformation2D.HOM_COORDS, ProjectiveTransformation2D.HOM_COORDS);
805 // elements ij of homography (last column is not required)
806 double h11;
807 double h12;
808 double h21;
809 double h22;
810 double h31;
811 double h32;
812 double rowNorm;
813 for (final var homography : homographies) {
814 // convert homography into projective so it can be normalized
815 homography.asMatrix(h);
816 if (t == null) {
817 t = new ProjectiveTransformation2D(h);
818 } else {
819 t.setT(h);
820 }
821
822 // normalize
823 t.normalize();
824
825 // obtain elements of projective transformation matrix
826 // there is no need to retrieve internal matrix h, as we already
827 // hold a reference
828 h11 = h.getElementAt(0, 0);
829 h12 = h.getElementAt(0, 1);
830
831 h21 = h.getElementAt(1, 0);
832 h22 = h.getElementAt(1, 1);
833
834 h31 = h.getElementAt(2, 0);
835 h32 = h.getElementAt(2, 1);
836
837 // fill first equation
838 a.setElementAt(counter, 0, h11 * h12 + h21 * h22 / sqrAspectRatio);
839 a.setElementAt(counter, 1, h11 * h32 + h31 * h12);
840 a.setElementAt(counter, 2, h21 * h32 + h31 * h22);
841 a.setElementAt(counter, 3, h31 * h32);
842
843 // normalize row
844 rowNorm = Math.sqrt(Math.pow(a.getElementAt(counter, 0), 2.0)
845 + Math.pow(a.getElementAt(counter, 1), 2.0)
846 + Math.pow(a.getElementAt(counter, 2), 2.0)
847 + Math.pow(a.getElementAt(counter, 3), 2.0));
848
849 a.setElementAt(counter, 0, a.getElementAt(counter, 0) / rowNorm);
850 a.setElementAt(counter, 1, a.getElementAt(counter, 1) / rowNorm);
851 a.setElementAt(counter, 2, a.getElementAt(counter, 2) / rowNorm);
852 a.setElementAt(counter, 3, a.getElementAt(counter, 3) / rowNorm);
853
854 counter++;
855
856 // in case we want an exact solution (up to scale) when LMSE is
857 // disabled, we stop after 4 equations
858 if (!isLMSESolutionAllowed() && (counter >= (MIN_REQUIRED_EQUATIONS - 2))) {
859 break;
860 }
861
862 // fill second equation
863 a.setElementAt(counter, 0, Math.pow(h11, 2.0) - Math.pow(h12, 2.0)
864 + (Math.pow(h21, 2.0) - Math.pow(h22, 2.0)) / sqrAspectRatio);
865 a.setElementAt(counter, 1, 2.0 * (h11 * h31 - h12 * h32));
866 a.setElementAt(counter, 2, 2.0 * (h21 * h31 - h22 * h32));
867 a.setElementAt(counter, 3, Math.pow(h31, 2.0) - Math.pow(h32, 2.0));
868
869 // normalize row
870 rowNorm = Math.sqrt(Math.pow(a.getElementAt(counter, 0), 2.0)
871 + Math.pow(a.getElementAt(counter, 1), 2.0)
872 + Math.pow(a.getElementAt(counter, 2), 2.0)
873 + Math.pow(a.getElementAt(counter, 3), 2.0));
874
875 a.setElementAt(counter, 0, a.getElementAt(counter, 0) / rowNorm);
876 a.setElementAt(counter, 1, a.getElementAt(counter, 1) / rowNorm);
877 a.setElementAt(counter, 2, a.getElementAt(counter, 2) / rowNorm);
878 a.setElementAt(counter, 3, a.getElementAt(counter, 3) / rowNorm);
879
880 counter++;
881 }
882
883 final var decomposer = new SingularValueDecomposer(a);
884 decomposer.decompose();
885
886 if (decomposer.getNullity() > 1) {
887 // homographies constitute a degenerate camera movement.
888 // A linear combination of possible IAC's exist (i.e. solution is
889 // not unique up to scale)
890 throw new ImageOfAbsoluteConicEstimatorException();
891 }
892
893 final var v = decomposer.getV();
894
895 // use last column of V as IAC vector
896
897 // the last column of V contains IAC matrix (B), which is symmetric
898 // and positive definite, ordered as follows: B11, B12, B22, B13,
899 // B23, B33
900 final var b11 = v.getElementAt(0, 3);
901 final var b13 = v.getElementAt(1, 3);
902 final var b23 = v.getElementAt(2, 3);
903 final var b33 = v.getElementAt(3, 3);
904
905 final var b22 = b11 / sqrAspectRatio;
906
907 // A conic is defined as [A B D]
908 // [B C E]
909 // [D E F]
910 // Since skewness is zero b12 = B = 0.0
911 return new ImageOfAbsoluteConic(b11, 0.0, b22, b13, b23, b33);
912 } catch (final AlgebraException e) {
913 throw new ImageOfAbsoluteConicEstimatorException(e);
914 }
915 }
916
917 /**
918 * Estimates Image of Absolute Conic (IAC) assuming that skewness is zero,
919 * principal point is located at origin of coordinates and that aspect ratio
920 * of focal distances is known.
921 *
922 * @return estimated IAC
923 * @throws ImageOfAbsoluteConicEstimatorException if an error occurs during
924 * estimation, usually because repeated homographies are
925 * provided, or homographies corresponding to degenerate
926 * camera movements such as pure parallel translations
927 * where no additional data is really provided
928 */
929 private ImageOfAbsoluteConic estimateZeroSkewnessPrincipalPointAtOriginAndKnownFocalDistanceAspectRatio()
930 throws ImageOfAbsoluteConicEstimatorException {
931
932 try {
933 final double sqrAspectRatio = Math.pow(focalDistanceAspectRatio, 2.0);
934
935 double b11;
936 double b33;
937
938 ProjectiveTransformation2D t = null;
939 final var h = new Matrix(ProjectiveTransformation2D.HOM_COORDS, ProjectiveTransformation2D.HOM_COORDS);
940 double h11;
941 double h12;
942 double h21;
943 double h22;
944 double h31;
945 double h32;
946
947 if (!isLMSESolutionAllowed()) {
948 // NO LMSE
949
950 // For a single homography we have two equations, but indeed we
951 // only need 1 to solve b11 because b33 is defined up to scale
952 // (i.e. b33 = 1.0)
953
954 // Hence
955 // b11 * (h11 * h12 + h21 * h22 / sqrAspectRatio) + b33 * h31 * h32 = 0
956 // b11 = -b33 * h31 * h32 / (h11 * h12 + h21 * h22 / sqrAspectRatio)
957
958 final var homography = homographies.get(0);
959
960 // convert homography into projective so it can be normalized
961 homography.asMatrix(h);
962 t = new ProjectiveTransformation2D(h);
963
964 // normalize
965 t.normalize();
966
967 // obtain elements of projective transformation matrix
968 // there is no need to retrieve internal matrix h, as we already
969 // hold a reference
970 h11 = h.getElementAt(0, 0);
971 h12 = h.getElementAt(0, 1);
972
973 h21 = h.getElementAt(1, 0);
974 h22 = h.getElementAt(1, 1);
975
976 h31 = h.getElementAt(2, 0);
977 h32 = h.getElementAt(2, 1);
978
979 b33 = 1.0;
980 b11 = -h31 * h32 / (h11 * h12 + h21 * h22 / sqrAspectRatio);
981 } else {
982 final var nHomographies = homographies.size();
983
984 final var a = new Matrix(2 * nHomographies, 2);
985
986 var counter = 0;
987 // elements ij of homography (last column is not required)
988 double rowNorm;
989 for (final var homography : homographies) {
990 // convert homography into projective so it can be normalized
991 homography.asMatrix(h);
992 if (t == null) {
993 t = new ProjectiveTransformation2D(h);
994 } else {
995 t.setT(h);
996 }
997
998 // normalize
999 t.normalize();
1000
1001 // obtain elements of projective transformation matrix
1002 // there is no need to retrieve internal matrix h, as we already
1003 // hold a reference
1004 h11 = h.getElementAt(0, 0);
1005 h12 = h.getElementAt(0, 1);
1006
1007 h21 = h.getElementAt(1, 0);
1008 h22 = h.getElementAt(1, 1);
1009
1010 h31 = h.getElementAt(2, 0);
1011 h32 = h.getElementAt(2, 1);
1012
1013 // fill first equation
1014 a.setElementAt(counter, 0, h11 * h12 + h21 * h22 / sqrAspectRatio);
1015 a.setElementAt(counter, 1, h31 * h32);
1016
1017 // normalize row
1018 rowNorm = Math.sqrt(Math.pow(a.getElementAt(counter, 0), 2.0)
1019 + Math.pow(a.getElementAt(counter, 1), 2.0));
1020
1021 a.setElementAt(counter, 0, a.getElementAt(counter, 0) / rowNorm);
1022 a.setElementAt(counter, 1, a.getElementAt(counter, 1) / rowNorm);
1023
1024 counter++;
1025
1026 // fill second equation
1027 a.setElementAt(counter, 0, Math.pow(h11, 2.0) - Math.pow(h12, 2.0)
1028 + (Math.pow(h21, 2.0) - Math.pow(h22, 2.0)) / sqrAspectRatio);
1029 a.setElementAt(counter, 1, Math.pow(h31, 2.0) - Math.pow(h32, 2.0));
1030
1031 // normalize row
1032 rowNorm = Math.sqrt(Math.pow(a.getElementAt(counter, 0), 2.0)
1033 + Math.pow(a.getElementAt(counter, 1), 2.0));
1034
1035 a.setElementAt(counter, 0, a.getElementAt(counter, 0) / rowNorm);
1036 a.setElementAt(counter, 1, a.getElementAt(counter, 1) / rowNorm);
1037
1038 counter++;
1039 }
1040
1041 final var decomposer = new SingularValueDecomposer(a);
1042 decomposer.decompose();
1043
1044 if (decomposer.getNullity() > 1) {
1045 // homographies constitute a degenerate camera movement.
1046 // A linear combination of possible IAC's exist (i.e.
1047 // solution is not unique up to scale)
1048 throw new ImageOfAbsoluteConicEstimatorException();
1049 }
1050
1051 final var v = decomposer.getV();
1052
1053 // use last column of V as IAC vector
1054
1055 // the last column of V contains IAC matrix (B), which is
1056 // symmetric and positive definite, ordered as follows: B11, B12,
1057 // B22, B13, B23, B33
1058 b11 = v.getElementAt(0, 1);
1059 b33 = v.getElementAt(1, 1);
1060 }
1061
1062 final var b22 = b11 / sqrAspectRatio;
1063
1064 // A conic is defined as [A B D]
1065 // [B C E]
1066 // [D E F]
1067 // Since principal point is at origin of coordinates
1068 // b12 = B = 0, b13 = D = 0.0, b23 = E = 0.0
1069 return new ImageOfAbsoluteConic(b11, 0.0, b22, 0.0, 0.0, b33);
1070 } catch (final AlgebraException e) {
1071 throw new ImageOfAbsoluteConicEstimatorException(e);
1072 }
1073 }
1074
1075 }