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