View Javadoc
1   /*
2    * Copyright (C) 2016 Alberto Irurueta Carro (alberto@irurueta.com)
3    *
4    * Licensed under the Apache License, Version 2.0 (the "License");
5    * you may not use this file except in compliance with the License.
6    * You may obtain a copy of the License at
7    *
8    *         http://www.apache.org/licenses/LICENSE-2.0
9    *
10   * Unless required by applicable law or agreed to in writing, software
11   * distributed under the License is distributed on an "AS IS" BASIS,
12   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13   * See the License for the specific language governing permissions and
14   * limitations under the License.
15   */
16  
17  package com.irurueta.ar.calibration.estimators;
18  
19  import com.irurueta.algebra.AlgebraException;
20  import com.irurueta.algebra.Matrix;
21  import com.irurueta.algebra.SingularValueDecomposer;
22  import com.irurueta.algebra.Utils;
23  import com.irurueta.ar.calibration.DualAbsoluteQuadric;
24  import com.irurueta.geometry.BaseQuadric;
25  import com.irurueta.geometry.PinholeCamera;
26  import com.irurueta.geometry.estimators.LockedException;
27  import com.irurueta.geometry.estimators.NotReadyException;
28  import com.irurueta.numerical.NumericalException;
29  import com.irurueta.numerical.robust.WeightSelection;
30  import com.irurueta.sorting.SortingException;
31  
32  import java.util.List;
33  
34  /**
35   * Implementation of a Dual Absolute Quadric estimator using a weighted solution
36   * for provided pinhole cameras.
37   * This implementation assumes that:
38   * - cameras are arbitrary (usually the initial camera is the identity and must
39   * be discarded) as it creates a numerical degeneracy.
40   * - all provided cameras have the same intrinsic parameters
41   * - it is assumed that skewness is zero, the principal point is at the center
42   * of the image plane (zero), and both horizontal and vertical focal planes are
43   * equal.
44   */
45  @SuppressWarnings("DuplicatedCode")
46  public class WeightedDualAbsoluteQuadricEstimator extends DualAbsoluteQuadricEstimator {
47  
48      /**
49       * Default number of cameras (i.e. correspondences) to be weighted and taken
50       * into account.
51       */
52      public static final int DEFAULT_MAX_CAMERAS = 50;
53  
54      /**
55       * Indicates if weights are sorted by default so that largest weighted
56       * cameras are used first.
57       */
58      public static final boolean DEFAULT_SORT_WEIGHTS = true;
59  
60      /**
61       * Maximum number of cameras (i.e. correspondences) to be weighted and taken
62       * into account.
63       */
64      private int maxCameras;
65  
66      /**
67       * Indicates if weights are sorted by default so that largest weighted
68       * cameras are used first.
69       */
70      private boolean sortWeights;
71  
72      /**
73       * Array containing weights for all cameras.
74       */
75      private double[] weights;
76  
77      /**
78       * Constructor.
79       */
80      public WeightedDualAbsoluteQuadricEstimator() {
81          super();
82          maxCameras = DEFAULT_MAX_CAMERAS;
83          sortWeights = DEFAULT_SORT_WEIGHTS;
84          weights = null;
85      }
86  
87      /**
88       * Constructor with listener.
89       *
90       * @param listener listener to be notified of events such as when estimation
91       *                 starts, ends or estimation progress changes.
92       */
93      public WeightedDualAbsoluteQuadricEstimator(final DualAbsoluteQuadricEstimatorListener listener) {
94          super(listener);
95          maxCameras = DEFAULT_MAX_CAMERAS;
96          sortWeights = DEFAULT_SORT_WEIGHTS;
97          weights = null;
98      }
99  
100     /**
101      * Constructor.
102      *
103      * @param cameras list of cameras used to estimate the Dual Absolute Quadric
104      *                (DAQ).
105      * @throws IllegalArgumentException if list of cameras is null.
106      */
107     public WeightedDualAbsoluteQuadricEstimator(final List<PinholeCamera> cameras) {
108         super(cameras);
109         maxCameras = DEFAULT_MAX_CAMERAS;
110         sortWeights = DEFAULT_SORT_WEIGHTS;
111         weights = null;
112     }
113 
114     /**
115      * Constructor.
116      *
117      * @param cameras  list of cameras used to estimate the Dual Absolute Quadric
118      *                 (DAQ).
119      * @param listener listener to be notified of events such as when estimation
120      *                 starts, ends or estimation progress changes.
121      * @throws IllegalArgumentException if list of cameras is null.
122      */
123     public WeightedDualAbsoluteQuadricEstimator(
124             final List<PinholeCamera> cameras, final DualAbsoluteQuadricEstimatorListener listener) {
125         super(cameras, listener);
126         maxCameras = DEFAULT_MAX_CAMERAS;
127         sortWeights = DEFAULT_SORT_WEIGHTS;
128         weights = null;
129     }
130 
131     /**
132      * Constructor.
133      *
134      * @param cameras list of cameras used to estimate the Dual Absolute Quadric
135      *                (DAQ).
136      * @param weights array containing a weight amount for each corresponding
137      *                camera. The larger the value of a weight, the most significant the
138      *                corresponding camera data will be.
139      * @throws IllegalArgumentException if provided lists of cameras and weights
140      *                                  don't have the same size or enough cameras.
141      */
142     public WeightedDualAbsoluteQuadricEstimator(
143             final List<PinholeCamera> cameras, final double[] weights) {
144         super(cameras);
145         maxCameras = DEFAULT_MAX_CAMERAS;
146         sortWeights = DEFAULT_SORT_WEIGHTS;
147         try {
148             setWeights(weights);
149         } catch (final LockedException ignore) {
150             // never thrown
151         }
152     }
153 
154     /**
155      * Constructor.
156      *
157      * @param cameras  list of cameras used to estimate the Dual Absolute Quadric
158      *                 (DAQ).
159      * @param weights  array containing a weight amount for each corresponding
160      *                 camera. The largest the value of a weight, the most significant the
161      *                 corresponding camera data will be.
162      * @param listener listener to be notified of events such as when estimation
163      *                 starts, ends or estimation progress changes.
164      * @throws IllegalArgumentException if provided lists of cameras and weights
165      *                                  don't have the same size or enough cameras.
166      */
167     public WeightedDualAbsoluteQuadricEstimator(
168             final List<PinholeCamera> cameras, final double[] weights,
169             final DualAbsoluteQuadricEstimatorListener listener) {
170         super(cameras, listener);
171         maxCameras = DEFAULT_MAX_CAMERAS;
172         sortWeights = DEFAULT_SORT_WEIGHTS;
173         try {
174             setWeights(weights);
175         } catch (final LockedException ignore) {
176             // never thrown
177         }
178     }
179 
180     /**
181      * Indicates whether provided cameras and weights are valid or not.
182      * Cameras and weights must have the same length to be valid and their
183      * length must be greater than 1.
184      *
185      * @param cameras list of cameras to check.
186      * @param weights array of weights to check.
187      * @return true if cameras and weights are valid, false otherwise.
188      */
189     public static boolean areValidCamerasAndWeights(final List<PinholeCamera> cameras, final double[] weights) {
190         return cameras != null && weights != null && cameras.size() == weights.length;
191     }
192 
193     /**
194      * Returns array containing a weight amount for each corresponding camera.
195      * The largest the value of a weight, the more significant the corresponding
196      * camera data will be.
197      *
198      * @return weights for each corresponding camera.
199      */
200     public double[] getWeights() {
201         return weights;
202     }
203 
204     /**
205      * Sets array of camera weight for each corresponding camera.
206      * The largest the value of a weight, the more significant the corresponding
207      * camera data will be.
208      *
209      * @param weights weights for each corresponding camera.
210      * @throws IllegalArgumentException if provided lists of cameras and weights
211      *                                  don't have the same size or enough cameras.
212      * @throws LockedException          if estimator is locked.
213      */
214     public final void setWeights(final double[] weights) throws LockedException {
215         if (!areValidCamerasAndWeights(cameras, weights)) {
216             throw new IllegalArgumentException("cameras and weights must have the same length");
217         }
218         if (isLocked()) {
219             throw new LockedException();
220         }
221 
222         this.weights = weights;
223     }
224 
225     /**
226      * Sets list of cameras and corresponding weights.
227      *
228      * @param cameras list of cameras used to estimate the Dual Absolute Quadric
229      *                (DAQ).
230      * @param weights array containing a weight amount for each corresponding
231      *                camera. The largest the value of a weight, the most significant the
232      *                corresponding camera data will be.
233      * @throws IllegalArgumentException if provided lists of cameras and weights
234      *                                  don't have the same size or enough cameras.
235      * @throws LockedException          if estimator is locked.
236      */
237     public void setCamerasAndWeights(final List<PinholeCamera> cameras, final double[] weights) throws LockedException {
238         if (!areValidCamerasAndWeights(cameras, weights)) {
239             throw new IllegalArgumentException("cameras and weights must have the same length");
240         }
241         if (isLocked()) {
242             throw new LockedException();
243         }
244 
245         this.cameras = cameras;
246         this.weights = weights;
247     }
248 
249     /**
250      * Indicates whether weights have already been provided or not.
251      *
252      * @return true if weights have been provided, false otherwise.
253      */
254     public boolean areWeightsAvailable() {
255         return weights != null;
256     }
257 
258     /**
259      * Gets the maximum number of cameras (i.e. correspondences) to be weighted
260      * and taken into account.
261      *
262      * @return maximum number of cameras.
263      */
264     public int getMaxCameras() {
265         return maxCameras;
266     }
267 
268     /**
269      * Sets the maximum number of cameras (i.e. correspondences) to be weighted
270      * and taken into account.
271      *
272      * @param maxCameras maximum number of cameras.
273      * @throws IllegalArgumentException if provided value is less than 2.
274      * @throws LockedException          if estimator is locked.
275      */
276     public void setMaxCameras(final int maxCameras) throws LockedException {
277         if (isLocked()) {
278             throw new LockedException();
279         }
280 
281         this.maxCameras = maxCameras;
282     }
283 
284     /**
285      * Indicates if weights are sorted by default so that largest weighted
286      * cameras are used first.
287      *
288      * @return true if weights are sorted by default, false otherwise.
289      */
290     public boolean isSortWeightsEnabled() {
291         return sortWeights;
292     }
293 
294     /**
295      * Specifies whether weights are sorted by default so that largest weighted
296      * cameras are used first.
297      *
298      * @param sortWeights true if weights are sorted by default, false
299      *                    otherwise.
300      * @throws LockedException if estimator is locked.
301      */
302     public void setSortWeightsEnabled(final boolean sortWeights) throws LockedException {
303         if (isLocked()) {
304             throw new LockedException();
305         }
306 
307         this.sortWeights = sortWeights;
308     }
309 
310     /**
311      * Indicates if this estimator is ready to start the estimation.
312      *
313      * @return true if estimator is ready, false otherwise.
314      */
315     @Override
316     public boolean isReady() {
317         return super.isReady() && areWeightsAvailable() && maxCameras >= getMinNumberOfRequiredCameras();
318     }
319 
320     /**
321      * Estimates the Dual Absolute Quadric using provided cameras.
322      *
323      * @param result instance where estimated Dual Absolute Quadric (DAQ) will
324      *               be stored.
325      * @throws LockedException                       if estimator is locked.
326      * @throws NotReadyException                     if no valid input data has already been
327      *                                               provided.
328      * @throws DualAbsoluteQuadricEstimatorException if an error occurs during
329      *                                               estimation, usually because input data is not valid or
330      *                                               numerically unstable.
331      */
332     @Override
333     public void estimate(final DualAbsoluteQuadric result) throws LockedException, NotReadyException,
334             DualAbsoluteQuadricEstimatorException {
335 
336         if (isLocked()) {
337             throw new LockedException();
338         }
339         if (!isReady()) {
340             throw new NotReadyException();
341         }
342 
343         try {
344             locked = true;
345             if (listener != null) {
346                 listener.onEstimateStart(this);
347             }
348 
349             if (principalPointAtOrigin) {
350                 if (zeroSkewness) {
351                     if (focalDistanceAspectRatioKnown) {
352                         estimateZeroSkewnessPrincipalPointAtOriginAndKnownFocalDistanceAspectRatio(result);
353                     } else {
354                         estimateZeroSkewnessAndPrincipalPointAtOrigin(result);
355                     }
356                 } else {
357                     estimatePrincipalPointAtOrigin(result);
358                 }
359             }
360 
361             if (listener != null) {
362                 listener.onEstimateEnd(this);
363             }
364         } finally {
365             locked = false;
366         }
367     }
368 
369     /**
370      * Returns type of Dual Absolute Quadric estimator.
371      *
372      * @return type of DAQ estimator.
373      */
374     @Override
375     public DualAbsoluteQuadricEstimatorType getType() {
376         return DualAbsoluteQuadricEstimatorType.WEIGHTED_DUAL_ABSOLUTE_QUADRIC_ESTIMATOR;
377     }
378 
379     /**
380      * Indicates whether current constraints are enough to start the estimation.
381      * In order to obtain a linear solution for the DAQ estimation, we need at
382      * least the principal point at origin constraint.
383      *
384      * @return true if constraints are valid, false otherwise.
385      */
386     @Override
387     public boolean areValidConstraints() {
388         return super.areValidConstraints() && isZeroSkewness() && isFocalDistanceAspectRatioKnown();
389     }
390 
391     /**
392      * Estimates Dual Absolute Quadric (DAQ) assuming that skewness is zero,
393      * principal point is located at origin of coordinates and that aspect ratio
394      * of focal distances is known.
395      *
396      * @param result instance where resulting estimated Dual Absolute Quadric
397      *               will be stored.
398      * @throws DualAbsoluteQuadricEstimatorException if an error occurs during
399      *                                               estimation, usually because repeated cameras are
400      *                                               provided, or cameras corresponding to critical motion
401      *                                               sequences such as pure parallel translations are
402      *                                               provided, where no additional data is really provided.
403      */
404     private void estimateZeroSkewnessPrincipalPointAtOriginAndKnownFocalDistanceAspectRatio(
405             final DualAbsoluteQuadric result) throws DualAbsoluteQuadricEstimatorException {
406 
407         try {
408             final var nCams = Math.min(cameras.size(), maxCameras);
409 
410             final var selection = WeightSelection.selectWeights(weights, sortWeights, nCams);
411             final var selected = selection.getSelected();
412 
413             final var a = new Matrix(BaseQuadric.N_PARAMS, BaseQuadric.N_PARAMS);
414             final var row = new Matrix(4, BaseQuadric.N_PARAMS);
415             final var transRow = new Matrix(BaseQuadric.N_PARAMS, 4);
416             final var tmp = new Matrix(BaseQuadric.N_PARAMS, BaseQuadric.N_PARAMS);
417 
418             Matrix cameraMatrix;
419             double p11;
420             double p12;
421             double p13;
422             double p14;
423             double p21;
424             double p22;
425             double p23;
426             double p24;
427             double p31;
428             double p32;
429             double p33;
430             double p34;
431             int eqCounter;
432             var cameraCounter = 0;
433             double weight;
434             var previousNorm = 1.0;
435             for (final var camera : cameras) {
436                 if (selected[cameraCounter]) {
437                     eqCounter = 0;
438 
439                     // normalize cameras to increase accuracy
440                     camera.normalize();
441 
442                     cameraMatrix = camera.getInternalMatrix();
443 
444                     p11 = cameraMatrix.getElementAt(0, 0);
445                     p21 = cameraMatrix.getElementAt(1, 0);
446                     p31 = cameraMatrix.getElementAt(2, 0);
447 
448                     p12 = cameraMatrix.getElementAt(0, 1);
449                     p22 = cameraMatrix.getElementAt(1, 1);
450                     p32 = cameraMatrix.getElementAt(2, 1);
451 
452                     p13 = cameraMatrix.getElementAt(0, 2);
453                     p23 = cameraMatrix.getElementAt(1, 2);
454                     p33 = cameraMatrix.getElementAt(2, 2);
455 
456                     p14 = cameraMatrix.getElementAt(0, 3);
457                     p24 = cameraMatrix.getElementAt(1, 3);
458                     p34 = cameraMatrix.getElementAt(2, 3);
459 
460                     weight = weights[cameraCounter];
461 
462                     // 1st row
463                     fill2ndRowAnd1stRowEquation(p11, p21, p12, p22, p13, p23, p14, p24, row, eqCounter);
464                     applyWeight(row, eqCounter, weight);
465                     eqCounter++;
466 
467                     // 2nd row
468                     fill3rdRowAnd1stRowEquation(p11, p31, p12, p32, p13, p33, p14, p34, row, eqCounter);
469                     applyWeight(row, eqCounter, weight);
470                     eqCounter++;
471 
472                     // 3rd row
473                     fill3rdRowAnd2ndRowEquation(p21, p31, p22, p32, p23, p33, p24, p34, row, eqCounter);
474                     applyWeight(row, eqCounter, weight);
475                     eqCounter++;
476 
477                     // 4th row
478                     fill1stRowEqualTo2ndRowEquation(p11, p21, p12, p22, p13, p23, p14, p24, row, eqCounter);
479                     applyWeight(row, eqCounter, weight);
480 
481                     // transRow = row'
482                     row.transpose(transRow);
483                     transRow.multiply(row, tmp);
484 
485                     tmp.multiplyByScalar(1.0 / previousNorm);
486 
487                     // a += 1.0 / previousNorm * tmp
488                     a.add(tmp);
489                     // normalize
490                     previousNorm = Utils.normF(a);
491                     a.multiplyByScalar(1.0 / previousNorm);
492                 }
493 
494                 cameraCounter++;
495             }
496 
497             final var decomposer = new SingularValueDecomposer(a);
498             enforceRank3IfNeeded(decomposer, result);
499 
500         } catch (final AlgebraException | SortingException | NumericalException e) {
501             throw new DualAbsoluteQuadricEstimatorException(e);
502         }
503     }
504 
505     /**
506      * Estimates Dual Absolute Quadric (DAQ) assuming that skewness is zero,
507      * and principal point is located at origin of coordinates.
508      *
509      * @param result instance where resulting estimated Dual Absolute Quadrics
510      *               will be stored.
511      * @throws DualAbsoluteQuadricEstimatorException if an error occurs during
512      *                                               estimation, usually because repeated cameras are
513      *                                               provided, or cameras corresponding to critical motion
514      *                                               sequences such as pure parallel translations are
515      *                                               provided, where no additional data is really provided.
516      */
517     private void estimateZeroSkewnessAndPrincipalPointAtOrigin(final DualAbsoluteQuadric result)
518             throws DualAbsoluteQuadricEstimatorException {
519 
520         try {
521             final var nCams = Math.min(cameras.size(), maxCameras);
522 
523             final var selection = WeightSelection.selectWeights(weights, sortWeights, nCams);
524             final var selected = selection.getSelected();
525 
526             final var a = new Matrix(BaseQuadric.N_PARAMS, BaseQuadric.N_PARAMS);
527             final var row = new Matrix(3, BaseQuadric.N_PARAMS);
528             final var transRow = new Matrix(BaseQuadric.N_PARAMS, 3);
529             final var tmp = new Matrix(BaseQuadric.N_PARAMS, BaseQuadric.N_PARAMS);
530 
531             Matrix cameraMatrix;
532             double p11;
533             double p12;
534             double p13;
535             double p14;
536             double p21;
537             double p22;
538             double p23;
539             double p24;
540             double p31;
541             double p32;
542             double p33;
543             double p34;
544             int eqCounter;
545             var cameraCounter = 0;
546             double weight;
547             var previousNorm = 1.0;
548             for (final var camera : cameras) {
549                 if (selected[cameraCounter]) {
550                     eqCounter = 0;
551 
552                     // normalize cameras to increase accuracy
553                     camera.normalize();
554 
555                     cameraMatrix = camera.getInternalMatrix();
556 
557                     p11 = cameraMatrix.getElementAt(0, 0);
558                     p21 = cameraMatrix.getElementAt(1, 0);
559                     p31 = cameraMatrix.getElementAt(2, 0);
560 
561                     p12 = cameraMatrix.getElementAt(0, 1);
562                     p22 = cameraMatrix.getElementAt(1, 1);
563                     p32 = cameraMatrix.getElementAt(2, 1);
564 
565                     p13 = cameraMatrix.getElementAt(0, 2);
566                     p23 = cameraMatrix.getElementAt(1, 2);
567                     p33 = cameraMatrix.getElementAt(2, 2);
568 
569                     p14 = cameraMatrix.getElementAt(0, 3);
570                     p24 = cameraMatrix.getElementAt(1, 3);
571                     p34 = cameraMatrix.getElementAt(2, 3);
572 
573                     weight = weights[cameraCounter];
574 
575                     // 1st row
576                     fill2ndRowAnd1stRowEquation(p11, p21, p12, p22, p13, p23, p14, p24, a, eqCounter);
577                     applyWeight(row, eqCounter, weight);
578                     eqCounter++;
579 
580                     // 2nd row
581                     fill3rdRowAnd1stRowEquation(p11, p31, p12, p32, p13, p33, p14, p34, a, eqCounter);
582                     applyWeight(row, eqCounter, weight);
583                     eqCounter++;
584 
585                     // 3rd row
586                     fill3rdRowAnd2ndRowEquation(p21, p31, p22, p32, p23, p33, p24, p34, a, eqCounter);
587                     applyWeight(row, eqCounter, weight);
588 
589                     // transRow = row'
590                     row.transpose(transRow);
591                     transRow.multiply(row, tmp);
592 
593                     tmp.multiplyByScalar(1.0 / previousNorm);
594 
595                     // a += 1.0 / previousNorm * tmp
596                     a.add(tmp);
597                     // normalize
598                     previousNorm = Utils.normF(a);
599                     a.multiplyByScalar(1.0 / previousNorm);
600                 }
601 
602                 cameraCounter++;
603             }
604 
605             final var decomposer = new SingularValueDecomposer(a);
606             enforceRank3IfNeeded(decomposer, result);
607 
608         } catch (final AlgebraException | SortingException | NumericalException e) {
609             throw new DualAbsoluteQuadricEstimatorException(e);
610         }
611     }
612 
613     /**
614      * Estimates Dual Absolute Quadric (DAQ) assuming that principal point is
615      * zero.
616      *
617      * @param result instance where resulting estimated Dual Absolute Quadrics
618      *               will be stored.
619      * @throws DualAbsoluteQuadricEstimatorException if an error occurs during
620      *                                               estimation, usually because repeated cameras are
621      *                                               provided, or cameras corresponding to critical motion
622      *                                               sequences such as pure parallel translations are
623      *                                               provided, where no additional data is really provided.
624      */
625     private void estimatePrincipalPointAtOrigin(DualAbsoluteQuadric result)
626             throws DualAbsoluteQuadricEstimatorException {
627 
628         try {
629             final var nCams = Math.min(cameras.size(), maxCameras);
630 
631             final var selection = WeightSelection.selectWeights(weights, sortWeights, nCams);
632             final var selected = selection.getSelected();
633 
634             final var a = new Matrix(BaseQuadric.N_PARAMS, BaseQuadric.N_PARAMS);
635             final var row = new Matrix(2, BaseQuadric.N_PARAMS);
636             final var transRow = new Matrix(BaseQuadric.N_PARAMS, 2);
637             final var tmp = new Matrix(BaseQuadric.N_PARAMS, BaseQuadric.N_PARAMS);
638 
639             Matrix cameraMatrix;
640             double p11;
641             double p12;
642             double p13;
643             double p14;
644             double p21;
645             double p22;
646             double p23;
647             double p24;
648             double p31;
649             double p32;
650             double p33;
651             double p34;
652             int eqCounter;
653             var cameraCounter = 0;
654             double weight;
655             var previousNorm = 1.0;
656             for (final var camera : cameras) {
657                 if (selected[cameraCounter]) {
658                     eqCounter = 0;
659 
660                     // normalize cameras to increase accuracy
661                     camera.normalize();
662 
663                     cameraMatrix = camera.getInternalMatrix();
664 
665                     p11 = cameraMatrix.getElementAt(0, 0);
666                     p21 = cameraMatrix.getElementAt(1, 0);
667                     p31 = cameraMatrix.getElementAt(2, 0);
668 
669                     p12 = cameraMatrix.getElementAt(0, 1);
670                     p22 = cameraMatrix.getElementAt(1, 1);
671                     p32 = cameraMatrix.getElementAt(2, 1);
672 
673                     p13 = cameraMatrix.getElementAt(0, 2);
674                     p23 = cameraMatrix.getElementAt(1, 2);
675                     p33 = cameraMatrix.getElementAt(2, 2);
676 
677                     p14 = cameraMatrix.getElementAt(0, 3);
678                     p24 = cameraMatrix.getElementAt(1, 3);
679                     p34 = cameraMatrix.getElementAt(2, 3);
680 
681                     weight = weights[cameraCounter];
682 
683                     // 1st row
684                     fill3rdRowAnd1stRowEquation(p11, p31, p12, p32, p13, p33, p14, p34, a, eqCounter);
685                     applyWeight(row, eqCounter, weight);
686                     eqCounter++;
687 
688                     // 2nd row
689                     fill3rdRowAnd2ndRowEquation(p21, p31, p22, p32, p23, p33, p24, p34, a, eqCounter);
690                     applyWeight(row, eqCounter, weight);
691 
692                     // transRow = row'
693                     row.transpose(transRow);
694                     transRow.multiply(row, tmp);
695 
696                     tmp.multiplyByScalar(1.0 / previousNorm);
697 
698                     // a += 1.0 / previousNorm * tmp
699                     a.add(tmp);
700                     // normalize
701                     previousNorm = Utils.normF(a);
702                     a.multiplyByScalar(1.0 / previousNorm);
703                 }
704 
705                 cameraCounter++;
706             }
707 
708             final var decomposer = new SingularValueDecomposer(a);
709             enforceRank3IfNeeded(decomposer, result);
710 
711         } catch (final AlgebraException | SortingException | NumericalException e) {
712             throw new DualAbsoluteQuadricEstimatorException(e);
713         }
714     }
715 
716     /**
717      * Apply provided weight to matrix at provided row.
718      *
719      * @param a      matrix to apply weight to.
720      * @param row    row within matrix to apply weight.
721      * @param weight weight to be applied.
722      */
723     private void applyWeight(final Matrix a, final int row, final double weight) {
724         final var cols = a.getColumns();
725         for (var i = 0; i < cols; i++) {
726             a.setElementAt(row, i, a.getElementAt(row, i) * weight);
727         }
728     }
729 }