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;
17
18 import com.irurueta.algebra.AlgebraException;
19 import com.irurueta.algebra.Matrix;
20 import com.irurueta.algebra.SingularValueDecomposer;
21 import com.irurueta.algebra.Utils;
22 import com.irurueta.geometry.*;
23
24 import java.io.Serializable;
25
26 /**
27 * The essential matrix defines the relation between two views in a similar way
28 * that the fundamental matrix does, but taking into account the intrinsic
29 * parameters of the cameras associated to both views. That ways the relation
30 * between their extrinsic parameters (rotation and translation) can be precisely
31 * obtained.
32 */
33 public class EssentialMatrix extends FundamentalMatrix implements Serializable {
34
35 /**
36 * Default threshold to determine that the two non-zero singular values are
37 * equal.
38 */
39 public static final double DEFAULT_SINGULAR_VALUES_THRESHOLD = 1e-8;
40
41 private Rotation3D rotation1;
42 private Rotation3D rotation2;
43
44 private Point2D translation1;
45 private Point2D translation2;
46
47 private boolean possibleRotationsAndTranslationsAvailable;
48
49 /**
50 * Constructor.
51 */
52 public EssentialMatrix() {
53 super();
54 }
55
56 /**
57 * Constructor.
58 *
59 * @param internalMatrix matrix to be set internally.
60 * @param singularValuesThreshold threshold to determine that both singular
61 * values are equal.
62 * @throws InvalidEssentialMatrixException if provided matrix is not 3x3,
63 * does not have rank 2 or its two non-zero singular values
64 * are not equal up to provided threshold.
65 * @throws IllegalArgumentException if provided threshold is negative.
66 */
67 public EssentialMatrix(final Matrix internalMatrix, final double singularValuesThreshold)
68 throws InvalidEssentialMatrixException {
69 super();
70 setInternalMatrix(internalMatrix, singularValuesThreshold);
71 }
72
73 /**
74 * Constructor.
75 *
76 * @param internalMatrix matrix to be set internally.
77 * @throws InvalidEssentialMatrixException if provided matrix is not 3x3,
78 * does not have rank 2 or its two non-zero singular values
79 * are not equal.
80 */
81 public EssentialMatrix(final Matrix internalMatrix) throws InvalidEssentialMatrixException {
82 this(internalMatrix, DEFAULT_SINGULAR_VALUES_THRESHOLD);
83 }
84
85 /**
86 * Constructor from a pair of cameras.
87 *
88 * @param leftCamera camera corresponding to left view.
89 * @param rightCamera camera corresponding to right view.
90 * @param singularValuesThreshold threshold to determine that both singular
91 * values of generated essential matrix are equal.
92 * @throws InvalidPairOfCamerasException if provided cameras do not span a
93 * valid epipolar geometry (i.e. they are set in a degenerate
94 * configuration).
95 * @throws IllegalArgumentException if provided threshold is negative.
96 */
97 public EssentialMatrix(
98 final PinholeCamera leftCamera, final PinholeCamera rightCamera, final double singularValuesThreshold)
99 throws InvalidPairOfCamerasException {
100 super();
101 setFromPairOfCameras(leftCamera, rightCamera, singularValuesThreshold);
102 }
103
104 /**
105 * Constructor from a pair of cameras.
106 *
107 * @param leftCamera camera corresponding to left view.
108 * @param rightCamera camera corresponding to right view.
109 * @throws InvalidPairOfCamerasException if provided cameras do not span a
110 * valid epipolar geometry (i.e. they are set in a degenerate
111 * configuration).
112 */
113 public EssentialMatrix(
114 final PinholeCamera leftCamera, final PinholeCamera rightCamera) throws InvalidPairOfCamerasException {
115 this(leftCamera, rightCamera, DEFAULT_SINGULAR_VALUES_THRESHOLD);
116 }
117
118 /**
119 * Constructor from rotation and translation of the image of world origin
120 * relative to left view camera, which is assumed to be located at origin
121 * of coordinates with no rotation.
122 *
123 * @param rotation rotation of right camera relative to left camera.
124 * @param translation translation of the image of world origin on right
125 * camera relative to left camera.
126 * @param singularValuesThreshold threshold to determine that both singular
127 * values of generated essential matrix are equal.
128 * @throws InvalidRotationAndTranslationException if provided rotation and
129 * translation yield a degenerate epipolar geometry.
130 * @throws IllegalArgumentException if provided threshold is negative.
131 */
132 public EssentialMatrix(
133 final Rotation3D rotation, final Point2D translation, final double singularValuesThreshold)
134 throws InvalidRotationAndTranslationException {
135 super();
136 setFromRotationAndTranslation(rotation, translation, singularValuesThreshold);
137 }
138
139 /**
140 * Constructor from rotation and translation of the image of world origin
141 * relative to left view camera, which is assumed to be located at origin
142 * of coordinates with no rotation.
143 *
144 * @param rotation rotation of right camera relative to left camera.
145 * @param translation translation of the image of world origin on right
146 * camera relative to left camera.
147 * @throws InvalidRotationAndTranslationException if provided rotation and
148 * translation yield a degenerate epipolar geometry.
149 */
150 public EssentialMatrix(
151 final Rotation3D rotation, final Point2D translation) throws InvalidRotationAndTranslationException {
152 this(rotation, translation, DEFAULT_SINGULAR_VALUES_THRESHOLD);
153 }
154
155 /**
156 * Constructor from rotation and translation of the camera center relative
157 * to left view camera, which is assumed to be located at origin of
158 * coordinates with no rotation.
159 *
160 * @param rotation rotation of right camera relative to left camera.
161 * @param cameraCenter camera center of right camera relative to left camera.
162 * @param singularValuesThreshold threshold to determine that both singular
163 * values of generated essential matrix are equal.
164 * @throws InvalidRotationAndTranslationException if provided rotation and
165 * translation yield a degenerate epipolar geometry.
166 * @throws IllegalArgumentException if provided threshold is negative.
167 */
168 public EssentialMatrix(
169 final Rotation3D rotation, final Point3D cameraCenter, final double singularValuesThreshold)
170 throws InvalidRotationAndTranslationException {
171 setFromRotationAndCameraCenter(rotation, cameraCenter, singularValuesThreshold);
172 }
173
174 /**
175 * Constructor from rotation and translation of the camera center relative
176 * to left view camera, which is assumed to be located at origin of
177 * coordinates with no rotation.
178 *
179 * @param rotation rotation of right camera relative to left camera.
180 * @param cameraCenter camera center of right camera relative to left camera.
181 * @throws InvalidRotationAndTranslationException if provided rotation and
182 * translation yield a degenerate epipolar geometry.
183 */
184 public EssentialMatrix(
185 final Rotation3D rotation, final Point3D cameraCenter) throws InvalidRotationAndTranslationException {
186 this(rotation, cameraCenter, DEFAULT_SINGULAR_VALUES_THRESHOLD);
187 }
188
189 /**
190 * Constructor from fundamental matrix and intrinsic camera parameters.
191 *
192 * @param fundamentalMatrix a fundamental matrix.
193 * @param leftIntrinsicParameters intrinsic camera parameters of left view.
194 * @param rightIntrinsicParameters intrinsic camera parameters of right view.
195 * @throws InvalidPairOfIntrinsicParametersException if provided intrinsic
196 * parameters generate an invalid essential matrix.
197 */
198 public EssentialMatrix(
199 final FundamentalMatrix fundamentalMatrix,
200 final PinholeCameraIntrinsicParameters leftIntrinsicParameters,
201 final PinholeCameraIntrinsicParameters rightIntrinsicParameters)
202 throws InvalidPairOfIntrinsicParametersException {
203 setFromFundamentalMatrixAndIntrinsics(fundamentalMatrix, leftIntrinsicParameters, rightIntrinsicParameters);
204 }
205
206 /**
207 * Sets internal matrix associated to this instance.
208 * This method makes a copy of provided matrix.
209 *
210 * @param internalMatrix matrix to be assigned to this instance.
211 * @throws InvalidEssentialMatrixException if provided matrix is not 3x3,
212 * does not have rank 2 or its two non-zero singular values
213 * are not equal.
214 */
215 @Override
216 public final void setInternalMatrix(final Matrix internalMatrix) throws InvalidEssentialMatrixException {
217 setInternalMatrix(internalMatrix, DEFAULT_SINGULAR_VALUES_THRESHOLD);
218 }
219
220 /**
221 * Sets internal matrix associated to this instance.
222 * This method makes a copy of provided matrix.
223 *
224 * @param internalMatrix matrix to be assigned to this instance.
225 * @param singularValuesThreshold threshold to determine that both
226 * singular values are equal.
227 * @throws IllegalArgumentException if provided threshold is negative.
228 * @throws InvalidEssentialMatrixException if provided matrix is not 3x3,
229 * does not have rank 2 or its two non-zero singular values
230 * are not equal up to provided threshold.
231 */
232 public final void setInternalMatrix(
233 final Matrix internalMatrix, final double singularValuesThreshold) throws InvalidEssentialMatrixException {
234 if (!isValidInternalMatrix(internalMatrix, singularValuesThreshold)) {
235 throw new InvalidEssentialMatrixException();
236 }
237
238 // because provided matrix is valid, we proceed to setting it
239 this.internalMatrix = new Matrix(internalMatrix);
240 normalized = false;
241 leftEpipole = rightEpipole = null;
242 }
243
244 /**
245 * Returns a boolean indicating whether provided matrix is a valid essential
246 * matrix (i.e. has size 3x3, rank 2 and two non-zero and equal singular
247 * values).
248 *
249 * @param internalMatrix matrix to be checked.
250 * @return true if provided matrix is a valid essential matrix, false
251 * otherwise.
252 */
253 public static boolean isValidInternalMatrix(final Matrix internalMatrix) {
254 return isValidInternalMatrix(internalMatrix, DEFAULT_SINGULAR_VALUES_THRESHOLD);
255 }
256
257 /**
258 * Returns a boolean indicating whether provided matrix is a valid
259 * essential matrix (i.e. has size 3x3, rank 2 and his two non-zero singular
260 * values are equal up to provided threshold).
261 *
262 * @param internalMatrix matrix to be checked.
263 * @param singularValuesThreshold threshold to determine that both singular
264 * values are equal.
265 * @return true if provided matrix is a valid essential matrix, false
266 * otherwise.
267 * @throws IllegalArgumentException if provided threshold is negative.
268 */
269 public static boolean isValidInternalMatrix(final Matrix internalMatrix, final double singularValuesThreshold) {
270 if (singularValuesThreshold < 0) {
271 throw new IllegalArgumentException();
272 }
273
274 if (internalMatrix.getColumns() != FUNDAMENTAL_MATRIX_COLS
275 || internalMatrix.getRows() != FUNDAMENTAL_MATRIX_ROWS) {
276 return false;
277 }
278
279 try {
280 final var decomposer = new SingularValueDecomposer(internalMatrix);
281
282 decomposer.decompose();
283
284 final var rankEssential = decomposer.getRank();
285
286 if (rankEssential != FUNDAMENTAL_MATRIX_RANK) {
287 return false;
288 }
289
290 final var singularValues = decomposer.getSingularValues();
291
292 return (Math.abs(singularValues[0] - singularValues[1]) <= singularValuesThreshold);
293 } catch (final AlgebraException e) {
294 return false;
295 }
296 }
297
298 /**
299 * Sets essential matrix from provided a pair of cameras.
300 *
301 * @param leftCamera camera corresponding to left view.
302 * @param rightCamera camera corresponding to right view.
303 * @throws InvalidPairOfCamerasException if provided cameras do not span a
304 * valid epipolar geometry (i.e. they are set in a degenerate
305 * configuration).
306 */
307 @Override
308 public void setFromPairOfCameras(final PinholeCamera leftCamera, final PinholeCamera rightCamera)
309 throws InvalidPairOfCamerasException {
310 setFromPairOfCameras(leftCamera, rightCamera, DEFAULT_SINGULAR_VALUES_THRESHOLD);
311 }
312
313 /**
314 * Sets essential matrix from provided a pair of cameras.
315 *
316 * @param leftCamera camera corresponding to left view.
317 * @param rightCamera camera corresponding to right view.
318 * @param singularValuesThreshold threshold to determine that both singular
319 * values of generated essential matrix are equal.
320 * @throws InvalidPairOfCamerasException if provided cameras do not span a
321 * valid epipolar geometry (i.e. they are set in a degenerate
322 * configuration).
323 * @throws IllegalArgumentException if provided threshold is negative.
324 */
325 public final void setFromPairOfCameras(
326 final PinholeCamera leftCamera, final PinholeCamera rightCamera, final double singularValuesThreshold)
327 throws InvalidPairOfCamerasException {
328
329 if (singularValuesThreshold < 0) {
330 throw new IllegalArgumentException();
331 }
332
333 try {
334 // normalize cameras to increase accuracy of results and fix their signs
335 // if needed
336 leftCamera.normalize();
337 rightCamera.normalize();
338
339 if (!leftCamera.isCameraSignFixed()) {
340 leftCamera.fixCameraSign();
341 }
342 if (!rightCamera.isCameraSignFixed()) {
343 rightCamera.fixCameraSign();
344 }
345
346 // Obtain intrinsic parameters of cameras to obtain normalized pinhole
347 // cameras where intrinsic parameters have been removed P1' = inv(K) * P1
348 if (!leftCamera.areIntrinsicParametersAvailable()) {
349 leftCamera.decompose(true, false);
350 }
351 if (!rightCamera.areIntrinsicParametersAvailable()) {
352 rightCamera.decompose(true, false);
353 }
354
355 final var leftIntrinsics = leftCamera.getIntrinsicParameters();
356 final var rightIntrinsics = rightCamera.getIntrinsicParameters();
357
358 final var leftIntrinsicsMatrix = leftIntrinsics.getInternalMatrix();
359 final var rightIntrinsicsMatrix = rightIntrinsics.getInternalMatrix();
360
361 // get left and right internal matrices of cameras
362 final var leftCameraInternalMatrix = leftCamera.getInternalMatrix();
363 final var rightCameraInternalMatrix = rightCamera.getInternalMatrix();
364
365 // normalize internal camera matrices using inverse intrinsic matrices
366 final var invLeftIntrinsicsMatrix = Utils.inverse(leftIntrinsicsMatrix);
367 final var invRightIntrinsicsMatrix = Utils.inverse(rightIntrinsicsMatrix);
368
369 // normalize cameras
370 // P1' = inv(K1) * P1
371 invLeftIntrinsicsMatrix.multiply(leftCameraInternalMatrix);
372
373 // P2' = inv(K2) * P2
374 invRightIntrinsicsMatrix.multiply(rightCameraInternalMatrix);
375
376 // instantiate normalized left camera to project right camera center
377 // and obtain left eipole
378 final var normLeftCamera = new PinholeCamera(invLeftIntrinsicsMatrix);
379
380 // instantiate normalized right camera to decompose it and obtain its
381 // center
382 final var normRightCamera = new PinholeCamera(invRightIntrinsicsMatrix);
383
384 normRightCamera.decompose(false, true);
385
386 final var rightCameraCenter = normRightCamera.getCameraCenter();
387 final var normLeftEpipole = normLeftCamera.project(rightCameraCenter);
388 // to increase accuracy
389 normLeftEpipole.normalize();
390
391 // compute skew matrix of left epipole
392 final var skewNormLeftEpipoleMatrix = Utils.skewMatrix(new double[]{
393 normLeftEpipole.getHomX(), normLeftEpipole.getHomY(), normLeftEpipole.getHomW()});
394
395 // compute transposed of internal normalized left pinhole camera
396 final var transNormLeftCameraMatrix = invLeftIntrinsicsMatrix.transposeAndReturnNew();
397
398 // compute transposed of internal normalized right pinhole camera
399 final var transNormRightCameraMatrix = invRightIntrinsicsMatrix.transposeAndReturnNew();
400
401 // compute pseudo-inverse of transposed normalized right pinhole camera
402 final var pseudoTransNormRightCameraMatrix = Utils.pseudoInverse(transNormRightCameraMatrix);
403
404 // obtain essential matrix as: inv(P2norm') * P1norm' * skew(e1)
405 transNormLeftCameraMatrix.multiply(skewNormLeftEpipoleMatrix);
406 pseudoTransNormRightCameraMatrix.multiply(transNormLeftCameraMatrix);
407
408 setInternalMatrix(pseudoTransNormRightCameraMatrix, singularValuesThreshold);
409 } catch (final GeometryException | AlgebraException e) {
410 throw new InvalidPairOfCamerasException(e);
411 }
412 }
413
414 /**
415 * Sets essential matrix from provided rotation and translation of the image
416 * of world origin relative to left view camera, which is assumed to be
417 * located at origin of coordinates with no rotation.
418 *
419 * @param rotation rotation of right camera relative to left camera.
420 * @param translation translation of the image of world origin on right
421 * camera relative to left camera.
422 * @throws InvalidRotationAndTranslationException if provided rotation and
423 * translation yield a degenerate epipolar geometry.
424 */
425 public void setFromRotationAndTranslation(
426 final Rotation3D rotation, final Point2D translation) throws InvalidRotationAndTranslationException {
427 setFromRotationAndTranslation(rotation, translation, DEFAULT_SINGULAR_VALUES_THRESHOLD);
428 }
429
430 /**
431 * Sets essential matrix from provided rotation and translation of the image
432 * of world origin relative to left view camera, which is assumed to be
433 * located at origin of coordinates with no rotation.
434 *
435 * @param rotation rotation of right camera relative to left camera.
436 * @param translation translation of the image of world origin on right
437 * camera relative to left camera.
438 * @param singularValuesThreshold threshold to determine that both singular
439 * values of generated essential matrix are equal.
440 * @throws InvalidRotationAndTranslationException if provided rotation and
441 * translation yield a degenerate epipolar geometry.
442 * @throws IllegalArgumentException if provided threshold is negative.
443 */
444 public final void setFromRotationAndTranslation(
445 final Rotation3D rotation, final Point2D translation, final double singularValuesThreshold)
446 throws InvalidRotationAndTranslationException {
447
448 if (singularValuesThreshold < 0) {
449 throw new IllegalArgumentException();
450 }
451
452 try {
453 // to increase accuracy
454 translation.normalize();
455 final var translationArray = new double[]{
456 translation.getHomX(), translation.getHomY(), translation.getHomW()
457 };
458
459 final var skewTranslationMatrix = Utils.skewMatrix(translationArray);
460
461 final var rotationMatrix = rotation.asInhomogeneousMatrix();
462
463 // obtain essential matrix as: skew(translation) * rotation
464 skewTranslationMatrix.multiply(rotationMatrix);
465
466 setInternalMatrix(skewTranslationMatrix, singularValuesThreshold);
467 } catch (final AlgebraException | InvalidEssentialMatrixException e) {
468 throw new InvalidRotationAndTranslationException(e);
469 }
470 }
471
472 /**
473 * Sets essential matrix from provided rotation and translation of the
474 * camera center relative to left view camera, which is assumed to be
475 * located at origin of coordinates with no rotation.
476 *
477 * @param rotation rotation of right camera relative to left camera.
478 * @param cameraCenter camera center of right camera relative to left camera.
479 * @throws InvalidRotationAndTranslationException if provided rotation and
480 * camera center yield a degenerate epipolar geometry.
481 */
482 public void setFromRotationAndCameraCenter(
483 final Rotation3D rotation, final Point3D cameraCenter) throws InvalidRotationAndTranslationException {
484 setFromRotationAndCameraCenter(rotation, cameraCenter, DEFAULT_SINGULAR_VALUES_THRESHOLD);
485 }
486
487 /**
488 * Sets essential matrix from provided rotation and translation of the
489 * camera center relative to left view camera, which is assumed to be
490 * located at origin of coordinates with no rotation.
491 *
492 * @param rotation rotation of right camera relative to left camera.
493 * @param cameraCenter camera center of right camera relative to left camera.
494 * @param singularValuesThreshold threshold to determine that both singular
495 * values of generated essential matrix are equal.
496 * @throws InvalidRotationAndTranslationException if provided rotation and
497 * camera center yield a degenerate epipolar geometry.
498 * @throws IllegalArgumentException if provided threshold is negative.
499 */
500 public final void setFromRotationAndCameraCenter(
501 final Rotation3D rotation, final Point3D cameraCenter, final double singularValuesThreshold)
502 throws InvalidRotationAndTranslationException {
503
504 if (singularValuesThreshold < 0) {
505 throw new IllegalArgumentException();
506 }
507
508 try {
509 var rotationMatrix = rotation.asInhomogeneousMatrix();
510
511 // to increase accuracy
512 cameraCenter.normalize();
513 final var inhomCenterMatrix = new Matrix(Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH, 1);
514 inhomCenterMatrix.setElementAtIndex(0, cameraCenter.getInhomX());
515 inhomCenterMatrix.setElementAtIndex(1, cameraCenter.getInhomY());
516 inhomCenterMatrix.setElementAtIndex(2, cameraCenter.getInhomZ());
517
518 // translationMatrix = -rotationMatrix * inhomCenterMatrix
519 rotationMatrix.multiplyByScalar(-1.0);
520 rotationMatrix.multiply(inhomCenterMatrix);
521 final var translationMatrix = rotationMatrix;
522
523 // essentialMatrix = skew(translationMatrix) * rotationMatrix
524 final var skewTranslationMatrix = Utils.skewMatrix(translationMatrix);
525 rotationMatrix = rotation.asInhomogeneousMatrix();
526 skewTranslationMatrix.multiply(rotationMatrix);
527
528 setInternalMatrix(skewTranslationMatrix, singularValuesThreshold);
529
530 } catch (final AlgebraException | InvalidEssentialMatrixException e) {
531 throw new InvalidRotationAndTranslationException(e);
532 }
533 }
534
535 /**
536 * Sets essential matrix from provided fundamental matrix and intrinsic
537 * camera parameters.
538 *
539 * @param fundamentalMatrix a fundamental matrix.
540 * @param leftIntrinsicParameters intrinsic camera parameters of left view.
541 * @param rightIntrinsicParameters intrinsic camera parameters of right view.
542 * @throws InvalidPairOfIntrinsicParametersException if provided intrinsic
543 * parameters generate an invalid essential matrix.
544 */
545 public final void setFromFundamentalMatrixAndIntrinsics(
546 final FundamentalMatrix fundamentalMatrix, final PinholeCameraIntrinsicParameters leftIntrinsicParameters,
547 final PinholeCameraIntrinsicParameters rightIntrinsicParameters)
548 throws InvalidPairOfIntrinsicParametersException {
549
550 try {
551 final var k1 = leftIntrinsicParameters.getInternalMatrix();
552 final var normK1 = Utils.normF(k1);
553 k1.multiplyByScalar(1.0 / normK1);
554
555 final var k2 = rightIntrinsicParameters.getInternalMatrix();
556 final var normK2 = Utils.normF(k2);
557 k2.multiplyByScalar(1.0 / normK2);
558
559 // to increase accuracy
560 fundamentalMatrix.normalize();
561 final var fundMatrix = fundamentalMatrix.getInternalMatrix();
562
563 k2.transpose();
564
565 // E = K2' * F * K1
566 fundMatrix.multiply(k1);
567 k2.multiply(fundMatrix);
568
569 final var normEssential = Utils.normF(k2);
570 k2.multiplyByScalar(1.0 / normEssential);
571
572 internalMatrix = k2;
573 normalized = false;
574 leftEpipole = rightEpipole = null;
575 } catch (final AlgebraException | GeometryException e) {
576 throw new InvalidPairOfIntrinsicParametersException(e);
577 }
578 }
579
580 /**
581 * Converts this essential matrix into a fundamental matrix by applying
582 * provided intrinsic parameters on left and right views.
583 * The essential matrix only contains information about rotation and
584 * translation relating two views, while fundamental matrix also contains
585 * information about the intrinsic parameters in both views.
586 * NOTE: although essential matrix is a subclass of fundamental matrix, it
587 * does not behave like a fundamental matrix.
588 *
589 * @param leftIntrinsicParameters intrinsic parameters in left view.
590 * @param rightIntrinsicParameters intrinsic parameters in right view.
591 * @return a fundamental matrix.
592 * @throws EpipolarException if something fails.
593 */
594 public FundamentalMatrix toFundamentalMatrix(
595 final PinholeCameraIntrinsicParameters leftIntrinsicParameters,
596 final PinholeCameraIntrinsicParameters rightIntrinsicParameters) throws EpipolarException {
597 try {
598 normalize();
599
600 final var essentialMatrix = getInternalMatrix();
601
602 final var k1 = leftIntrinsicParameters.getInternalMatrix();
603 final var invK1 = Utils.inverse(k1);
604 final var normInvK1 = Utils.normF(invK1);
605 invK1.multiplyByScalar(1.0 / normInvK1);
606
607 final var k2 = rightIntrinsicParameters.getInternalMatrix();
608 final var invK2 = Utils.inverse(k2);
609 final var normInvK2 = Utils.normF(invK2);
610 invK2.multiplyByScalar(1.0 / normInvK2);
611 invK2.transpose();
612
613 essentialMatrix.multiply(invK1);
614 invK2.multiply(essentialMatrix);
615
616 return new FundamentalMatrix(invK2);
617
618 } catch (final AlgebraException | GeometryException e) {
619 throw new EpipolarException(e);
620 }
621 }
622
623 /**
624 * Computes all possible camera rotations and translations that can generate
625 * this essential matrix.
626 *
627 * @throws InvalidEssentialMatrixException if essential matrix contains
628 * numerically unstable values.
629 */
630 public void computePossibleRotationAndTranslations() throws InvalidEssentialMatrixException {
631 try {
632 final var decomposer = new SingularValueDecomposer(internalMatrix);
633
634 decomposer.decompose();
635
636 final var u = decomposer.getU();
637 final var v = decomposer.getV();
638
639 v.transpose();
640
641 translation1 = new HomogeneousPoint2D(u.getElementAt(0, 2),
642 u.getElementAt(1, 2), u.getElementAt(2, 2));
643 translation2 = new HomogeneousPoint2D(-u.getElementAt(0, 2),
644 -u.getElementAt(1, 2), -u.getElementAt(2, 2));
645
646 // W is a skew-symmetric matrix that can be used to obtain two possible
647 // rotations
648 final var w = new Matrix(FUNDAMENTAL_MATRIX_ROWS, FUNDAMENTAL_MATRIX_COLS);
649 w.setElementAt(0, 1, -1.0);
650 w.setElementAt(1, 0, 1.0);
651 w.setElementAt(2, 2, 1.0);
652
653 final var transW = w.transposeAndReturnNew();
654
655 // R1 = U * W * V'
656 w.multiply(v);
657 final var rotationMatrix1 = u.multiplyAndReturnNew(w);
658
659 // R2 = U * W' * V'
660 transW.multiply(v);
661 final var rotationMatrix2 = u.multiplyAndReturnNew(transW);
662
663 rotation1 = new MatrixRotation3D(rotationMatrix1);
664 rotation2 = new MatrixRotation3D(rotationMatrix2);
665
666 possibleRotationsAndTranslationsAvailable = true;
667 } catch (final AlgebraException | InvalidRotationMatrixException e) {
668 throw new InvalidEssentialMatrixException(e);
669 }
670 }
671
672 /**
673 * Indicates whether possible camera rotations and translations that can
674 * generate this essential matrix have already been computed or not.
675 *
676 * @return true if possible camera rotations and translations have been
677 * computed, false otherwise.
678 */
679 public boolean arePossibleRotationsAndTranslationsAvailable() {
680 return possibleRotationsAndTranslationsAvailable;
681 }
682
683 /**
684 * Gets first possible rotation that can generate this essential matrix.
685 *
686 * @return first possible rotation.
687 * @throws NotAvailableException if possible rotation has not yet been
688 * computed.
689 */
690 public Rotation3D getFirstPossibleRotation() throws NotAvailableException {
691 if (!arePossibleRotationsAndTranslationsAvailable()) {
692 throw new NotAvailableException();
693 }
694
695 return rotation1;
696 }
697
698 /**
699 * Gets second possible rotation that can generate this essential matrix.
700 *
701 * @return second possible rotation.
702 * @throws NotAvailableException if possible rotation has not yet been
703 * computed.
704 */
705 public Rotation3D getSecondPossibleRotation() throws NotAvailableException {
706 if (!arePossibleRotationsAndTranslationsAvailable()) {
707 throw new NotAvailableException();
708 }
709
710 return rotation2;
711 }
712
713 /**
714 * Gets first possible translation that can generate this essential matrix.
715 *
716 * @return first possible translation.
717 * @throws NotAvailableException if possible translation has not yet been
718 * computed.
719 */
720 public Point2D getFirstPossibleTranslation() throws NotAvailableException {
721 if (!arePossibleRotationsAndTranslationsAvailable()) {
722 throw new NotAvailableException();
723 }
724
725 return translation1;
726 }
727
728 /**
729 * Gets second possible translation that can generate this essential matrix.
730 *
731 * @return second possible translation.
732 * @throws NotAvailableException if possible translation has not yet been
733 * computed.
734 */
735 public Point2D getSecondPossibleTranslation() throws NotAvailableException {
736 if (!arePossibleRotationsAndTranslationsAvailable()) {
737 throw new NotAvailableException();
738 }
739
740 return translation2;
741 }
742 }