/*
    * Copyright (C) 2017 Alberto Irurueta Carro (alberto@irurueta.com)
    *
    * Licensed under the Apache License, Version 2.0 (the "License");
    * you may not use this file except in compliance with the License.
    * You may obtain a copy of the License at
    *
    *         http://www.apache.org/licenses/LICENSE-2.0
    *
   * Unless required by applicable law or agreed to in writing, software
   * distributed under the License is distributed on an "AS IS" BASIS,
   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   * See the License for the specific language governing permissions and
   * limitations under the License.
   */
  
  package com.irurueta.ar.sfm;
  
  import com.irurueta.algebra.Matrix;
  import com.irurueta.ar.calibration.estimators.KruppaDualImageOfAbsoluteConicEstimator;
  import com.irurueta.ar.calibration.estimators.LMSEImageOfAbsoluteConicEstimator;
  import com.irurueta.ar.epipolar.Corrector;
  import com.irurueta.ar.epipolar.EpipolarException;
  import com.irurueta.ar.epipolar.EssentialMatrix;
  import com.irurueta.ar.epipolar.FundamentalMatrix;
  import com.irurueta.ar.epipolar.estimators.EightPointsFundamentalMatrixEstimator;
  import com.irurueta.ar.epipolar.estimators.FundamentalMatrixEstimatorMethod;
  import com.irurueta.ar.epipolar.estimators.FundamentalMatrixRobustEstimator;
  import com.irurueta.ar.epipolar.estimators.LMedSFundamentalMatrixRobustEstimator;
  import com.irurueta.ar.epipolar.estimators.MSACFundamentalMatrixRobustEstimator;
  import com.irurueta.ar.epipolar.estimators.PROMedSFundamentalMatrixRobustEstimator;
  import com.irurueta.ar.epipolar.estimators.PROSACFundamentalMatrixRobustEstimator;
  import com.irurueta.ar.epipolar.estimators.RANSACFundamentalMatrixRobustEstimator;
  import com.irurueta.ar.epipolar.estimators.SevenPointsFundamentalMatrixEstimator;
  import com.irurueta.geometry.PinholeCamera;
  import com.irurueta.geometry.PinholeCameraIntrinsicParameters;
  import com.irurueta.geometry.Point2D;
  import com.irurueta.geometry.Point3D;
  import com.irurueta.geometry.Transformation2D;
  import com.irurueta.geometry.estimators.*;
  import com.irurueta.numerical.robust.RobustEstimatorMethod;
  
  import java.util.ArrayList;
  import java.util.BitSet;
  import java.util.List;
  
  /**
   * Base class in charge of estimating cameras and 3D reconstructed points from sparse
   * image point correspondences for multiple views.
   *
   * @param <C> type of configuration.
   * @param <R> type of re-constructor.
   * @param <L> type of listener.
   */
  public abstract class BaseSparseReconstructor<C extends BaseSparseReconstructorConfiguration<C>,
          R extends BaseSparseReconstructor<C, R, L>, L extends BaseSparseReconstructorListener<R>> {
  
      /**
       * Minimum required number of views.
       */
      public static final int MIN_NUMBER_OF_VIEWS = 2;
  
      /**
       * Default scale.
       */
      protected static final double DEFAULT_SCALE = 1.0;
  
      /**
       * Current estimated camera in a metric stratum (i.e. up to scale).
       */
      protected EstimatedCamera currentMetricEstimatedCamera;
  
      /**
       * Previous estimated camera in a metric stratum (i.e. up to scale).
       */
      protected EstimatedCamera previousMetricEstimatedCamera;
  
      /**
       * Reconstructed 3D points which still remain active to match next view in a metric stratum (i.e. up
       * to scale).
       */
      protected List<ReconstructedPoint3D> activeMetricReconstructedPoints;
  
      /**
       * Current estimated scale. This will typically converge to a constant value as more views are
       * processed.
       * The smaller the variance of estimated scale, the more accurate the scale will be.
       */
      protected double currentScale = DEFAULT_SCALE;
  
      /**
       * Current estimated camera in euclidean stratum (i.e. with actual scale).
       */
      protected EstimatedCamera currentEuclideanEstimatedCamera;
  
      /**
       * Previous estimated camera in Euclidean stratum (i.e. with actual scale).
       */
      protected EstimatedCamera previousEuclideanEstimatedCamera;
 
     /**
      * Reconstructed 3D points which still remain active to match next view in Euclidean stratum (i.e.
      * with actual scale).
      */
     protected List<ReconstructedPoint3D> activeEuclideanReconstructedPoints;
 
     /**
      * Configuration for this re-constructor.
      */
     protected C configuration;
 
     /**
      * Listener in charge of handling events such as when reconstruction starts, ends,
      * when certain data is needed or when estimation of data has been computed.
      */
     protected L listener;
 
     /**
      * Indicates whether reconstruction has failed or not.
      */
     protected volatile boolean failed;
 
     /**
      * Indicates whether reconstruction is running or not.
      */
     protected volatile boolean running;
 
     /**
      * Current estimated fundamental matrix.
      */
     private EstimatedFundamentalMatrix currentEstimatedFundamentalMatrix;
 
     /**
      * Indicates whether reconstruction has been cancelled or not.
      */
     private volatile boolean cancelled;
 
     /**
      * Counter of number of processed views.
      */
     private int viewCount;
 
     /**
      * Indicates whether reconstruction has finished or not.
      */
     private boolean finished = false;
 
     /**
      * All samples (tracked and non-tracked) on previous view.
      */
     private List<Sample2D> allPreviousViewSamples;
 
     /**
      * Tracked samples on previous view.
      */
     private List<Sample2D> previousViewTrackedSamples;
 
     /**
      * Tracked samples on last processed view (i.e. current view).
      */
     private List<Sample2D> currentViewTrackedSamples;
 
     /**
      * New samples on las processed view (i.e. current view).
      */
     private List<Sample2D> currentViewNewlySpawnedSamples;
 
     /**
      * Active matches between current and previous views.
      */
     private final List<MatchedSamples> matches = new ArrayList<>();
 
     /**
      * ID of previous view.
      */
     private int previousViewId;
 
     /**
      * ID of current view.
      */
     private int currentViewId;
 
     /**
      * Constructor.
      *
      * @param configuration configuration for this re-constructor.
      * @param listener      listener in charge of handling events.
      * @throws NullPointerException if listener or configuration is not provided.
      */
     protected BaseSparseReconstructor(final C configuration, final L listener) {
         if (configuration == null || listener == null) {
             throw new NullPointerException();
         }
         this.configuration = configuration;
         this.listener = listener;
     }
 
     /**
      * Gets configuration for this re-constructor.
      *
      * @return configuration for this re-constructor.
      */
     public C getConfiguration() {
         return configuration;
     }
 
     /**
      * Gets listener in charge of handling events such as when reconstruction starts,
      * ends, when certain data is needed or when estimation of data has been computed.
      *
      * @return listener in charge of handling events.
      */
     public L getListener() {
         return listener;
     }
 
     /**
      * Indicates whether reconstruction is running or not.
      *
      * @return true if reconstruction is running, false if reconstruction has stopped
      * for any reason.
      */
     public boolean isRunning() {
         return running;
     }
 
     /**
      * Indicates whether reconstruction has been cancelled or not.
      *
      * @return true if reconstruction has been cancelled, false otherwise.
      */
     public boolean isCancelled() {
         return cancelled;
     }
 
     /**
      * Indicates whether reconstruction has failed or not.
      *
      * @return true if reconstruction has failed, false otherwise.
      */
     public boolean hasFailed() {
         return failed;
     }
 
     /**
      * Indicates whether the reconstruction has finished.
      *
      * @return true if reconstruction has finished, false otherwise.
      */
     public boolean isFinished() {
         return finished;
     }
 
     /**
      * Gets counter of number of processed views.
      *
      * @return counter of number of processed views.
      */
     public int getViewCount() {
         return viewCount;
     }
 
     /**
      * Gets estimated fundamental matrix for current view.
      * This fundamental matrix relates current view with the previously processed one.
      *
      * @return current estimated fundamental matrix.
      */
     public EstimatedFundamentalMatrix getCurrentEstimatedFundamentalMatrix() {
         return currentEstimatedFundamentalMatrix;
     }
 
     /**
      * Gets estimated metric camera for current view (i.e. up to scale).
      *
      * @return current estimated metric camera.
      */
     public EstimatedCamera getCurrentMetricEstimatedCamera() {
         return currentMetricEstimatedCamera;
     }
 
     /**
      * Gets estimated camera for previous view (i.e. up to scale).
      *
      * @return previous estimated metric camera.
      */
     public EstimatedCamera getPreviousMetricEstimatedCamera() {
         return previousMetricEstimatedCamera;
     }
 
     /**
      * Gets estimated euclidean camera for current view (i.e. with actual scale).
      *
      * @return current estimated euclidean camera.
      */
     public EstimatedCamera getCurrentEuclideanEstimatedCamera() {
         return currentEuclideanEstimatedCamera;
     }
 
     /**
      * Gets estimated Euclidean camera for previous view (i.e. with actual scale).
      *
      * @return previous estimated euclidean camera.
      */
     public EstimatedCamera getPreviousEuclideanEstimatedCamera() {
         return previousEuclideanEstimatedCamera;
     }
 
     /**
      * Gets metric reconstructed 3D points (i.e. up to scale) which still remain active to match next view.
      *
      * @return active metric reconstructed 3D points.
      */
     public List<ReconstructedPoint3D> getActiveMetricReconstructedPoints() {
         return activeMetricReconstructedPoints;
     }
 
     /**
      * Gets Euclidean reconstructed 3D points (i.e. with actual scale) which still remain active to match
      * next view.
      *
      * @return active euclidean reconstructed 3D points.
      */
     public List<ReconstructedPoint3D> getActiveEuclideanReconstructedPoints() {
         return activeEuclideanReconstructedPoints;
     }
 
     /**
      * Gets current estimated scale. This will typically converge to a constant value as more views are
      * processed.
      * The smaller the variance of estimated scale, the more accurate the scale will be.
      *
      * @return current estimated scale.
      */
     public double getCurrentScale() {
         return currentScale;
     }
 
     /**
      * Gets tracked samples on previous view.
      *
      * @return tracked samples on previous view.
      */
     public List<Sample2D> getPreviousViewTrackedSamples() {
         return previousViewTrackedSamples;
     }
 
     /**
      * Gets tracked samples (from previous view) on current view.
      *
      * @return tracked samples on current view
      */
     public List<Sample2D> getCurrentViewTrackedSamples() {
         return currentViewTrackedSamples;
     }
 
     /**
      * Gets new samples (not tracked) on current view.
      *
      * @return new samples on current view.
      */
     public List<Sample2D> getCurrentViewNewlySpawnedSamples() {
         return currentViewNewlySpawnedSamples;
     }
 
     /**
      * Process one view of all the available data during the reconstruction.
      * This method can be called multiple times instead of {@link #start()} to build the
      * reconstruction step by step, one view at a time.
      * This method is useful when data is gathered on real time from a camera and the
      * number of views is unknown.
      *
      * @return true if more views can be processed, false when reconstruction has finished.
      */
     public boolean processOneView() {
         if (viewCount == 0) {
             if (running) {
                 // already started
                 return true;
             }
 
             reset();
             running = true;
 
             //noinspection unchecked
             listener.onStart((R) this);
         }
 
         //noinspection unchecked
         if (!listener.hasMoreViewsAvailable((R) this)) {
             //noinspection unchecked
             listener.onFinish((R) this);
             running = false;
             finished = true;
             return false;
         }
 
         previousViewTrackedSamples = new ArrayList<>();
         currentViewTrackedSamples = new ArrayList<>();
         currentViewNewlySpawnedSamples = new ArrayList<>();
         //noinspection unchecked
         listener.onRequestSamples((R) this, previousViewId, viewCount, previousViewTrackedSamples,
                 currentViewTrackedSamples, currentViewNewlySpawnedSamples);
 
         boolean processed;
         if (isFirstView()) {
             currentEstimatedFundamentalMatrix = null;
             // for first view we simply keep samples (if enough are provided)
             processed = processFirstView();
         } else {
 
             if (isSecondView()) {
                 // for second view, check that we have enough samples
                 processed = processSecondView();
             } else {
                 processed = processAdditionalView();
             }
         }
 
         if (processed) {
             viewCount++;
         }
 
         if (cancelled) {
             //noinspection unchecked
             listener.onCancel((R) this);
         }
 
         return !finished;
     }
 
     /**
      * Indicates whether current view is the first view.
      *
      * @return true if current view is the first view, false otherwise.
      */
     public boolean isFirstView() {
         return viewCount == 0 && (previousViewTrackedSamples == null || previousViewTrackedSamples.isEmpty());
     }
 
     /**
      * Indicates whether current view is the second view.
      *
      * @return true if current view is the second view, false otherwise.
      */
     public boolean isSecondView() {
         return !isFirstView() && currentEstimatedFundamentalMatrix == null;
     }
 
     /**
      * Indicates whether current view is an additional view.
      *
      * @return true if current view is an additional view, false otherwise.
      */
     public boolean isAdditionalView() {
         return !isFirstView() && !isSecondView();
     }
 
     /**
      * Starts reconstruction of all available data to reconstruct the whole scene.
      * If reconstruction has already started and is running, calling this method has
      * no effect.
      * This method is useful when all data is available before starting the reconstruction.
      */
     public void start() {
         while (processOneView()) {
             if (cancelled) {
                 break;
             }
         }
     }
 
     /**
      * Cancels reconstruction.
      * If reconstruction has already been cancelled, calling this method has no effect.
      */
     public void cancel() {
         if (cancelled) {
             // already cancelled
             return;
         }
 
         cancelled = true;
     }
 
     /**
      * Resets this instance so that a reconstruction can be started from the beginning without cancelling
      * current one.
      */
     public void reset() {
         if (previousViewTrackedSamples != null) {
             previousViewTrackedSamples.clear();
         }
         if (currentViewTrackedSamples != null) {
             currentViewTrackedSamples.clear();
         }
         if (currentViewNewlySpawnedSamples != null) {
             currentViewNewlySpawnedSamples.clear();
         }
         matches.clear();
 
         cancelled = failed = false;
         viewCount = 0;
         running = false;
 
         currentEstimatedFundamentalMatrix = null;
         currentMetricEstimatedCamera = previousMetricEstimatedCamera = null;
         activeMetricReconstructedPoints = null;
         currentScale = DEFAULT_SCALE;
         currentEuclideanEstimatedCamera = previousEuclideanEstimatedCamera = null;
         activeEuclideanReconstructedPoints = null;
 
         previousViewId = 0;
         currentViewId = 0;
 
         finished = false;
     }
 
     /**
      * Called when processing one frame is successfully finished. This can be done to estimate scale on
      * those implementations where scale can be measured or is already known.
      *
      * @param isInitialPairOfViews true if initial pair of views is being processed, false otherwise.
      * @return true if post-processing succeeded, false otherwise.
      */
     protected abstract boolean postProcessOne(final boolean isInitialPairOfViews);
 
     /**
      * Processes data for first view.
      *
      * @return true if view was successfully processed, false otherwise.
      */
     private boolean processFirstView() {
         if (hasEnoughSamplesForFundamentalMatrixEstimation(currentViewTrackedSamples)) {
             //noinspection unchecked
             listener.onSamplesAccepted((R) this, viewCount, previousViewTrackedSamples, currentViewTrackedSamples);
             if (allPreviousViewSamples == null) {
                 allPreviousViewSamples = new ArrayList<>();
             } else {
                 allPreviousViewSamples.clear();
             }
 
             allPreviousViewSamples.addAll(currentViewTrackedSamples);
             allPreviousViewSamples.addAll(currentViewNewlySpawnedSamples);
 
             previousViewTrackedSamples = currentViewTrackedSamples;
             previousViewId = viewCount;
             return true;
         } else {
             //noinspection unchecked
             listener.onSamplesRejected((R) this, viewCount, previousViewTrackedSamples, currentViewTrackedSamples);
             return false;
         }
     }
 
     /**
      * Processes data for second view.
      *
      * @return true if view was successfully processed, false otherwise.
      */
     private boolean processSecondView() {
         if (hasEnoughSamplesForFundamentalMatrixEstimation(currentViewTrackedSamples)) {
 
             // find matches
             matches.clear();
 
             // matching is up to listener implementation
             //noinspection unchecked
             listener.onRequestMatches((R) this, allPreviousViewSamples, previousViewTrackedSamples,
                     currentViewTrackedSamples, previousViewId, viewCount, matches);
 
             if (hasEnoughMatchesForFundamentalMatrixEstimation(matches)) {
                 // if enough matches are retrieved, attempt to compute
                 // fundamental matrix
                 if ((configuration.isGeneralSceneAllowed()
                         && estimateFundamentalMatrix(matches, previousViewId, viewCount, true))
                         || (configuration.isPlanarSceneAllowed()
                         && estimatePlanarFundamentalMatrix(matches, previousViewId, viewCount, true))) {
                     // fundamental matrix could be estimated
                     //noinspection unchecked
                     listener.onSamplesAccepted((R) this, viewCount, previousViewTrackedSamples,
                             currentViewTrackedSamples);
 
                     allPreviousViewSamples.clear();
                     allPreviousViewSamples.addAll(currentViewTrackedSamples);
                     allPreviousViewSamples.addAll(currentViewNewlySpawnedSamples);
 
                     previousViewTrackedSamples = currentViewTrackedSamples;
                     previousViewId = currentViewId;
                     currentViewId = viewCount;
 
                     //noinspection unchecked
                     listener.onFundamentalMatrixEstimated((R) this, currentEstimatedFundamentalMatrix);
 
                     if (estimateInitialCamerasAndPoints()) {
                         // cameras and points have been estimated
                         //noinspection unchecked
                         listener.onMetricCameraEstimated((R) this, previousViewId, currentViewId,
                                 previousMetricEstimatedCamera, currentMetricEstimatedCamera);
                         //noinspection unchecked
                         listener.onMetricReconstructedPointsEstimated((R) this, matches,
                                 activeMetricReconstructedPoints);
 
                         if (!postProcessOne(true)) {
                             // something failed
                             failed = true;
                             //noinspection unchecked
                             listener.onFail((R) this);
                             return false;
                         } else {
                             // post-processing succeeded
                             //noinspection unchecked
                             listener.onEuclideanCameraEstimated((R) this, previousViewId, currentViewId, currentScale,
                                     previousEuclideanEstimatedCamera, currentEuclideanEstimatedCamera);
                             //noinspection unchecked
                             listener.onEuclideanReconstructedPointsEstimated((R) this, currentScale,
                                     activeEuclideanReconstructedPoints);
                             return true;
                         }
                     } else {
                         // initial cameras failed
                         failed = true;
                         //noinspection unchecked
                         listener.onFail((R) this);
                         return false;
                     }
                 } else {
                     // estimation of fundamental matrix failed
                     //noinspection unchecked
                     listener.onSamplesRejected((R) this, viewCount, previousViewTrackedSamples,
                             currentViewTrackedSamples);
                     return false;
                 }
             }
         }
 
         //noinspection unchecked
         listener.onSamplesRejected((R) this, viewCount, previousViewTrackedSamples, currentViewTrackedSamples);
         return false;
     }
 
     /**
      * Processes data for one additional view.
      *
      * @return true if view was successfully processed, false otherwise.
      */
     private boolean processAdditionalView() {
         // find matches
         matches.clear();
 
         //noinspection unchecked
         listener.onRequestMatches((R) this, allPreviousViewSamples, previousViewTrackedSamples,
                 currentViewTrackedSamples, currentViewId, viewCount, matches);
 
         final var points3D = new ArrayList<Point3D>();
         final var points2D = new ArrayList<Point2D>();
         final var qualityScores = setUpCameraEstimatorMatches(points3D, points2D);
         var samplesRejected = false;
 
         if (hasEnoughSamplesForCameraEstimation(points3D, points2D) && hasEnoughMatchesForCameraEstimation(matches)) {
             // enough matches available.
             PinholeCamera currentCamera = null;
             Matrix currentCameraCovariance = null;
             if (configuration.getUseEPnPForAdditionalCamerasEstimation()) {
                 // use EPnP for additional cameras' estimation.
                 // EPnP requires knowledge of camera intrinsics
 
                 PinholeCameraIntrinsicParameters intrinsicParameters = null;
                 if ((configuration.getUseDAQForAdditionalCamerasIntrinsics()
                         || configuration.getUseDIACForAdditionalCamerasIntrinsics())
                         && hasEnoughMatchesForFundamentalMatrixEstimation(matches)) {
 
                     // compute fundamental matrix to estimate intrinsics
                     if ((configuration.isGeneralSceneAllowed()
                             && estimateFundamentalMatrix(matches, currentViewId, viewCount, false))
                             || (configuration.isPlanarSceneAllowed()
                             && estimatePlanarFundamentalMatrix(matches, currentViewId, viewCount,
                             false))) {
                         // fundamental matrix could be estimated
                         //noinspection unchecked
                         listener.onFundamentalMatrixEstimated((R) this, currentEstimatedFundamentalMatrix);
 
                         // use fundamental matrix to estimate intrinsics using DIAC or DAQ
                         if (configuration.getUseDIACForAdditionalCamerasIntrinsics()) {
                             intrinsicParameters = estimateIntrinsicsDIAC();
                         } else if (configuration.getUseDAQForAdditionalCamerasIntrinsics()) {
                             intrinsicParameters = estimateIntrinsicsDAQ();
                         }
 
                     } else {
                         // fundamental matrix estimation failed
 
                         //noinspection unchecked
                         listener.onSamplesRejected((R) this, viewCount, previousViewTrackedSamples,
                                 currentViewTrackedSamples);
                         return false;
                     }
 
                 } else if (configuration.getAdditionalCamerasIntrinsics() != null) {
                     // use configuration provided intrinsics
                     intrinsicParameters = configuration.getAdditionalCamerasIntrinsics();
 
                     if (intrinsicParameters == null) {
                         // something failed or bad configuration
                         failed = true;
                         //noinspection unchecked
                         listener.onFail((R) this);
                         return false;
                     }
                 }
 
                 try {
                     if (intrinsicParameters != null) {
                         // use EPnP for additional cameras estimation
                         final var cameraEstimator = EPnPPointCorrespondencePinholeCameraRobustEstimator.create(
                                 intrinsicParameters, points3D, points2D, qualityScores,
                                 configuration.getAdditionalCamerasRobustEstimationMethod());
                         cameraEstimator.setPlanarConfigurationAllowed(
                                 configuration.getAdditionalCamerasAllowPlanarConfiguration());
                         cameraEstimator.setNullspaceDimension2Allowed(
                                 configuration.getAdditionalCamerasAllowNullspaceDimension2());
                         cameraEstimator.setNullspaceDimension3Allowed(
                                 configuration.getAdditionalCamerasAllowNullspaceDimension3());
                         cameraEstimator.setPlanarThreshold(configuration.getAdditionalCamerasPlanarThreshold());
                         cameraEstimator.setResultRefined(configuration.areAdditionalCamerasRefined());
                         cameraEstimator.setCovarianceKept(configuration.isAdditionalCamerasCovarianceKept());
                         cameraEstimator.setFastRefinementUsed(configuration.getAdditionalCamerasUseFastRefinement());
                         cameraEstimator.setConfidence(configuration.getAdditionalCamerasConfidence());
                         cameraEstimator.setMaxIterations(configuration.getAdditionalCamerasMaxIterations());
 
                         switch (configuration.getAdditionalCamerasRobustEstimationMethod()) {
                             case LMEDS:
                                 ((LMedSEPnPPointCorrespondencePinholeCameraRobustEstimator) cameraEstimator)
                                         .setStopThreshold(configuration.getAdditionalCamerasThreshold());
                                 break;
                             case MSAC:
                                 ((MSACEPnPPointCorrespondencePinholeCameraRobustEstimator) cameraEstimator)
                                         .setThreshold(configuration.getAdditionalCamerasThreshold());
                                 break;
                             case PROMEDS:
                                 ((PROMedSEPnPPointCorrespondencePinholeCameraRobustEstimator) cameraEstimator)
                                         .setStopThreshold(configuration.getAdditionalCamerasThreshold());
                                 break;
                             case PROSAC:
                                 var prosacCameraEstimator =
                                         (PROSACEPnPPointCorrespondencePinholeCameraRobustEstimator) cameraEstimator;
                                 prosacCameraEstimator.setThreshold(configuration.getAdditionalCamerasThreshold());
                                 prosacCameraEstimator.setComputeAndKeepInliersEnabled(
                                         configuration.getAdditionalCamerasComputeAndKeepInliers());
                                 prosacCameraEstimator.setComputeAndKeepResidualsEnabled(
                                         configuration.getAdditionalCamerasComputeAndKeepResiduals());
                                 break;
                             case RANSAC:
                                 var ransacCameraEstimator =
                                         (RANSACEPnPPointCorrespondencePinholeCameraRobustEstimator) cameraEstimator;
                                 ransacCameraEstimator.setThreshold(configuration.getAdditionalCamerasThreshold());
                                 ransacCameraEstimator.setComputeAndKeepInliersEnabled(
                                         configuration.getAdditionalCamerasComputeAndKeepInliers());
                                 ransacCameraEstimator.setComputeAndKeepResidualsEnabled(
                                         configuration.getAdditionalCamerasComputeAndKeepResiduals());
                                 break;
                             default:
                                 break;
                         }
 
                         cameraEstimator.setSuggestSkewnessValueEnabled(
                                 configuration.isAdditionalCamerasSuggestSkewnessValueEnabled());
                         cameraEstimator.setSuggestedSkewnessValue(
                                 configuration.getAdditionalCamerasSuggestedSkewnessValue());
 
                         cameraEstimator.setSuggestHorizontalFocalLengthEnabled(
                                 configuration.isAdditionalCamerasSuggestHorizontalFocalLengthEnabled());
                         cameraEstimator.setSuggestedHorizontalFocalLengthValue(
                                 configuration.getAdditionalCamerasSuggestedHorizontalFocalLengthValue());
 
                         cameraEstimator.setSuggestVerticalFocalLengthEnabled(
                                 configuration.isAdditionalCamerasSuggestVerticalFocalLengthEnabled());
                         cameraEstimator.setSuggestedVerticalFocalLengthValue(
                                 configuration.getAdditionalCamerasSuggestedVerticalFocalLengthValue());
 
                         cameraEstimator.setSuggestAspectRatioEnabled(
                                 configuration.isAdditionalCamerasSuggestAspectRatioEnabled());
                         cameraEstimator.setSuggestedAspectRatioValue(
                                 configuration.getAdditionalCamerasSuggestedAspectRatioValue());
 
                         cameraEstimator.setSuggestPrincipalPointEnabled(
                                 configuration.isAdditionalCamerasSuggestPrincipalPointEnabled());
                         cameraEstimator.setSuggestedPrincipalPointValue(
                                 configuration.getAdditionalCamerasSuggestedPrincipalPointValue());
 
                         currentCamera = cameraEstimator.estimate();
                         currentCameraCovariance = cameraEstimator.getCovariance();
 
                         //noinspection unchecked
                         listener.onSamplesAccepted((R) this, viewCount, previousViewTrackedSamples,
                                 currentViewTrackedSamples);
 
                         allPreviousViewSamples.clear();
                         allPreviousViewSamples.addAll(currentViewTrackedSamples);
                         allPreviousViewSamples.addAll(currentViewNewlySpawnedSamples);
 
                         previousViewTrackedSamples = currentViewTrackedSamples;
                         previousViewId = currentViewId;
                         currentViewId = viewCount;
                     }
 
                 } catch (final Exception e) {
                     // camera estimation failed
                     samplesRejected = true;
                 }
 
             } else if (configuration.getUseUPnPForAdditionalCamerasEstimation()) {
 
                 try {
                     // use UPnP for additional cameras estimation
                     final var cameraEstimator = UPnPPointCorrespondencePinholeCameraRobustEstimator.create(points3D,
                             points2D, qualityScores, configuration.getAdditionalCamerasRobustEstimationMethod());
                     cameraEstimator.setPlanarConfigurationAllowed(
                             configuration.getAdditionalCamerasAllowPlanarConfiguration());
                     cameraEstimator.setNullspaceDimension2Allowed(
                             configuration.getAdditionalCamerasAllowNullspaceDimension2());
                     cameraEstimator.setPlanarThreshold(configuration.getAdditionalCamerasPlanarThreshold());
                     cameraEstimator.setResultRefined(configuration.areAdditionalCamerasRefined());
                     cameraEstimator.setCovarianceKept(configuration.isAdditionalCamerasCovarianceKept());
                     cameraEstimator.setFastRefinementUsed(configuration.getAdditionalCamerasUseFastRefinement());
                     cameraEstimator.setConfidence(configuration.getAdditionalCamerasConfidence());
                     cameraEstimator.setMaxIterations(configuration.getAdditionalCamerasMaxIterations());
 
                     switch (configuration.getAdditionalCamerasRobustEstimationMethod()) {
                         case LMEDS:
                             ((LMedSUPnPPointCorrespondencePinholeCameraRobustEstimator) cameraEstimator)
                                     .setStopThreshold(configuration.getAdditionalCamerasThreshold());
                             break;
                         case MSAC:
                             ((MSACUPnPPointCorrespondencePinholeCameraRobustEstimator) cameraEstimator)
                                     .setThreshold(configuration.getAdditionalCamerasThreshold());
                             break;
                         case PROMEDS:
                             ((PROMedSUPnPPointCorrespondencePinholeCameraRobustEstimator) cameraEstimator)
                                     .setStopThreshold(configuration.getAdditionalCamerasThreshold());
                             break;
                         case PROSAC:
                             var prosacCameraEstimator =
                                     (PROSACUPnPPointCorrespondencePinholeCameraRobustEstimator) cameraEstimator;
                             prosacCameraEstimator.setThreshold(configuration.getAdditionalCamerasThreshold());
                             prosacCameraEstimator.setComputeAndKeepInliersEnabled(
                                     configuration.getAdditionalCamerasComputeAndKeepInliers());
                             prosacCameraEstimator.setComputeAndKeepResidualsEnabled(
                                     configuration.getAdditionalCamerasComputeAndKeepResiduals());
                             break;
                         case RANSAC:
                             var ransacCameraEstimator =
                                     (RANSACUPnPPointCorrespondencePinholeCameraRobustEstimator) cameraEstimator;
                             ransacCameraEstimator.setThreshold(configuration.getAdditionalCamerasThreshold());
                             ransacCameraEstimator.setComputeAndKeepInliersEnabled(
                                     configuration.getAdditionalCamerasComputeAndKeepInliers());
                             ransacCameraEstimator.setComputeAndKeepResidualsEnabled(
                                     configuration.getAdditionalCamerasComputeAndKeepResiduals());
                             break;
                         default:
                             break;
                     }
 
                     cameraEstimator.setSkewness(configuration.getAdditionalCamerasSkewness());
                     cameraEstimator.setHorizontalPrincipalPoint(
                             configuration.getAdditionalCamerasHorizontalPrincipalPoint());
                     cameraEstimator.setVerticalPrincipalPoint(
                             configuration.getAdditionalCamerasVerticalPrincipalPoint());
 
                     cameraEstimator.setSuggestSkewnessValueEnabled(
                             configuration.isAdditionalCamerasSuggestSkewnessValueEnabled());
                     cameraEstimator.setSuggestedSkewnessValue(
                             configuration.getAdditionalCamerasSuggestedSkewnessValue());
 
                     cameraEstimator.setSuggestHorizontalFocalLengthEnabled(
                             configuration.isAdditionalCamerasSuggestHorizontalFocalLengthEnabled());
                     cameraEstimator.setSuggestedHorizontalFocalLengthValue(
                             configuration.getAdditionalCamerasSuggestedHorizontalFocalLengthValue());
 
                     cameraEstimator.setSuggestVerticalFocalLengthEnabled(
                             configuration.isAdditionalCamerasSuggestVerticalFocalLengthEnabled());
                     cameraEstimator.setSuggestedVerticalFocalLengthValue(
                             configuration.getAdditionalCamerasSuggestedVerticalFocalLengthValue());
 
                     cameraEstimator.setSuggestAspectRatioEnabled(
                             configuration.isAdditionalCamerasSuggestAspectRatioEnabled());
                     cameraEstimator.setSuggestedAspectRatioValue(
                             configuration.getAdditionalCamerasSuggestedAspectRatioValue());
 
                     cameraEstimator.setSuggestPrincipalPointEnabled(
                             configuration.isAdditionalCamerasSuggestPrincipalPointEnabled());
                     cameraEstimator.setSuggestedPrincipalPointValue(
                             configuration.getAdditionalCamerasSuggestedPrincipalPointValue());
 
                     currentCamera = cameraEstimator.estimate();
                     currentCameraCovariance = cameraEstimator.getCovariance();
 
                     //noinspection unchecked
                     listener.onSamplesAccepted((R) this, viewCount, previousViewTrackedSamples,
                             currentViewTrackedSamples);
 
                     allPreviousViewSamples.clear();
                     allPreviousViewSamples.addAll(currentViewTrackedSamples);
                     allPreviousViewSamples.addAll(currentViewNewlySpawnedSamples);
 
                     previousViewTrackedSamples = currentViewTrackedSamples;
                     previousViewId = currentViewId;
                     currentViewId = viewCount;
 
                 } catch (final Exception e) {
                     // camera estimation failed
                     samplesRejected = true;
                 }
 
             } else {
 
                 try {
                     // use DLT for additional cameras estimation
                     final var cameraEstimator = DLTPointCorrespondencePinholeCameraRobustEstimator.create(points3D,
                             points2D, qualityScores, configuration.getAdditionalCamerasRobustEstimationMethod());
                     cameraEstimator.setResultRefined(configuration.areAdditionalCamerasRefined());
                     cameraEstimator.setCovarianceKept(configuration.isAdditionalCamerasCovarianceKept());
                     cameraEstimator.setFastRefinementUsed(configuration.getAdditionalCamerasUseFastRefinement());
                     cameraEstimator.setConfidence(configuration.getAdditionalCamerasConfidence());
                     cameraEstimator.setMaxIterations(configuration.getAdditionalCamerasMaxIterations());
 
                     switch (configuration.getAdditionalCamerasRobustEstimationMethod()) {
                         case LMEDS:
                             ((LMedSDLTPointCorrespondencePinholeCameraRobustEstimator) cameraEstimator)
                                     .setStopThreshold(configuration.getAdditionalCamerasThreshold());
                             break;
                         case MSAC:
                             ((MSACDLTPointCorrespondencePinholeCameraRobustEstimator) cameraEstimator)
                                     .setThreshold(configuration.getAdditionalCamerasThreshold());
                             break;
                         case PROMEDS:
                             ((PROMedSDLTPointCorrespondencePinholeCameraRobustEstimator) cameraEstimator)
                                     .setStopThreshold(configuration.getAdditionalCamerasThreshold());
                             break;
                         case PROSAC:
                             var prosacCameraEstimator =
                                     (PROSACDLTPointCorrespondencePinholeCameraRobustEstimator) cameraEstimator;
                             prosacCameraEstimator.setThreshold(configuration.getAdditionalCamerasThreshold());
                             prosacCameraEstimator.setComputeAndKeepInliersEnabled(
                                     configuration.getAdditionalCamerasComputeAndKeepInliers());
                             prosacCameraEstimator.setComputeAndKeepResidualsEnabled(
                                     configuration.getAdditionalCamerasComputeAndKeepResiduals());
                             break;
                         case RANSAC:
                             var ransacCameraEstimator =
                                     (RANSACDLTPointCorrespondencePinholeCameraRobustEstimator) cameraEstimator;
                             ransacCameraEstimator.setThreshold(configuration.getAdditionalCamerasThreshold());
                             ransacCameraEstimator.setComputeAndKeepInliersEnabled(
                                     configuration.getAdditionalCamerasComputeAndKeepInliers());
                             ransacCameraEstimator.setComputeAndKeepResidualsEnabled(
                                     configuration.getAdditionalCamerasComputeAndKeepResiduals());
                             break;
                         default:
                             break;
                     }
 
                     cameraEstimator.setSuggestSkewnessValueEnabled(
                             configuration.isAdditionalCamerasSuggestSkewnessValueEnabled());
                     cameraEstimator.setSuggestedSkewnessValue(
                             configuration.getAdditionalCamerasSuggestedSkewnessValue());
 
                     cameraEstimator.setSuggestHorizontalFocalLengthEnabled(
                             configuration.isAdditionalCamerasSuggestHorizontalFocalLengthEnabled());
                     cameraEstimator.setSuggestedHorizontalFocalLengthValue(
                             configuration.getAdditionalCamerasSuggestedHorizontalFocalLengthValue());
 
                     cameraEstimator.setSuggestVerticalFocalLengthEnabled(
                             configuration.isAdditionalCamerasSuggestVerticalFocalLengthEnabled());
                     cameraEstimator.setSuggestedVerticalFocalLengthValue(
                             configuration.getAdditionalCamerasSuggestedVerticalFocalLengthValue());
 
                     cameraEstimator.setSuggestAspectRatioEnabled(
                             configuration.isAdditionalCamerasSuggestAspectRatioEnabled());
                     cameraEstimator.setSuggestedAspectRatioValue(
                             configuration.getAdditionalCamerasSuggestedAspectRatioValue());
 
                     cameraEstimator.setSuggestPrincipalPointEnabled(
                             configuration.isAdditionalCamerasSuggestPrincipalPointEnabled());
                     cameraEstimator.setSuggestedPrincipalPointValue(
                             configuration.getAdditionalCamerasSuggestedPrincipalPointValue());
 
                     currentCamera = cameraEstimator.estimate();
                     currentCameraCovariance = cameraEstimator.getCovariance();
 
                     //noinspection unchecked
                     listener.onSamplesAccepted((R) this, viewCount, previousViewTrackedSamples,
                             currentViewTrackedSamples);
 
                     allPreviousViewSamples.clear();
                     allPreviousViewSamples.addAll(currentViewTrackedSamples);
                     allPreviousViewSamples.addAll(currentViewNewlySpawnedSamples);
 
                     previousViewTrackedSamples = currentViewTrackedSamples;
                     previousViewId = currentViewId;
                     currentViewId = viewCount;
 
                 } catch (final Exception e) {
                     // camera estimation failed
                     samplesRejected = true;
                 }
             }
 
             if (!samplesRejected) {
                 previousMetricEstimatedCamera = currentMetricEstimatedCamera;
 
                 currentMetricEstimatedCamera = new EstimatedCamera();
                 currentMetricEstimatedCamera.setCamera(currentCamera);
                 currentMetricEstimatedCamera.setViewId(currentViewId);
                 currentMetricEstimatedCamera.setCovariance(currentCameraCovariance);
 
                 // notify camera estimation
                 //noinspection unchecked
                 listener.onMetricCameraEstimated((R) this, previousViewId, currentViewId, previousMetricEstimatedCamera,
                         currentMetricEstimatedCamera);
 
                 // reconstruct all matches and refine existing reconstructed points
                 reconstructAndRefineMatches();
 
                 // notify reconstruction update
                 //noinspection unchecked
                 listener.onMetricReconstructedPointsEstimated((R) this, matches, activeMetricReconstructedPoints);
 
                 if (!postProcessOne(false)) {
                     // something failed
                     failed = true;
                     //noinspection unchecked
                     listener.onFail((R) this);
                     return false;
                 } else {
                     // post-processing succeeded
                     //noinspection unchecked
                     listener.onEuclideanCameraEstimated((R) this, previousViewId, currentViewId, currentScale,
                             previousEuclideanEstimatedCamera, currentEuclideanEstimatedCamera);
                     //noinspection unchecked
                     listener.onEuclideanReconstructedPointsEstimated((R) this, currentScale,
                             activeEuclideanReconstructedPoints);
                     return true;
                 }
             }
         }
 
         //noinspection unchecked
         listener.onSamplesRejected((R) this, viewCount, previousViewTrackedSamples, currentViewTrackedSamples);
         return false;
     }
 
     /**
      * Reconstructs new 3D points or refines existing ones taking into account existing matches and estimated cameras
      */
     private void reconstructAndRefineMatches() {
         if (matches.isEmpty()) {
             return;
         }
 
         try {
             RobustSinglePoint3DTriangulator robustTriangulator = null;
             SinglePoint3DTriangulator triangulator = null;
             var qualityScoresRequired = false;
             if (configuration.getAdditionalCamerasRobustEstimationMethod() != null) {
                 robustTriangulator = RobustSinglePoint3DTriangulator.create(
                         configuration.getAdditionalCamerasRobustEstimationMethod());
                 robustTriangulator.setConfidence(configuration.getPointTriangulatorConfidence());
                 robustTriangulator.setMaxIterations(configuration.getPointTriangulatorMaxIterations());
 
                 var threshold = configuration.getPointTriangulatorThreshold();
                 switch (configuration.getAdditionalCamerasRobustEstimationMethod()) {
                     case LMEDS:
                         ((LMedSRobustSinglePoint3DTriangulator) robustTriangulator).setStopThreshold(threshold);
                         break;
                     case MSAC:
                         ((MSACRobustSinglePoint3DTriangulator) robustTriangulator).setThreshold(threshold);
                         break;
                     case PROMEDS:
                         ((PROMedSRobustSinglePoint3DTriangulator) robustTriangulator).setStopThreshold(threshold);
                         qualityScoresRequired = true;
                         break;
                     case PROSAC:
                         ((PROSACRobustSinglePoint3DTriangulator) robustTriangulator).setThreshold(threshold);
                         qualityScoresRequired = true;
                         break;
                     case RANSAC:
                         ((RANSACRobustSinglePoint3DTriangulator) robustTriangulator).setThreshold(threshold);
                         break;
                     default:
                         break;
                 }
 
             } else {
                 if (configuration.isHomogeneousPointTriangulatorUsed()) {
                     triangulator = SinglePoint3DTriangulator.create(
                             Point3DTriangulatorType.LMSE_HOMOGENEOUS_TRIANGULATOR);
                 } else {
                     triangulator = SinglePoint3DTriangulator.create(
                             Point3DTriangulatorType.LMSE_INHOMOGENEOUS_TRIANGULATOR);
                 }
             }
 
             activeMetricReconstructedPoints = new ArrayList<>();
             ReconstructedPoint3D reconstructedPoint;
             var matchPos = 0;
             for (final var match : matches) {
                 final var samples = match.getSamples();
                 final var estimatedCameras = match.getCameras();
 
                 // estimated cameras does not yet contain last estimated camera
                 if (samples.length != estimatedCameras.length + 1) {
                     continue;
                 }
 
                 final var points = new ArrayList<Point2D>();
                 final var cameras = new ArrayList<PinholeCamera>();
                 final var validSamples = new BitSet(samples.length);
                 PinholeCamera camera = null;
                 Point2D point2D;
                 var numValid = 0;
                 final var samplesLength = samples.length;
                 final var samplesLengthMinusOne = samplesLength - 1;
                 boolean isLast;
                 for (var i = 0; i < samples.length; i++) {
                     isLast = (i == samplesLengthMinusOne);
                     point2D = samples[i].getPoint();
 
                     if (!isLast) {
                         camera = estimatedCameras[i].getCamera();
                     }
 
                     if (point2D == null || (camera == null && !isLast)) {
                         validSamples.clear(i);
                     } else {
                         validSamples.set(i);
 
                         points.add(point2D);
                         if (!isLast) {
                             cameras.add(camera);
                         }
 
                         numValid++;
                     }
                 }
 
                 // also add current camera which is not yet available on estimated cameras array
                 cameras.add(currentMetricEstimatedCamera.getCamera());
 
                 if (points.size() < SinglePoint3DTriangulator.MIN_REQUIRED_VIEWS || points.size() != cameras.size()) {
                     // point cannot be triangulated
                     continue;
                 }
 
                 Point3D point3D;
                 if (robustTriangulator != null) {
                     robustTriangulator.setPointsAndCameras(points, cameras);
                     if (qualityScoresRequired) {
                         // copy quality scores
                         final var qualityScores = new double[numValid];
                         var j = 0;
                         for (var i = 0; i < samples.length; i++) {
                             if (validSamples.get(i)) {
                                 qualityScores[j] = samples[i].getQualityScore();
                                 j++;
                             }
                         }
                         robustTriangulator.setQualityScores(qualityScores);
                     }
 
                     point3D = robustTriangulator.triangulate();
 
                 } else if (triangulator != null) {
                     triangulator.setPointsAndCameras(points, cameras);
                     point3D = triangulator.triangulate();
 
                 } else {
                     continue;
                 }
 
                 // save triangulated point
                 reconstructedPoint = new ReconstructedPoint3D();
                 reconstructedPoint.setPoint(point3D);
                 reconstructedPoint.setInlier(true);
                 reconstructedPoint.setId(String.valueOf(matchPos));
                 match.setReconstructedPoint(reconstructedPoint);
 
                 activeMetricReconstructedPoints.add(reconstructedPoint);
 
                 matchPos++;
             }
         } catch (final Exception e) {
             // something failed
             failed = true;
             //noinspection all
             listener.onFail((R) this);
         }
     }
 
     /**
      * Setups current matched 3D/2D points to estimate a pinhole camera.
      *
      * @param points3D 3D matched points.
      * @param points2D 2D matched points.
      * @return quality scores for matched points.
      */
     private double[] setUpCameraEstimatorMatches(final List<Point3D> points3D, final List<Point2D> points2D) {
         if (matches.isEmpty()) {
             return null;
         }
 
         points3D.clear();
         points2D.clear();
 
         final var qualityScoresRequired =
                 configuration.getAdditionalCamerasRobustEstimationMethod() == RobustEstimatorMethod.PROSAC
                         || configuration.getAdditionalCamerasRobustEstimationMethod() == RobustEstimatorMethod.PROMEDS;
 
 
         int[] positions = null;
         if (qualityScoresRequired) {
             positions = new int[matches.size()];
         }
 
         var numMatches = 0;
         var i = 0;
         for (final var match : matches) {
             final var samples = match.getSamples();
             final var viewIds = match.getViewIds();
             final var pos = getPositionForViewId(viewIds, viewCount);
             if (pos < 0) {
                 continue;
             }
             if (positions != null) {
                 positions[i] = pos;
             }
 
             final var sample = samples[pos];
             final var reconstructedPoint3D = match.getReconstructedPoint();
 
             if (sample == null || sample.getPoint() == null || reconstructedPoint3D == null
                     || reconstructedPoint3D.getPoint() == null) {
                 if (positions != null) {
                     positions[i] = -1;
                 }
             } else {
                 points2D.add(sample.getPoint());
                 points3D.add(reconstructedPoint3D.getPoint());
                 numMatches++;
             }
 
             i++;
         }
 
         // pick quality scores
         double[] qualityScores = null;
         if (qualityScoresRequired && numMatches > 0) {
             qualityScores = new double[numMatches];
             var j = 0;
             for (i = 0; i < positions.length; i++) {
                 if (positions[i] < 0) {
                     continue;
                 }
 
                 qualityScores[j] = matches.get(i).getQualityScore();
                 j++;
             }
         }
 
         return qualityScores;
     }
 
     /**
      * Estimates additional camera intrinsics using DIAC (Dual Image of Absolute Conic) method.
      *
      * @return additional camera intrinsics or null if something fails.
      */
     private PinholeCameraIntrinsicParameters estimateIntrinsicsDIAC() {
         final var fundamentalMatrix = currentEstimatedFundamentalMatrix.getFundamentalMatrix();
 
         try {
             final var diacEstimator = new KruppaDualImageOfAbsoluteConicEstimator(fundamentalMatrix);
             diacEstimator.setPrincipalPointX(configuration.getAdditionalCamerasHorizontalPrincipalPoint());
             diacEstimator.setPrincipalPointY(configuration.getAdditionalCamerasVerticalPrincipalPoint());
             diacEstimator.setFocalDistanceAspectRatioKnown(true);
             diacEstimator.setFocalDistanceAspectRatio(configuration.getAdditionalCamerasAspectRatio());
 
             final var diac = diacEstimator.estimate();
             return diac.getIntrinsicParameters();
 
         } catch (final Exception e) {
             return null;
         }
     }
 
     /**
      * Estimates additional cameras intrinsics using DAQ (Dual Absolute Quadric) method.
      *
      * @return additional camera intrinsics or null if something fails.
      */
     private PinholeCameraIntrinsicParameters estimateIntrinsicsDAQ() {
         try {
             final var fundamentalMatrix = currentEstimatedFundamentalMatrix.getFundamentalMatrix();
             fundamentalMatrix.normalize();
 
             final var estimator = new DualAbsoluteQuadricInitialCamerasEstimator(fundamentalMatrix);
             estimator.setAspectRatio(configuration.getInitialCamerasAspectRatio());
             estimator.estimate();
 
             final var camera = estimator.getEstimatedLeftCamera();
             camera.decompose();
             return camera.getIntrinsicParameters();
 
         } catch (final Exception e) {
             return null;
         }
     }
 
     /**
      * Indicates whether there are enough matched points to estimate an additional camera.
      *
      * @param points3D 3D matched points to check.
      * @param points2D 2D matched points to check.
      * @return true if there are enough matched points, false otherwise.
      */
     private boolean hasEnoughSamplesForCameraEstimation(final List<Point3D> points3D, final List<Point2D> points2D) {
         return points3D != null && points2D != null && points3D.size() == points2D.size()
                 && hasEnoughSamplesOrMatchesForCameraEstimation(points3D.size());
     }
 
     /**
      * Indicates whether there are enough matches to estimate an additional camera.
      *
      * @param matches matches to check.
      * @return true if there are enough matches, false otherwise.
      */
     private boolean hasEnoughMatchesForCameraEstimation(final List<MatchedSamples> matches) {
         return hasEnoughSamplesOrMatchesForCameraEstimation(matches != null ? matches.size() : 0);
     }
 
     /**
      * Indicates whether there are enough matches or samples to estimate an additional
      * camera.
      *
      * @param count number of matches or samples.
      * @return true if there are enough matches or samples, false otherwise.
      */
     private boolean hasEnoughSamplesOrMatchesForCameraEstimation(final int count) {
         if (configuration.getUseDAQForAdditionalCamerasIntrinsics()
                 || configuration.getUseDIACForAdditionalCamerasIntrinsics()) {
             // when DAQ or DIAC is required for additional cameras, fundamental matrix
             // also needs to be computed, which requires 7 or 8 matches.
             return hasEnoughSamplesOrMatchesForFundamentalMatrixEstimation(count);
         } else {
             // EPnP, UPnP or DLT is used for additional cameras estimation without fundamental
             // matrix. Only 6 matches are required
             return count >= PointCorrespondencePinholeCameraRobustEstimator.MIN_NUMBER_OF_POINT_CORRESPONDENCES;
         }
     }
 
     /**
      * Indicates whether there are enough samples to estimate a fundamental matrix.
      *
      * @param samples samples to check.
      * @return true if there are enough samples, false otherwise.
      */
     private boolean hasEnoughSamplesForFundamentalMatrixEstimation(final List<Sample2D> samples) {
         return hasEnoughSamplesOrMatchesForFundamentalMatrixEstimation(samples != null ? samples.size() : 0);
     }
 
     /**
      * Indicates whether there are enough matches to estimate a fundamental matrix.
      *
      * @param matches matches to check.
      * @return true if there are enough matches, false otherwise.
      */
     private boolean hasEnoughMatchesForFundamentalMatrixEstimation(final List<MatchedSamples> matches) {
         return hasEnoughSamplesOrMatchesForFundamentalMatrixEstimation(matches != null ? matches.size() : 0);
     }
 
     /**
      * Indicates whether there are enough matches or samples to estimate a fundamental
      * matrix.
      *
      * @param count number of matches or samples.
      * @return true if there are enough matches or samples, false otherwise.
      */
     private boolean hasEnoughSamplesOrMatchesForFundamentalMatrixEstimation(final int count) {
         if (configuration.isGeneralSceneAllowed()) {
             if (configuration.getNonRobustFundamentalMatrixEstimatorMethod()
                     == FundamentalMatrixEstimatorMethod.EIGHT_POINTS_ALGORITHM) {
                 return count >= EightPointsFundamentalMatrixEstimator.MIN_REQUIRED_POINTS;
             } else if (configuration.getNonRobustFundamentalMatrixEstimatorMethod()
                     == FundamentalMatrixEstimatorMethod.SEVEN_POINTS_ALGORITHM) {
                 return count >= SevenPointsFundamentalMatrixEstimator.MIN_REQUIRED_POINTS;
             }
         } else if (configuration.isPlanarSceneAllowed()) {
             return count >= ProjectiveTransformation2DRobustEstimator.MINIMUM_SIZE;
         }
         return false;
     }
 
     /**
      * Estimates fundamental matrix for provided matches, when 3D points lay in a general
      * non-degenerate 3D configuration.
      *
      * @param matches              pairs of matches to find fundamental matrix.
      * @param viewId1              id of first view being related by estimated fundamental matrix.
      * @param viewId2              id of second view being related by estimated fundamental matrix.
      * @param isInitialPairOfViews true if fundamental matrix needs to be estimated for the initial
      *                             pair of views, false otherwise.
      * @return true if estimation succeeded, false otherwise.
      */
     private boolean estimateFundamentalMatrix(
             final List<MatchedSamples> matches, final int viewId1, final int viewId2,
             final boolean isInitialPairOfViews) {
         if (matches == null) {
             return false;
         }
 
         final var count = matches.size();
         final var leftSamples = new ArrayList<Sample2D>(count);
         final var rightSamples = new ArrayList<Sample2D>(count);
         final var leftPoints = new ArrayList<Point2D>(count);
         final var rightPoints = new ArrayList<Point2D>(count);
         final var qualityScores = new double[count];
         final double principalPointX;
         final double principalPointY;
         if (isInitialPairOfViews) {
             if (configuration.getInitialCamerasEstimatorMethod() == InitialCamerasEstimatorMethod.DUAL_ABSOLUTE_QUADRIC
                     || configuration.getInitialCamerasEstimatorMethod()
                     == InitialCamerasEstimatorMethod.DUAL_ABSOLUTE_QUADRIC_AND_ESSENTIAL_MATRIX) {
                 principalPointX = configuration.getPrincipalPointX();
                 principalPointY = configuration.getPrincipalPointY();
             } else {
                 principalPointX = principalPointY = 0.0;
             }
         } else {
             if (configuration.getUseDIACForAdditionalCamerasIntrinsics()
                     || configuration.getUseDAQForAdditionalCamerasIntrinsics()) {
                 principalPointX = configuration.getAdditionalCamerasHorizontalPrincipalPoint();
                 principalPointY = configuration.getAdditionalCamerasVerticalPrincipalPoint();
             } else {
                 principalPointX = principalPointY = 0.0;
             }
         }
 
         var i = 0;
         for (final var match : matches) {
             final var samples = match.getSamples();
             if (samples.length < MIN_NUMBER_OF_VIEWS) {
                 return false;
             }
 
             final var viewIds = match.getViewIds();
             final var pos1 = getPositionForViewId(viewIds, viewId1);
             if (pos1 < 0) {
                 return false;
             }
 
             final var pos2 = getPositionForViewId(viewIds, viewId2);
             if (pos2 < 0) {
                 return false;
             }
 
             final var leftSample = samples[pos1];
             final var rightSample = samples[pos2];
             final var p1 = leftSample.getPoint();
             final var p2 = rightSample.getPoint();
 
             leftSamples.add(leftSample);
             rightSamples.add(rightSample);
 
             final var leftPoint = Point2D.create();
             leftPoint.setInhomogeneousCoordinates(p1.getInhomX() - principalPointX,
                     p1.getInhomY() - principalPointY);
             leftPoints.add(leftPoint);
 
             final var rightPoint = Point2D.create();
             rightPoint.setInhomogeneousCoordinates(p2.getInhomX() - principalPointX,
                     p2.getInhomY() - principalPointY);
             rightPoints.add(rightPoint);
 
             qualityScores[i] = match.getQualityScore();
             i++;
         }
 
         try {
             final var estimator = FundamentalMatrixRobustEstimator.create(leftPoints, rightPoints, qualityScores,
                     configuration.getRobustFundamentalMatrixEstimatorMethod());
             estimator.setNonRobustFundamentalMatrixEstimatorMethod(
                     configuration.getNonRobustFundamentalMatrixEstimatorMethod());
             estimator.setResultRefined(configuration.isFundamentalMatrixRefined());
             estimator.setCovarianceKept(configuration.isFundamentalMatrixCovarianceKept());
             estimator.setConfidence(configuration.getFundamentalMatrixConfidence());
             estimator.setMaxIterations(configuration.getFundamentalMatrixMaxIterations());
 
             switch (configuration.getRobustFundamentalMatrixEstimatorMethod()) {
                 case LMEDS:
                     ((LMedSFundamentalMatrixRobustEstimator) estimator)
                             .setStopThreshold(configuration.getFundamentalMatrixThreshold());
                     break;
                 case MSAC:
                     ((MSACFundamentalMatrixRobustEstimator) estimator)
                             .setThreshold(configuration.getFundamentalMatrixThreshold());
                     break;
                 case PROMEDS:
                     ((PROMedSFundamentalMatrixRobustEstimator) estimator)
                             .setStopThreshold(configuration.getFundamentalMatrixThreshold());
                     break;
                 case PROSAC:
                     var prosacEstimator = (PROSACFundamentalMatrixRobustEstimator) estimator;
                     prosacEstimator.setThreshold(configuration.getFundamentalMatrixThreshold());
                     prosacEstimator.setComputeAndKeepInliersEnabled(
                             configuration.getFundamentalMatrixComputeAndKeepInliers());
                     prosacEstimator.setComputeAndKeepResidualsEnabled(
                             configuration.getFundamentalMatrixComputeAndKeepResiduals());
                     break;
                 case RANSAC:
                     var ransacEstimator = (RANSACFundamentalMatrixRobustEstimator) estimator;
                     ransacEstimator.setThreshold(configuration.getFundamentalMatrixThreshold());
                     ransacEstimator.setComputeAndKeepInliersEnabled(
                             configuration.getFundamentalMatrixComputeAndKeepInliers());
                     ransacEstimator.setComputeAndKeepResidualsEnabled(
                             configuration.getFundamentalMatrixComputeAndKeepResiduals());
                     break;
                 default:
                     break;
             }
 
             final var fundamentalMatrix = estimator.estimate();
 
             currentEstimatedFundamentalMatrix = new EstimatedFundamentalMatrix();
             currentEstimatedFundamentalMatrix.setFundamentalMatrix(fundamentalMatrix);
             currentEstimatedFundamentalMatrix.setViewId1(viewId1);
             currentEstimatedFundamentalMatrix.setViewId2(viewId2);
             currentEstimatedFundamentalMatrix.setCovariance(estimator.getCovariance());
 
             // determine quality score and inliers
             final var inliersData = estimator.getInliersData();
             if (inliersData != null) {
                 final var numInliers = inliersData.getNumInliers();
                 final var inliers = inliersData.getInliers();
                 final var length = inliers.length();
                 var fundamentalMatrixQualityScore = 0.0;
                 for (i = 0; i < length; i++) {
                     if (inliers.get(i)) {
                         // inlier
                         fundamentalMatrixQualityScore += qualityScores[i] / numInliers;
                     }
                 }
                 currentEstimatedFundamentalMatrix.setQualityScore(fundamentalMatrixQualityScore);
                 currentEstimatedFundamentalMatrix.setInliers(inliers);
             }
 
             // store left/right samples
             currentEstimatedFundamentalMatrix.setLeftSamples(leftSamples);
             currentEstimatedFundamentalMatrix.setRightSamples(rightSamples);
 
             return true;
         } catch (final Exception e) {
             return false;
         }
     }
 
     /**
      * Estimates fundamental matrix for provided matches, when 3D points lay in a planar 3D scene.
      *
      * @param matches              pairs of matches to find fundamental matrix.
      * @param viewId1              id of first view being related by estimated fundamental matrix.
      * @param viewId2              id of second view being related by estimated fundamental matrix.
      * @param isInitialPairOfViews true if fundamental matrix needs to be estimated for the initial
      *                             pair of views, false otherwise.
      * @return true if estimation succeeded, false otherwise.
      */
     private boolean estimatePlanarFundamentalMatrix(
             final List<MatchedSamples> matches, final int viewId1, final int viewId2,
             final boolean isInitialPairOfViews) {
         if (matches == null) {
             return false;
         }
 
         final var count = matches.size();
         final var leftSamples = new ArrayList<Sample2D>(count);
         final var rightSamples = new ArrayList<Sample2D>(count);
         final var leftPoints = new ArrayList<Point2D>(count);
         final var rightPoints = new ArrayList<Point2D>(count);
         final var qualityScores = new double[count];
         double principalPointX;
         double principalPointY;
         if (isInitialPairOfViews) {
             if (configuration.getInitialCamerasEstimatorMethod() == InitialCamerasEstimatorMethod.DUAL_ABSOLUTE_QUADRIC
                     || configuration.getInitialCamerasEstimatorMethod()
                     == InitialCamerasEstimatorMethod.DUAL_ABSOLUTE_QUADRIC_AND_ESSENTIAL_MATRIX) {
                 principalPointX = configuration.getPrincipalPointX();
                 principalPointY = configuration.getPrincipalPointY();
             } else {
                 principalPointX = principalPointY = 0.0;
             }
         } else {
             if (configuration.getUseDIACForAdditionalCamerasIntrinsics()
                     || configuration.getUseDAQForAdditionalCamerasIntrinsics()) {
                 principalPointX = configuration.getAdditionalCamerasHorizontalPrincipalPoint();
                 principalPointY = configuration.getAdditionalCamerasVerticalPrincipalPoint();
             } else {
                 principalPointX = principalPointY = 0.0;
             }
         }
 
         var i = 0;
         for (final var match : matches) {
             final var samples = match.getSamples();
             if (samples.length < MIN_NUMBER_OF_VIEWS) {
                 return false;
             }
 
             final var viewIds = match.getViewIds();
             final var pos1 = getPositionForViewId(viewIds, viewId1);
             if (pos1 < 0) {
                 return false;
             }
 
             final var pos2 = getPositionForViewId(viewIds, viewId2);
             if (pos2 < 0) {
                 return false;
             }
 
             final var leftSample = samples[pos1];
             final var rightSample = samples[pos2];
             final var p1 = leftSample.getPoint();
             final var p2 = rightSample.getPoint();
 
             leftSamples.add(leftSample);
             rightSamples.add(rightSample);
 
             final var leftPoint = Point2D.create();
             leftPoint.setInhomogeneousCoordinates(p1.getInhomX() - principalPointX,
                     p1.getInhomY() - principalPointY);
             leftPoints.add(leftPoint);
 
             final var rightPoint = Point2D.create();
             rightPoint.setInhomogeneousCoordinates(p2.getInhomX() - principalPointX,
                     p2.getInhomY() - principalPointY);
             rightPoints.add(rightPoint);
 
             qualityScores[i] = match.getQualityScore();
             i++;
         }
 
         try {
             final var homographyEstimator = PointCorrespondenceProjectiveTransformation2DRobustEstimator.create(
                     configuration.getRobustPlanarHomographyEstimatorMethod());
             homographyEstimator.setResultRefined(configuration.isPlanarHomographyRefined());
             homographyEstimator.setCovarianceKept(configuration.isPlanarHomographyCovarianceKept());
             homographyEstimator.setConfidence(configuration.getPlanarHomographyConfidence());
             homographyEstimator.setMaxIterations(configuration.getPlanarHomographyMaxIterations());
 
             switch (configuration.getRobustPlanarHomographyEstimatorMethod()) {
                 case LMEDS:
                     ((LMedSPointCorrespondenceProjectiveTransformation2DRobustEstimator) homographyEstimator)
                             .setStopThreshold(configuration.getPlanarHomographyThreshold());
                     break;
                 case MSAC:
                     ((MSACPointCorrespondenceProjectiveTransformation2DRobustEstimator) homographyEstimator)
                             .setThreshold(configuration.getPlanarHomographyThreshold());
                     break;
                 case PROMEDS:
                     ((PROMedSPointCorrespondenceProjectiveTransformation2DRobustEstimator) homographyEstimator)
                             .setStopThreshold(configuration.getPlanarHomographyThreshold());
                     break;
                 case PROSAC:
                     final var prosacHomographyEstimator =
                             (PROSACPointCorrespondenceProjectiveTransformation2DRobustEstimator) homographyEstimator;
 
                     prosacHomographyEstimator.setThreshold(configuration.getPlanarHomographyThreshold());
                     prosacHomographyEstimator.setComputeAndKeepInliersEnabled(
                             configuration.getPlanarHomographyComputeAndKeepInliers());
                     prosacHomographyEstimator.setComputeAndKeepResidualsEnabled(
                             configuration.getPlanarHomographyComputeAndKeepResiduals());
                     break;
                 case RANSAC:
                     final var ransacHomographyEstimator =
                             (RANSACPointCorrespondenceProjectiveTransformation2DRobustEstimator) homographyEstimator;
 
                     ransacHomographyEstimator.setThreshold(configuration.getPlanarHomographyThreshold());
                     ransacHomographyEstimator.setComputeAndKeepInliersEnabled(
                             configuration.getPlanarHomographyComputeAndKeepInliers());
                     ransacHomographyEstimator.setComputeAndKeepResidualsEnabled(
                             configuration.getPlanarHomographyComputeAndKeepResiduals());
                     break;
                 default:
                     break;
             }
 
             final var fundamentalMatrixEstimator = new PlanarBestFundamentalMatrixEstimatorAndReconstructor();
             fundamentalMatrixEstimator.setHomographyEstimator(homographyEstimator);
             fundamentalMatrixEstimator.setLeftAndRightPoints(leftPoints, rightPoints);
             fundamentalMatrixEstimator.setQualityScores(qualityScores);
 
             var intrinsic1 = configuration.getInitialIntrinsic1();
             var intrinsic2 = configuration.getInitialIntrinsic1();
             if (intrinsic1 == null && intrinsic2 == null) {
                 // estimate homography
                 final var homography = homographyEstimator.estimate();
 
                 // estimate intrinsic parameters using the Image of Absolute
                 // Conic (IAC)
                 final var homographies = new ArrayList<Transformation2D>();
                 homographies.add(homography);
 
                 final var iacEstimator = new LMSEImageOfAbsoluteConicEstimator(homographies);
                 final var iac = iacEstimator.estimate();
 
                 intrinsic1 = intrinsic2 = iac.getIntrinsicParameters();
 
             } else if (intrinsic1 == null) { //&& intrinsic2 != null
                 intrinsic1 = intrinsic2;
             } else if (intrinsic2 == null) { //&& intrinsic1 != null
                 intrinsic2 = intrinsic1;
             }
             fundamentalMatrixEstimator.setLeftIntrinsics(intrinsic1);
             fundamentalMatrixEstimator.setRightIntrinsics(intrinsic2);
 
             fundamentalMatrixEstimator.estimateAndReconstruct();
 
             final var fundamentalMatrix = fundamentalMatrixEstimator.getFundamentalMatrix();
 
             currentEstimatedFundamentalMatrix = new EstimatedFundamentalMatrix();
             currentEstimatedFundamentalMatrix.setFundamentalMatrix(fundamentalMatrix);
             currentEstimatedFundamentalMatrix.setViewId1(viewId1);
             currentEstimatedFundamentalMatrix.setViewId2(viewId2);
 
             // determine quality score and inliers
             final var inliersData = homographyEstimator.getInliersData();
             if (inliersData != null) {
                 final var numInliers = inliersData.getNumInliers();
                 final var inliers = inliersData.getInliers();
                 final var length = inliers.length();
                 var fundamentalMatrixQualityScore = 0.0;
                 for (i = 0; i < length; i++) {
                     if (inliers.get(i)) {
                         // inlier
                         fundamentalMatrixQualityScore += qualityScores[i] / numInliers;
                     }
                 }
                 currentEstimatedFundamentalMatrix.setQualityScore(fundamentalMatrixQualityScore);
                 currentEstimatedFundamentalMatrix.setInliers(inliers);
             }
 
             // store left/right samples
             currentEstimatedFundamentalMatrix.setLeftSamples(leftSamples);
             currentEstimatedFundamentalMatrix.setRightSamples(rightSamples);
 
             return true;
         } catch (final Exception e) {
             return false;
         }
     }
 
     /**
      * Gets position of a view id within provided array of view id's.
      *
      * @param viewIds array of view IDs where search is done.
      * @param viewId  view id to be searched.
      * @return position where view id is found or -1 if not found.
      */
     private int getPositionForViewId(final int[] viewIds, final int viewId) {
         final var length = viewIds.length;
         for (var i = 0; i < length; i++) {
             if (viewIds[i] == viewId) {
                 return i;
             }
         }
         return -1;
     }
 
     /**
      * Estimates initial cameras and reconstructed points.
      *
      * @return true if cameras and points could be estimated, false if something
      * failed.
      */
     private boolean estimateInitialCamerasAndPoints() {
         return switch (configuration.getInitialCamerasEstimatorMethod()) {
             case ESSENTIAL_MATRIX -> estimateInitialCamerasAndPointsEssential();
             case DUAL_IMAGE_OF_ABSOLUTE_CONIC -> estimateInitialCamerasAndPointsDIAC();
             case DUAL_ABSOLUTE_QUADRIC -> estimateInitialCamerasAndPointsDAQ();
             default -> estimateInitialCamerasAndPointsDAQAndEssential();
         };
     }
 
     /**
      * Estimates initial cameras and reconstructed points using the Dual
      * Absolute Quadric to estimate intrinsic parameters and then use those
      * intrinsic parameters with the essential matrix.
      *
      * @return true if cameras and points could be estimated, false if something
      * failed.
      */
     private boolean estimateInitialCamerasAndPointsDAQAndEssential() {
         try {
             final var fundamentalMatrix = currentEstimatedFundamentalMatrix.getFundamentalMatrix();
 
             final var estimator = new DualAbsoluteQuadricInitialCamerasEstimator(fundamentalMatrix);
             estimator.setAspectRatio(configuration.getInitialCamerasAspectRatio());
             estimator.estimate();
 
             final var camera1 = estimator.getEstimatedLeftCamera();
             final var camera2 = estimator.getEstimatedRightCamera();
 
             camera1.decompose();
             camera2.decompose();
 
             final var intrinsicZeroPrincipalPoint1 = camera1.getIntrinsicParameters();
             final var intrinsicZeroPrincipalPoint2 = camera2.getIntrinsicParameters();
 
             final double principalPointX = configuration.getPrincipalPointX();
             final double principalPointY = configuration.getPrincipalPointY();
 
             final var intrinsic1 = new PinholeCameraIntrinsicParameters(intrinsicZeroPrincipalPoint1);
             intrinsic1.setHorizontalPrincipalPoint(intrinsic1.getHorizontalPrincipalPoint() + principalPointX);
             intrinsic1.setVerticalPrincipalPoint(intrinsic1.getVerticalPrincipalPoint() + principalPointY);
 
             final var intrinsic2 = new PinholeCameraIntrinsicParameters(intrinsicZeroPrincipalPoint2);
             intrinsic2.setHorizontalPrincipalPoint(intrinsic2.getHorizontalPrincipalPoint() + principalPointX);
             intrinsic2.setVerticalPrincipalPoint(intrinsic2.getVerticalPrincipalPoint() + principalPointY);
 
             // fix fundamental matrix to account for principal point different
             // from zero
             fixFundamentalMatrix(fundamentalMatrix, intrinsicZeroPrincipalPoint1, intrinsicZeroPrincipalPoint2,
                     intrinsic1, intrinsic2);
 
             return estimateInitialCamerasAndPointsEssential(intrinsic1, intrinsic2);
         } catch (final Exception e) {
             return false;
         }
     }
 
     /**
      * Estimates initial cameras and reconstructed points using the Dual
      * Absolute Quadric.
      *
      * @return true if cameras and points could be estimated, false if something
      * failed.
      */
     private boolean estimateInitialCamerasAndPointsDAQ() {
         try {
             final var fundamentalMatrix = currentEstimatedFundamentalMatrix.getFundamentalMatrix();
             fundamentalMatrix.normalize();
 
             final var estimator = new DualAbsoluteQuadricInitialCamerasEstimator(fundamentalMatrix);
             estimator.setAspectRatio(configuration.getInitialCamerasAspectRatio());
             estimator.estimate();
 
             final var camera1 = estimator.getEstimatedLeftCamera();
             final var camera2 = estimator.getEstimatedRightCamera();
 
             camera1.decompose();
             camera2.decompose();
 
             final var intrinsicZeroPrincipalPoint1 = camera1.getIntrinsicParameters();
             final var intrinsicZeroPrincipalPoint2 = camera2.getIntrinsicParameters();
 
             final var principalPointX = configuration.getPrincipalPointX();
             final var principalPointY = configuration.getPrincipalPointY();
 
             final var intrinsic1 = new PinholeCameraIntrinsicParameters(intrinsicZeroPrincipalPoint1);
             intrinsic1.setHorizontalPrincipalPoint(intrinsic1.getHorizontalPrincipalPoint() + principalPointX);
             intrinsic1.setVerticalPrincipalPoint(intrinsic1.getVerticalPrincipalPoint() + principalPointY);
             camera1.setIntrinsicParameters(intrinsic1);
 
             final var intrinsic2 = new PinholeCameraIntrinsicParameters(intrinsicZeroPrincipalPoint2);
             intrinsic2.setHorizontalPrincipalPoint(intrinsic2.getHorizontalPrincipalPoint() + principalPointX);
             intrinsic2.setVerticalPrincipalPoint(intrinsic2.getVerticalPrincipalPoint() + principalPointY);
             camera2.setIntrinsicParameters(intrinsic2);
 
             previousMetricEstimatedCamera = new EstimatedCamera();
             previousMetricEstimatedCamera.setCamera(camera1);
             previousMetricEstimatedCamera.setViewId(previousViewId);
 
             currentMetricEstimatedCamera = new EstimatedCamera();
             currentMetricEstimatedCamera.setCamera(camera2);
             currentMetricEstimatedCamera.setViewId(currentViewId);
 
             // fix fundamental matrix to account for principal point different
             // from zero
             fixFundamentalMatrix(fundamentalMatrix, intrinsicZeroPrincipalPoint1, intrinsicZeroPrincipalPoint2,
                     intrinsic1, intrinsic2);
 
             // triangulate points
             Corrector corrector = null;
             if (configuration.getInitialCamerasCorrectorType() != null) {
                 corrector = Corrector.create(fundamentalMatrix, configuration.getInitialCamerasCorrectorType());
             }
 
             // use all points used for fundamental matrix estimation
             final var samples1 = currentEstimatedFundamentalMatrix.getLeftSamples();
             final var samples2 = currentEstimatedFundamentalMatrix.getRightSamples();
 
             final var points1 = new ArrayList<Point2D>();
             final var points2 = new ArrayList<Point2D>();
             final var length = samples1.size();
             for (var i = 0; i < length; i++) {
                 final var sample1 = samples1.get(i);
                 final var sample2 = samples2.get(i);
 
                 final var point1 = sample1.getPoint();
                 final var point2 = sample2.getPoint();
 
                 points1.add(point1);
                 points2.add(point2);
             }
 
             // correct points if needed
             final List<Point2D> correctedPoints1;
             final List<Point2D> correctedPoints2;
             if (corrector != null) {
                 corrector.setLeftAndRightPoints(points1, points2);
                 corrector.correct();
 
                 correctedPoints1 = corrector.getLeftCorrectedPoints();
                 correctedPoints2 = corrector.getRightCorrectedPoints();
             } else {
                 correctedPoints1 = points1;
                 correctedPoints2 = points2;
             }
 
             // triangulate points
             final SinglePoint3DTriangulator triangulator;
             if (configuration.getDaqUseHomogeneousPointTriangulator()) {
                 triangulator = SinglePoint3DTriangulator.create(Point3DTriangulatorType.LMSE_HOMOGENEOUS_TRIANGULATOR);
             } else {
                 triangulator = SinglePoint3DTriangulator.create(
                         Point3DTriangulatorType.LMSE_INHOMOGENEOUS_TRIANGULATOR);
             }
 
             final var cameras = new ArrayList<PinholeCamera>();
             cameras.add(camera1);
             cameras.add(camera2);
 
             activeMetricReconstructedPoints = new ArrayList<>();
             final var points = new ArrayList<Point2D>();
             final var numPoints = correctedPoints1.size();
 
             Point3D triangulatedPoint;
             ReconstructedPoint3D reconstructedPoint;
             for (var i = 0; i < numPoints; i++) {
                 points.clear();
                 points.add(correctedPoints1.get(i));
                 points.add(correctedPoints2.get(i));
 
                 triangulator.setPointsAndCameras(points, cameras);
                 triangulatedPoint = triangulator.triangulate();
 
                 reconstructedPoint = new ReconstructedPoint3D();
                 reconstructedPoint.setPoint(triangulatedPoint);
 
                 // only points reconstructed in front of both cameras are
                 // considered valid
                 final var front1 = camera1.isPointInFrontOfCamera(triangulatedPoint);
                 final var front2 = camera2.isPointInFrontOfCamera(triangulatedPoint);
                 final var inlier = front1 && front2;
                 reconstructedPoint.setInlier(inlier);
 
                 activeMetricReconstructedPoints.add(reconstructedPoint);
 
                 if (inlier) {
                     matches.get(i).setReconstructedPoint(reconstructedPoint);
                 }
             }
 
             return true;
         } catch (final Exception e) {
             return false;
         }
     }
 
     /**
      * Estimates initial cameras and reconstructed points using Dual Image of
      * Absolute Conic.
      *
      * @return true if cameras and points could be estimated, false if something
      * failed.
      */
     private boolean estimateInitialCamerasAndPointsDIAC() {
         final var fundamentalMatrix = currentEstimatedFundamentalMatrix.getFundamentalMatrix();
 
         // use inlier points used for fundamental matrix estimation
         final var samples1 = currentEstimatedFundamentalMatrix.getLeftSamples();
         final var samples2 = currentEstimatedFundamentalMatrix.getRightSamples();
 
         final var points1 = new ArrayList<Point2D>();
         final var points2 = new ArrayList<Point2D>();
         final var length = samples1.size();
         for (var i = 0; i < length; i++) {
             final var sample1 = samples1.get(i);
             final var sample2 = samples2.get(i);
 
             final var point1 = sample1.getPoint();
             final var point2 = sample2.getPoint();
 
             points1.add(point1);
             points2.add(point2);
         }
 
         try {
             final var estimator = new DualImageOfAbsoluteConicInitialCamerasEstimator(fundamentalMatrix, points1,
                     points2);
             estimator.setPrincipalPoint(configuration.getPrincipalPointX(), configuration.getPrincipalPointY());
             estimator.setAspectRatio(configuration.getInitialCamerasAspectRatio());
             estimator.setCorrectorType(configuration.getInitialCamerasCorrectorType());
             estimator.setPointsTriangulated(true);
             estimator.setValidTriangulatedPointsMarked(configuration.getInitialCamerasMarkValidTriangulatedPoints());
 
             estimator.estimate();
 
             // store cameras
             final var camera1 = estimator.getEstimatedLeftCamera();
             final var camera2 = estimator.getEstimatedRightCamera();
 
             previousMetricEstimatedCamera = new EstimatedCamera();
             previousMetricEstimatedCamera.setCamera(camera1);
 
             currentMetricEstimatedCamera = new EstimatedCamera();
             currentMetricEstimatedCamera.setCamera(camera2);
 
             // store points
             final var triangulatedPoints = estimator.getTriangulatedPoints();
             final var validTriangulatedPoints = estimator.getValidTriangulatedPoints();
 
             activeMetricReconstructedPoints = new ArrayList<>();
             final var triangulatedPointsSize = triangulatedPoints.size();
             for (var i = 0; i < triangulatedPointsSize; i++) {
                 final var reconstructedPoint = new ReconstructedPoint3D();
                 reconstructedPoint.setPoint(triangulatedPoints.get(i));
                 reconstructedPoint.setInlier(validTriangulatedPoints.get(i));
                 activeMetricReconstructedPoints.add(reconstructedPoint);
 
                 if (validTriangulatedPoints.get(i)) {
                     matches.get(i).setReconstructedPoint(reconstructedPoint);
                 }
             }
 
             return true;
         } catch (final Exception e) {
             return false;
         }
     }
 
     /**
      * Estimates initial cameras and reconstructed points using the essential
      * matrix and provided intrinsic parameters that must have been set during
      * offline calibration.
      *
      * @return true if cameras and points could be estimated, false if something
      * failed.
      */
     private boolean estimateInitialCamerasAndPointsEssential() {
         final var intrinsic1 = configuration.getInitialIntrinsic1();
         final var intrinsic2 = configuration.getInitialIntrinsic2();
         return estimateInitialCamerasAndPointsEssential(intrinsic1, intrinsic2);
     }
 
     /**
      * Estimates initial cameras and reconstructed points using the essential
      * matrix and provided intrinsic parameters that must have been set during
      * offline calibration.
      *
      * @param intrinsic1 intrinsic parameters of 1st camera.
      * @param intrinsic2 intrinsic parameters of 2nd camera.
      * @return true if cameras and points could be estimated, false if something
      * failed.
      */
     private boolean estimateInitialCamerasAndPointsEssential(
             final PinholeCameraIntrinsicParameters intrinsic1, final PinholeCameraIntrinsicParameters intrinsic2) {
         final var fundamentalMatrix = currentEstimatedFundamentalMatrix.getFundamentalMatrix();
 
         // use all points used for fundamental matrix estimation
         final var samples1 = currentEstimatedFundamentalMatrix.getLeftSamples();
         final var samples2 = currentEstimatedFundamentalMatrix.getRightSamples();
 
         final var points1 = new ArrayList<Point2D>();
         final var points2 = new ArrayList<Point2D>();
         final var length = samples1.size();
         for (var i = 0; i < length; i++) {
             final var sample1 = samples1.get(i);
             final var sample2 = samples2.get(i);
 
             final var point1 = sample1.getPoint();
             final var point2 = sample2.getPoint();
 
             points1.add(point1);
             points2.add(point2);
         }
 
         try {
             final var estimator = new EssentialMatrixInitialCamerasEstimator(fundamentalMatrix, intrinsic1, intrinsic2,
                     points1, points2);
 
             estimator.setCorrectorType(configuration.getInitialCamerasCorrectorType());
             estimator.setPointsTriangulated(true);
             estimator.setValidTriangulatedPointsMarked(configuration.getInitialCamerasMarkValidTriangulatedPoints());
 
             estimator.estimate();
 
             // store cameras
             final var camera1 = estimator.getEstimatedLeftCamera();
             final var camera2 = estimator.getEstimatedRightCamera();
 
             previousMetricEstimatedCamera = new EstimatedCamera();
             previousMetricEstimatedCamera.setCamera(camera1);
             previousMetricEstimatedCamera.setViewId(previousViewId);
 
             currentMetricEstimatedCamera = new EstimatedCamera();
             currentMetricEstimatedCamera.setCamera(camera2);
             currentMetricEstimatedCamera.setViewId(currentViewId);
 
             // store points
             final var triangulatedPoints = estimator.getTriangulatedPoints();
             final var validTriangulatedPoints = estimator.getValidTriangulatedPoints();
 
             activeMetricReconstructedPoints = new ArrayList<>();
             final var triangulatedPointsSize = triangulatedPoints.size();
             final var matchesSize = matches.size();
             var j = 0;
             for (var i = 0; i < triangulatedPointsSize && j < matchesSize; i++, j++) {
                 if (!validTriangulatedPoints.get(i)) {
                     continue;
                 }
 
                 final var reconstructedPoint = new ReconstructedPoint3D();
                 reconstructedPoint.setPoint(triangulatedPoints.get(i));
                 reconstructedPoint.setInlier(validTriangulatedPoints.get(i));
                 activeMetricReconstructedPoints.add(reconstructedPoint);
 
                 matches.get(j).setReconstructedPoint(reconstructedPoint);
             }
 
             return true;
         } catch (final Exception e) {
             return false;
         }
     }
 
     /**
      * Fixes fundamental matrix to account for principal point different from
      * zero when using DAQ estimation.
      *
      * @param fundamentalMatrix            fundamental matrix to be fixed.
      * @param intrinsicZeroPrincipalPoint1 intrinsic parameters of camera 1
      *                                     assuming zero principal point.
      * @param intrinsicZeroPrincipalPoint2 intrinsic parameters of camera 2
      *                                     assuming zero principal point.
      * @param intrinsicPrincipalPoint1     intrinsic parameters of camera 1 using
      *                                     proper principal point.
      * @param intrinsicPrincipalPoint2     intrinsic parameters of camera 2 using
      *                                     proper principal point.
      * @throws EpipolarException if something fails.
      * @throws NotReadyException never happens.
      */
     private void fixFundamentalMatrix(
             final FundamentalMatrix fundamentalMatrix,
             final PinholeCameraIntrinsicParameters intrinsicZeroPrincipalPoint1,
             final PinholeCameraIntrinsicParameters intrinsicZeroPrincipalPoint2,
             final PinholeCameraIntrinsicParameters intrinsicPrincipalPoint1,
             final PinholeCameraIntrinsicParameters intrinsicPrincipalPoint2)
             throws EpipolarException, NotReadyException {
 
         // first compute essential matrix as E = K2a'F*K1a
         final var essential = new EssentialMatrix(fundamentalMatrix, intrinsicZeroPrincipalPoint1,
                 intrinsicZeroPrincipalPoint2);
         final var fixedFundamentalMatrix = essential.toFundamentalMatrix(intrinsicPrincipalPoint1,
                 intrinsicPrincipalPoint2);
         fixedFundamentalMatrix.normalize();
         currentEstimatedFundamentalMatrix.setFundamentalMatrix(fixedFundamentalMatrix);
         currentEstimatedFundamentalMatrix.setCovariance(null);
     }
 }