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.epipolar.estimators;
17
18 import com.irurueta.algebra.AlgebraException;
19 import com.irurueta.algebra.Complex;
20 import com.irurueta.algebra.Matrix;
21 import com.irurueta.algebra.SingularValueDecomposer;
22 import com.irurueta.algebra.Utils;
23 import com.irurueta.ar.epipolar.FundamentalMatrix;
24 import com.irurueta.ar.epipolar.InvalidFundamentalMatrixException;
25 import com.irurueta.geometry.Point2D;
26 import com.irurueta.geometry.ProjectiveTransformation2D;
27 import com.irurueta.geometry.estimators.LockedException;
28 import com.irurueta.geometry.estimators.NormalizerException;
29 import com.irurueta.geometry.estimators.NotReadyException;
30 import com.irurueta.geometry.estimators.Point2DNormalizer;
31 import com.irurueta.numerical.NumericalException;
32 import com.irurueta.numerical.roots.FirstDegreePolynomialRootsEstimator;
33 import com.irurueta.numerical.roots.SecondDegreePolynomialRootsEstimator;
34 import com.irurueta.numerical.roots.ThirdDegreePolynomialRootsEstimator;
35
36 import java.util.ArrayList;
37 import java.util.List;
38
39 /**
40 * Non-robust fundamental matrix estimator that uses 7 matched 2D points on
41 * left and right views.
42 * Although this algorithm requires one less point than the 8 points algorithm,
43 * it might return up to three different fundamental matrices that must be
44 * later checked (i.e. using a robust algorithm to maximize inliers) so that
45 * the correct solution is picked.
46 * If the correct solution is found, it typically obtains results with higher
47 * accuracy than the 8 points algorithm, because rank-2 is not approximated
48 * using SVD, and rather it is accurately enforced.
49 */
50 public class SevenPointsFundamentalMatrixEstimator extends FundamentalMatrixEstimator {
51
52 /**
53 * Constant indicating that by default an LMSE solution is not allowed.
54 */
55 public static final boolean DEFAULT_ALLOW_LMSE_SOLUTION = false;
56
57 /**
58 * Minimum number of matched 2D points to start the estimation.
59 */
60 public static final int MIN_REQUIRED_POINTS = 7;
61
62 /**
63 * Indicates if by default provided point correspondences are normalized to
64 * increase the accuracy of the estimation.
65 */
66 public static final boolean DEFAULT_NORMALIZE_POINT_CORRESPONDENCES = true;
67
68 /**
69 * Tiniest value closest to zero.
70 */
71 public static final double EPS = Double.MIN_VALUE;
72
73 /**
74 * Indicates whether an LMSE (the Least Mean Square Error) solution is allowed
75 * or not. When an LMSE solution is allowed, more than 7 matched points can
76 * be used for fundamental matrix estimation. If LMSE solution is not
77 * allowed then only the 7 former matched points will be taken into account.
78 */
79 private boolean allowLMSESolution;
80
81 /**
82 * Indicates whether provided matched 2D points must be normalized to
83 * increase the accuracy of the estimation.
84 */
85 private boolean normalizePoints;
86
87 /**
88 * Constructor.
89 */
90 public SevenPointsFundamentalMatrixEstimator() {
91 super();
92 allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
93 normalizePoints = DEFAULT_NORMALIZE_POINT_CORRESPONDENCES;
94 }
95
96 /**
97 * Constructor with matched 2D points.
98 *
99 * @param leftPoints 2D points on left view.
100 * @param rightPoints 2D points on right view.
101 * @throws IllegalArgumentException if provided list of points do not
102 * have the same length.
103 */
104 public SevenPointsFundamentalMatrixEstimator(final List<Point2D> leftPoints, final List<Point2D> rightPoints) {
105 super(leftPoints, rightPoints);
106 allowLMSESolution = DEFAULT_ALLOW_LMSE_SOLUTION;
107 normalizePoints = DEFAULT_NORMALIZE_POINT_CORRESPONDENCES;
108 }
109
110 /**
111 * Returns boolean indicating whether an LMSE (the Least Mean Square Error)
112 * solution is allowed or not. When an LMSE solution is allowed, more than 8
113 * matched points can be used for fundamental matrix estimation. If LMSE
114 * solution is not allowed then only the 7 former matched points will be
115 * taken into account.
116 *
117 * @return true if an LMSE solution is allowed, false otherwise.
118 */
119 public boolean isLMSESolutionAllowed() {
120 return allowLMSESolution;
121 }
122
123 /**
124 * Sets boolean indicating whether an LMSE (the Least Mean Square Error)
125 * solution is allowed or not. When an LMSE solution is allowed, more than 8
126 * matched points can be used for fundamental matrix estimation. If LMSE
127 * solution is not allowed then only the 7 former matched points will be
128 * taken into account.
129 *
130 * @param allowed true if an LMSE solution is allowed, false otherwise.
131 * @throws LockedException if this instance is locked because an estimation
132 * is in progress.
133 */
134 public void setLMSESolutionAllowed(final boolean allowed) throws LockedException {
135 if (isLocked()) {
136 throw new LockedException();
137 }
138
139 allowLMSESolution = allowed;
140 }
141
142 /**
143 * Indicates whether provided matched 2D points must be normalized to
144 * increase the accuracy of the estimation.
145 *
146 * @return true if points must be normalized, false otherwise.
147 */
148 public boolean arePointsNormalized() {
149 return normalizePoints;
150 }
151
152 /**
153 * Sets boolean indicating whether provided matched 2D points must be
154 * normalized to increase the accuracy of the estimation.
155 *
156 * @param normalizePoints true if points must be normalized, false
157 * otherwise.
158 * @throws LockedException if this instance is locked because an estimation
159 * is in progress.
160 */
161 public void setPointsNormalized(final boolean normalizePoints) throws LockedException {
162 if (isLocked()) {
163 throw new LockedException();
164 }
165
166 this.normalizePoints = normalizePoints;
167 }
168
169 /**
170 * Returns boolean indicating whether estimator is ready to start the
171 * fundamental matrix estimation.
172 * This is true when the required minimum number of matched points is
173 * provided to obtain a solution and both left and right views have the
174 * same number of matched points.
175 *
176 * @return true if estimator is ready to start the fundamental matrix
177 * estimation, false otherwise.
178 */
179 @Override
180 public boolean isReady() {
181 return leftPoints != null && rightPoints != null && leftPoints.size() == rightPoints.size()
182 && leftPoints.size() >= MIN_REQUIRED_POINTS;
183 }
184
185 /**
186 * Estimates all possible fundamental matrices found using provided points.
187 * Because the algorithm uses 7 points and enforces rank 2 by solving a
188 * third degree polynomial, the algorithm might return 1 real solution
189 * (and 2 imaginary ones which are discarded), 3 real solutions, or 1 real
190 * solution with triple multiplicity.
191 *
192 * @return all possible fundamental matrices found using provided points.
193 * @throws LockedException if estimator is locked doing an estimation.
194 * @throws NotReadyException if estimator is not ready because required
195 * input points have not already been provided.
196 * @throws FundamentalMatrixEstimatorException if configuration of provided
197 * 2D points is degenerate and fundamental matrix
198 * estimation fails.
199 */
200 public List<FundamentalMatrix> estimateAll() throws LockedException, NotReadyException,
201 FundamentalMatrixEstimatorException {
202 final var result = new ArrayList<FundamentalMatrix>();
203 estimateAll(result);
204 return result;
205 }
206
207 /**
208 * Estimates all possible fundamental matrices found using provided points
209 * and adds the result to provided result list.
210 * Because the algorithm uses 7 points and enforces rank 2 by solving a
211 * third degree polynomial, the algorithm might return 1 real solution
212 * (and 2 imaginary ones which are discarded), 3 real solutions, or 1 real
213 * solution with triple multiplicity.
214 *
215 * @param result list where results will be stored.
216 * @throws LockedException if estimator is locked doing an estimation.
217 * @throws NotReadyException if estimator is not ready because required
218 * input points have not already been provided.
219 * @throws FundamentalMatrixEstimatorException if configuration of provided
220 * 2D points is degenerate and fundamental matrix
221 * estimation fails.
222 */
223 @SuppressWarnings("DuplicatedCode")
224 public void estimateAll(final List<FundamentalMatrix> result) throws LockedException, NotReadyException,
225 FundamentalMatrixEstimatorException {
226
227 if (isLocked()) {
228 throw new LockedException();
229 }
230 if (!isReady()) {
231 throw new NotReadyException();
232 }
233
234 locked = true;
235
236 result.clear();
237
238 if (listener != null) {
239 listener.onEstimateStart(this);
240 }
241
242 final var nPoints = leftPoints.size();
243
244 try {
245 ProjectiveTransformation2D leftNormalization = null;
246 ProjectiveTransformation2D rightNormalization = null;
247 final List<Point2D> leftPoints;
248 final List<Point2D> rightPoints;
249 if (normalizePoints) {
250 // normalize points on left view
251 final var normalizer = new Point2DNormalizer(this.leftPoints);
252 normalizer.compute();
253
254 leftNormalization = normalizer.getTransformation();
255
256 // normalize points on right view
257 normalizer.setPoints(this.rightPoints);
258 normalizer.compute();
259
260 rightNormalization = normalizer.getTransformation();
261
262 // normalize to increase accuracy
263 leftNormalization.normalize();
264 rightNormalization.normalize();
265
266 leftPoints = leftNormalization.transformPointsAndReturnNew(this.leftPoints);
267 rightPoints = rightNormalization.transformPointsAndReturnNew(this.rightPoints);
268 } else {
269 leftPoints = this.leftPoints;
270 rightPoints = this.rightPoints;
271 }
272
273 final Matrix a;
274 if (isLMSESolutionAllowed()) {
275 a = new Matrix(nPoints, 9);
276 } else {
277 a = new Matrix(MIN_REQUIRED_POINTS, 9);
278 }
279
280 Point2D leftPoint;
281 Point2D rightPoint;
282 double homLeftX;
283 double homLeftY;
284 double homLeftW;
285 double homRightX;
286 double homRightY;
287 double homRightW;
288 double value0;
289 double value1;
290 double value2;
291 double value3;
292 double value4;
293 double value5;
294 double value6;
295 double value7;
296 double value8;
297 double rowNorm;
298 for (var i = 0; i < nPoints; i++) {
299 leftPoint = leftPoints.get(i);
300 rightPoint = rightPoints.get(i);
301
302 // normalize points to increase accuracy
303 leftPoint.normalize();
304 rightPoint.normalize();
305
306 homLeftX = leftPoint.getHomX();
307 homLeftY = leftPoint.getHomY();
308 homLeftW = leftPoint.getHomW();
309
310 homRightX = rightPoint.getHomX();
311 homRightY = rightPoint.getHomY();
312 homRightW = rightPoint.getHomW();
313
314 // set a row values
315 value0 = homLeftX * homRightX;
316 value1 = homLeftY * homRightX;
317 value2 = homLeftW * homRightX;
318
319 value3 = homLeftX * homRightY;
320 value4 = homLeftY * homRightY;
321 value5 = homLeftW * homRightY;
322
323 value6 = homLeftX * homRightW;
324 value7 = homLeftY * homRightW;
325 value8 = homLeftW * homRightW;
326
327 // normalize row to increase accuracy
328 rowNorm = Math.sqrt(Math.pow(value0, 2.0)
329 + Math.pow(value1, 2.0) + Math.pow(value2, 2.0)
330 + Math.pow(value3, 2.0) + Math.pow(value4, 2.0)
331 + Math.pow(value5, 2.0) + Math.pow(value6, 2.0)
332 + Math.pow(value7, 2.0) + Math.pow(value8, 2.0));
333
334 a.setElementAt(i, 0, value0 / rowNorm);
335 a.setElementAt(i, 1, value1 / rowNorm);
336 a.setElementAt(i, 2, value2 / rowNorm);
337 a.setElementAt(i, 3, value3 / rowNorm);
338 a.setElementAt(i, 4, value4 / rowNorm);
339 a.setElementAt(i, 5, value5 / rowNorm);
340 a.setElementAt(i, 6, value6 / rowNorm);
341 a.setElementAt(i, 7, value7 / rowNorm);
342 a.setElementAt(i, 8, value8 / rowNorm);
343
344 if (!isLMSESolutionAllowed() && i == (MIN_REQUIRED_POINTS - 1)) {
345 break;
346 }
347 }
348
349 final var decomposer = new SingularValueDecomposer(a);
350
351 decomposer.decompose();
352
353 // having 7 points for 9 variables means that rank can be as high as
354 // 7, and nullity must be 2, so that the final fundamental matrix
355 // can be found as a linear combination of the null-space.
356 // If nullity is bigger than 2, then geometry is degenerate, usually
357 // due to co-linearities or co-planarities on projected image points.
358 // In this case we throw an exception
359 if (decomposer.getNullity() > 2) {
360 throw new FundamentalMatrixEstimatorException();
361 }
362
363 final var v = decomposer.getV();
364
365 // The last two column vectors of V contain the "base" matrices
366 // to be used for the retrieval of the true fundamental matrix, since
367 // the fundamental matrix we are looking for will be a linear
368 // combination of such matrices after reshaping the vectors into 3x3
369 // matrices
370 var fundMatrix1 = new Matrix(FundamentalMatrix.FUNDAMENTAL_MATRIX_ROWS,
371 FundamentalMatrix.FUNDAMENTAL_MATRIX_COLS);
372 fundMatrix1.setElementAt(0, 0, v.getElementAt(0, 8));
373 fundMatrix1.setElementAt(0, 1, v.getElementAt(1, 8));
374 fundMatrix1.setElementAt(0, 2, v.getElementAt(2, 8));
375 fundMatrix1.setElementAt(1, 0, v.getElementAt(3, 8));
376 fundMatrix1.setElementAt(1, 1, v.getElementAt(4, 8));
377 fundMatrix1.setElementAt(1, 2, v.getElementAt(5, 8));
378 fundMatrix1.setElementAt(2, 0, v.getElementAt(6, 8));
379 fundMatrix1.setElementAt(2, 1, v.getElementAt(7, 8));
380 fundMatrix1.setElementAt(2, 2, v.getElementAt(8, 8));
381
382 var fundMatrix2 = new Matrix(FundamentalMatrix.FUNDAMENTAL_MATRIX_ROWS,
383 FundamentalMatrix.FUNDAMENTAL_MATRIX_COLS);
384 fundMatrix2.setElementAt(0, 0, v.getElementAt(0, 7));
385 fundMatrix2.setElementAt(0, 1, v.getElementAt(1, 7));
386 fundMatrix2.setElementAt(0, 2, v.getElementAt(2, 7));
387 fundMatrix2.setElementAt(1, 0, v.getElementAt(3, 7));
388 fundMatrix2.setElementAt(1, 1, v.getElementAt(4, 7));
389 fundMatrix2.setElementAt(1, 2, v.getElementAt(5, 7));
390 fundMatrix2.setElementAt(2, 0, v.getElementAt(6, 7));
391 fundMatrix2.setElementAt(2, 1, v.getElementAt(7, 7));
392 fundMatrix2.setElementAt(2, 2, v.getElementAt(8, 7));
393
394 if (normalizePoints && leftNormalization != null) {
395 // denormalize linear combination of fundamental matrices
396 // fundMatrix1 and fundMatrix2
397 final var transposedRightTransformationMatrix = rightNormalization.asMatrix().transposeAndReturnNew();
398 final var leftTransformationMatrix = leftNormalization.asMatrix();
399
400 // compute fundMatrix1 = transposedRightTransformationMatrix *
401 // fundMatrix1 * leftTransformationMatrix
402 fundMatrix1.multiply(leftTransformationMatrix);
403 fundMatrix1 = transposedRightTransformationMatrix.multiplyAndReturnNew(fundMatrix1);
404
405 // normalize by Frobenius norm to increase accuracy after point
406 // de-normalization
407 var norm = Utils.normF(fundMatrix1);
408 fundMatrix1.multiplyByScalar(1.0 / norm);
409
410 // compute fundMatrix2 = transposedRightTransformationMatrix *
411 // fundMatrix2 * leftTransformationMatrix
412 fundMatrix2.multiply(leftTransformationMatrix);
413 transposedRightTransformationMatrix.multiply(fundMatrix2);
414 fundMatrix2 = transposedRightTransformationMatrix;
415
416 // normalize by Frobenius norm to increase accuracy after point
417 // de-normalization
418 norm = Utils.normF(fundMatrix2);
419 fundMatrix2.multiplyByScalar(1.0 / norm);
420 }
421
422 // because fundMatrix1, and fundMatrix2 have been obtained as
423 // columns of V, then its Frobenius norm will be 1 because SVD
424 // already returns normalized singular vectors, and there is no need
425 // to normalize by Frobenius norm if points are NOT normalized
426
427 // The last thing we need to do is to enforce rank 2 on fundamental
428 // matrix, since we know it is always a rank 2 matrix. For that
429 // reason we know that det(F) = 0.
430 // Since the fundamental matrix F is a linear combination of the
431 // two matrices F1, F2 we have found then: F = b * F1 + (1.0 - b) *F2
432 // where b will range from 0 to 1.
433 // Hence: det(b * F1 + (1.0 - b) * F2) = 0
434 // This produces a third degree polynomial as follows:
435
436 // coefficients of polynomial: a*x^3 + b*x^2 + c*x + d
437 var aPoly = 0.0;
438 var bPoly = 0.0;
439 var cPoly = 0.0;
440 var dPoly = 0.0;
441 final var params = new double[4];
442
443 computeParams(fundMatrix1.getElementAt(0, 0),
444 fundMatrix2.getElementAt(0, 0),
445 fundMatrix1.getElementAt(1, 1),
446 fundMatrix2.getElementAt(1, 1),
447 fundMatrix1.getElementAt(2, 2),
448 fundMatrix2.getElementAt(2, 2), params);
449
450 aPoly += params[0];
451 bPoly += params[1];
452 cPoly += params[2];
453 dPoly += params[3];
454
455 computeParams(fundMatrix1.getElementAt(2, 1),
456 fundMatrix2.getElementAt(2, 1),
457 fundMatrix1.getElementAt(1, 0),
458 fundMatrix2.getElementAt(1, 0),
459 fundMatrix1.getElementAt(0, 2),
460 fundMatrix2.getElementAt(0, 2), params);
461
462 aPoly += params[0];
463 bPoly += params[1];
464 cPoly += params[2];
465 dPoly += params[3];
466
467 computeParams(fundMatrix1.getElementAt(2, 0),
468 fundMatrix2.getElementAt(2, 0),
469 fundMatrix1.getElementAt(0, 1),
470 fundMatrix2.getElementAt(0, 1),
471 fundMatrix1.getElementAt(1, 2),
472 fundMatrix2.getElementAt(1, 2), params);
473
474 aPoly += params[0];
475 bPoly += params[1];
476 cPoly += params[2];
477 dPoly += params[3];
478
479 computeParams(fundMatrix1.getElementAt(2, 0),
480 fundMatrix2.getElementAt(2, 0),
481 fundMatrix1.getElementAt(1, 1),
482 fundMatrix2.getElementAt(1, 1),
483 fundMatrix1.getElementAt(0, 2),
484 fundMatrix2.getElementAt(0, 2), params);
485
486 aPoly -= params[0];
487 bPoly -= params[1];
488 cPoly -= params[2];
489 dPoly -= params[3];
490
491 computeParams(fundMatrix1.getElementAt(1, 2),
492 fundMatrix2.getElementAt(1, 2),
493 fundMatrix1.getElementAt(2, 1),
494 fundMatrix2.getElementAt(2, 1),
495 fundMatrix1.getElementAt(0, 0),
496 fundMatrix2.getElementAt(0, 0), params);
497
498 aPoly -= params[0];
499 bPoly -= params[1];
500 cPoly -= params[2];
501 dPoly -= params[3];
502
503 computeParams(fundMatrix1.getElementAt(1, 0),
504 fundMatrix2.getElementAt(1, 0),
505 fundMatrix1.getElementAt(0, 1),
506 fundMatrix2.getElementAt(0, 1),
507 fundMatrix1.getElementAt(2, 2),
508 fundMatrix2.getElementAt(2, 2), params);
509
510 aPoly -= params[0];
511 bPoly -= params[1];
512 cPoly -= params[2];
513 dPoly -= params[3];
514
515 // normalize polynomial coefficients to increase accuracy
516 final var coeffNorm = Math.sqrt(Math.pow(aPoly, 2.0)
517 + Math.pow(bPoly, 2.0) + Math.pow(cPoly, 2.0)
518 + Math.pow(dPoly, 2.0));
519 aPoly /= coeffNorm;
520 bPoly /= coeffNorm;
521 cPoly /= coeffNorm;
522 dPoly /= coeffNorm;
523
524 // store polynomial coefficients into array and find its roots to
525 // enforce det(F) = 0. The solution must be unique and real!
526 params[0] = dPoly;
527 params[1] = cPoly;
528 params[2] = bPoly;
529 params[3] = aPoly;
530
531 var beta1 = 0.0;
532 var beta2 = 0.0;
533 var beta3 = 0.0;
534 var beta1Available = false;
535 var beta2Available = false;
536 var beta3Available = false;
537 final Complex[] roots;
538 final Complex root1;
539 final Complex root2;
540 final Complex root3;
541
542 if (ThirdDegreePolynomialRootsEstimator.isThirdDegree(params)) {
543 // solve third degree polynomial
544 final var estimator = new ThirdDegreePolynomialRootsEstimator(params);
545 estimator.estimate();
546 roots = estimator.getRoots();
547 root1 = roots[0];
548 root2 = roots[1];
549 root3 = roots[2];
550
551 if (Math.abs(root1.getImaginary()) <= EPS) {
552 // 1st root is real, so we keep it
553 beta1 = root1.getReal();
554 beta1Available = true;
555 }
556 if (Math.abs(root2.getImaginary()) <= EPS) {
557 // 2nd root is real, so we keep it
558 beta2 = root2.getReal();
559 beta2Available = true;
560 }
561 if (Math.abs(root3.getImaginary()) <= EPS) {
562 // 3rd root is real, so we keep it
563 beta3 = root3.getReal();
564 beta3Available = true;
565 }
566 } else if (SecondDegreePolynomialRootsEstimator.isSecondDegree(params)) {
567 // solve second degree polynomial
568 final var estimator = new SecondDegreePolynomialRootsEstimator(params);
569 estimator.estimate();
570 roots = estimator.getRoots();
571 root1 = roots[0];
572 root2 = roots[1];
573 if (Math.abs(root1.getImaginary()) <= EPS) {
574 // 1st root is real, so we keep it
575 beta1 = root1.getReal();
576 beta1Available = true;
577 }
578 if (Math.abs(root2.getImaginary()) <= EPS) {
579 // 2nd root is real, so we keep it
580 beta2 = root2.getReal();
581 beta2Available = true;
582 }
583 } else if (FirstDegreePolynomialRootsEstimator.isFirstDegree(params)) {
584 // solve first degree polynomial
585 final var estimator = new FirstDegreePolynomialRootsEstimator(params);
586 estimator.estimate();
587 roots = estimator.getRoots();
588 // this is the only solution and is real
589 // because coefficients are real
590 root1 = roots[0];
591 beta1 = root1.getReal();
592 beta1Available = true;
593 } else {
594 // invalid polynomial degree
595 throw new FundamentalMatrixEstimatorException();
596 }
597
598 if (!beta1Available && !beta2Available && !beta3Available) {
599 // No solution was found
600 throw new FundamentalMatrixEstimatorException();
601 }
602
603 // Once the polynomial is solved we compute the linear combination
604 // F = b * F1 + (1 - b) * F2 which has rank 2 using all available
605 // solutions
606 Matrix fundMatrix;
607 FundamentalMatrix f;
608 // clear previous values
609 result.clear();
610 if (beta1Available) {
611 fundMatrix = fundMatrix1.multiplyByScalarAndReturnNew(beta1).addAndReturnNew(
612 fundMatrix2.multiplyByScalarAndReturnNew(1.0 - beta1));
613 if (enforceRank2(fundMatrix, decomposer)) {
614 throw new FundamentalMatrixEstimatorException();
615 }
616
617 f = new FundamentalMatrix(fundMatrix);
618 result.add(f);
619 }
620 if (beta2Available) {
621 fundMatrix = fundMatrix1.multiplyByScalarAndReturnNew(beta2).addAndReturnNew(
622 fundMatrix2.multiplyByScalarAndReturnNew(1.0 - beta2));
623 if (enforceRank2(fundMatrix, decomposer)) {
624 throw new FundamentalMatrixEstimatorException();
625 }
626
627 f = new FundamentalMatrix(fundMatrix);
628 result.add(f);
629 }
630 if (beta3Available) {
631 fundMatrix = fundMatrix1.multiplyByScalarAndReturnNew(beta3).addAndReturnNew(
632 fundMatrix2.multiplyByScalarAndReturnNew(1.0 - beta3));
633 if (enforceRank2(fundMatrix, decomposer)) {
634 throw new FundamentalMatrixEstimatorException();
635 }
636
637 f = new FundamentalMatrix(fundMatrix);
638 result.add(f);
639 }
640
641 if (listener != null) {
642 listener.onEstimateEnd(this, result.get(0));
643 }
644
645 } catch (final InvalidFundamentalMatrixException | AlgebraException | NumericalException
646 | NormalizerException e) {
647 throw new FundamentalMatrixEstimatorException(e);
648 } finally {
649 locked = false;
650 }
651 }
652
653 /**
654 * Estimates a fundamental matrix using provided lists of matched points on
655 * left and right views.
656 * This method returns a solution only if one possible solution exists.
657 * If more than one solution is available, this method will fail.
658 * Because this algorithm might return more than one solution, it is highly
659 * encouraged to use estimateAll method instead.
660 *
661 * @return a fundamental matrix.
662 * @throws LockedException if estimator is locked doing an estimation.
663 * @throws NotReadyException if estimator is not ready because required
664 * input points have not already been provided.
665 * @throws FundamentalMatrixEstimatorException if configuration of provided
666 * 2D points is degenerate and fundamental matrix
667 * estimation fails or more than one solution exists.
668 */
669 @Override
670 public FundamentalMatrix estimate() throws LockedException, NotReadyException, FundamentalMatrixEstimatorException {
671 final var list = estimateAll();
672 if (list.size() > 1) {
673 throw new FundamentalMatrixEstimatorException();
674 }
675
676 return list.get(0);
677 }
678
679 /**
680 * Returns method of non-robust fundamental matrix estimator.
681 *
682 * @return method of fundamental matrix estimator.
683 */
684 @Override
685 public FundamentalMatrixEstimatorMethod getMethod() {
686 return FundamentalMatrixEstimatorMethod.SEVEN_POINTS_ALGORITHM;
687 }
688
689 /**
690 * Returns minimum number of matched pair of points required to start
691 * the estimation. This implementation requires a minimum of 7 points.
692 *
693 * @return minimum number of matched pair of points required to start
694 * the estimation. Always returns 7.
695 */
696 @Override
697 public int getMinRequiredPoints() {
698 return MIN_REQUIRED_POINTS;
699 }
700
701 /**
702 * Enforces rank 2 into provided matrix.
703 * This method modifies provided matrix.
704 *
705 * @param matrix matrix to be enforced to have rank 2.
706 * @param decomposer an SVD decomposer.
707 * @return false if rank was successfully enforced, true otherwise.
708 * @throws AlgebraException if an error occurs during SVD decomposition
709 * because of numerical instabilities.
710 */
711 private boolean enforceRank2(final Matrix matrix, final SingularValueDecomposer decomposer)
712 throws AlgebraException {
713
714 decomposer.setInputMatrix(matrix);
715 decomposer.decompose();
716
717 final var rank = decomposer.getRank();
718 if (rank > FundamentalMatrix.FUNDAMENTAL_MATRIX_RANK) {
719 // rank needs to be reduced
720 final var u = decomposer.getU();
721 final var w = decomposer.getW();
722 final var v = decomposer.getV();
723
724 // transpose V
725 v.transpose();
726
727 // set last singular value to zero to enforce rank 2
728 w.setElementAt(2, 2, 0.0);
729
730 // compute matrix = U * W * V'
731 w.multiply(v);
732 u.multiply(w);
733 matrix.copyFrom(u);
734 return false;
735 } else {
736 // if rank is 2, rank is ok, otherwise rank is lower than fundamental
737 // matrix rank (rank 1) and estimation has failed because of
738 // co-planarities
739 return rank != FundamentalMatrix.FUNDAMENTAL_MATRIX_RANK;
740 }
741 }
742
743 /**
744 * Computes parameters of third degree polynomial to obtain possible
745 * solutions.
746 *
747 * @param a 1st param.
748 * @param b 2nd param.
749 * @param c 3rd param.
750 * @param d 4th param.
751 * @param e 5th param.
752 * @param f 6th param.
753 * @param out array of length 4 where partial parameters of third degree
754 * polynomial are stored.
755 */
756 private void computeParams(
757 final double a, final double b, final double c, final double d, final double e, final double f,
758 final double[] out) {
759
760 final var ace = a * c * e;
761 final var ade = a * d * e;
762 final var bce = b * c * e;
763 final var bde = b * d * e;
764 final var acf = a * c * f;
765 final var adf = a * d * f;
766 final var bcf = b * c * f;
767 final var bdf = b * d * f;
768
769 out[0] = ace - ade - bce + bde - acf + adf + bcf - bdf;
770 out[1] = ade + bce - 2.0 * bde + acf - 2.0 * adf - 2.0 * bcf + 3.0 * bdf;
771 out[2] = bde + adf + bcf - 3.0 * bdf;
772 out[3] = bdf;
773 }
774 }