/*
    * 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.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.LMedSPointCorrespondenceProjectiveTransformation2DRobustEstimator;
  import com.irurueta.geometry.estimators.MSACPointCorrespondenceProjectiveTransformation2DRobustEstimator;
  import com.irurueta.geometry.estimators.NotReadyException;
  import com.irurueta.geometry.estimators.PROMedSPointCorrespondenceProjectiveTransformation2DRobustEstimator;
  import com.irurueta.geometry.estimators.PROSACPointCorrespondenceProjectiveTransformation2DRobustEstimator;
  import com.irurueta.geometry.estimators.PointCorrespondenceProjectiveTransformation2DRobustEstimator;
  import com.irurueta.geometry.estimators.ProjectiveTransformation2DRobustEstimator;
  import com.irurueta.geometry.estimators.RANSACPointCorrespondenceProjectiveTransformation2DRobustEstimator;
  
  import java.util.ArrayList;
  import java.util.List;
  
  /**
   * Base class in charge of estimating cameras and 3D reconstructed points from
   * sparse image point correspondences in two views.
   *
   * @param <C> type of configuration.
   * @param <R> type of re-constructor.
   * @param <L> type of listener.
   */
  @SuppressWarnings("DuplicatedCode")
  public abstract class BaseTwoViewsSparseReconstructor<
          C extends BaseTwoViewsSparseReconstructorConfiguration<C>,
          R extends BaseTwoViewsSparseReconstructor<C, R, L>,
          L extends BaseTwoViewsSparseReconstructorListener<R>> {
  
      /**
       * Number of views.
       */
      public static final int NUMBER_OF_VIEWS = 2;
  
      /**
       * Estimated fundamental matrix.
       */
      protected EstimatedFundamentalMatrix estimatedFundamentalMatrix;
  
      /**
       * Estimated first camera.
       */
      protected EstimatedCamera estimatedCamera1;
  
      /**
       * Estimated second camera.
       */
      protected EstimatedCamera estimatedCamera2;
  
      /**
       * Reconstructed 3D points.
       */
      protected List<ReconstructedPoint3D> reconstructedPoints;
  
      /**
       * 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;
 
     /**
      * 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;
 
     /**
      * Samples on first view.
      */
     private List<Sample2D> firstViewSamples = null;
 
     /**
      * Samples on last processed view (i.e. current view).
      */
     private List<Sample2D> currentViewSamples;
 
     /**
      * Matches between first and current view.
      */
     private final List<MatchedSamples> matches = new ArrayList<>();
 
     /**
      * ID of first view.
      */
     private int firstViewId = 0;
 
     /**
      * 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 BaseTwoViewsSparseReconstructor(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 BaseTwoViewsSparseReconstructorListener<R> 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.
      *
      * @return estimated fundamental matrix.
      */
     public EstimatedFundamentalMatrix getEstimatedFundamentalMatrix() {
         return estimatedFundamentalMatrix;
     }
 
     /**
      * Gets estimated first camera.
      *
      * @return estimated first camera.
      */
     public EstimatedCamera getEstimatedCamera1() {
         return estimatedCamera1;
     }
 
     /**
      * Gets estimated second camera.
      *
      * @return estimated second camera.
      */
     public EstimatedCamera getEstimatedCamera2() {
         return estimatedCamera2;
     }
 
     /**
      * Gets reconstructed 3D points.
      *
      * @return reconstructed 3D points.
      */
     public List<ReconstructedPoint3D> getReconstructedPoints() {
         return reconstructedPoints;
     }
 
     /**
      * 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.
      *
      * @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)) {
             return false;
         }
 
         estimatedFundamentalMatrix = null;
         currentViewSamples = new ArrayList<>();
         //noinspection unchecked
         listener.onRequestSamplesForCurrentView((R) this, viewCount, currentViewSamples);
 
         if (firstViewSamples == null) {
             // for first view we simply keep samples (if enough are provided)
             if (hasEnoughSamples(currentViewSamples)) {
                 //noinspection unchecked
                 listener.onSamplesAccepted((R) this, viewCount, currentViewSamples);
                 firstViewSamples = currentViewSamples;
                 firstViewId = viewCount;
             }
 
         } else {
 
             // for second view, check that we have enough samples
             if (hasEnoughSamples(currentViewSamples)) {
 
                 // find matches
                 matches.clear();
                 //noinspection unchecked
                 listener.onRequestMatches((R) this, firstViewSamples, currentViewSamples, firstViewId, viewCount,
                         matches);
 
                 if (hasEnoughMatches(matches)) {
                     // if enough matches are retrieved, attempt to compute
                     // fundamental matrix
                     if ((configuration.isGeneralSceneAllowed()
                             && estimateFundamentalMatrix(matches, firstViewId, viewCount))
                             || (configuration.isPlanarSceneAllowed()
                             && estimatePlanarFundamentalMatrix(matches, firstViewId, viewCount))) {
                         // fundamental matrix could be estimated
                         //noinspection unchecked
                         listener.onSamplesAccepted((R) this, viewCount, currentViewSamples);
                         var secondViewId = viewCount;
 
                         //noinspection unchecked
                         listener.onFundamentalMatrixEstimated((R) this, estimatedFundamentalMatrix);
 
                         if (estimateInitialCamerasAndPoints()) {
                             // cameras and points have been estimated
                             //noinspection unchecked
                             listener.onCamerasEstimated((R) this, firstViewId, secondViewId, estimatedCamera1,
                                     estimatedCamera2);
                             //noinspection unchecked
                             listener.onReconstructedPointsEstimated((R) this, matches, reconstructedPoints);
                             if (postProcessOne()) {
                                 //noinspection unchecked
                                 listener.onFinish((R) this);
                                 running = false;
                                 finished = true;
                             }
                         } else {
                             // initial cameras failed
                             failed = true;
                             //noinspection unchecked
                             listener.onFail((R) this);
                         }
                     } else {
                         // estimation of fundamental matrix failed
                         //noinspection unchecked
                         listener.onSamplesRejected((R) this, viewCount, currentViewSamples);
                     }
                 }
             }
         }
 
         viewCount++;
 
         if (cancelled) {
             //noinspection unchecked
             listener.onCancel((R) this);
         }
 
         return !finished;
     }
 
     /**
      * 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.
      */
     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() {
         firstViewSamples = currentViewSamples = null;
 
         cancelled = failed = false;
         viewCount = 0;
         running = false;
 
         estimatedFundamentalMatrix = null;
         estimatedCamera1 = estimatedCamera2 = null;
         reconstructedPoints = null;
 
         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.
      *
      * @return true if post-processing succeeded, false otherwise.
      */
     protected abstract boolean postProcessOne();
 
     /**
      * 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 hasEnoughSamples(final List<Sample2D> samples) {
         return hasEnoughSamplesOrMatches(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 hasEnoughMatches(final List<MatchedSamples> matches) {
         return hasEnoughSamplesOrMatches(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 hasEnoughSamplesOrMatches(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.
      * @param viewId2 id of second view.
      * @return true if estimation succeeded, false otherwise.
      */
     private boolean estimateFundamentalMatrix(final List<MatchedSamples> matches, final int viewId1,
                                               final int viewId2) {
         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 (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;
         }
 
         var i = 0;
         for (final var match : matches) {
             final var samples = match.getSamples();
             if (samples.length != NUMBER_OF_VIEWS) {
                 return false;
             }
 
             leftSamples.add(samples[0]);
             rightSamples.add(samples[1]);
 
             final var leftPoint = Point2D.create();
             leftPoint.setInhomogeneousCoordinates(
                     samples[0].getPoint().getInhomX() - principalPointX,
                     samples[0].getPoint().getInhomY() - principalPointY);
             leftPoints.add(leftPoint);
 
             final var rightPoint = Point2D.create();
             rightPoint.setInhomogeneousCoordinates(
                     samples[1].getPoint().getInhomX() - principalPointX,
                     samples[1].getPoint().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();
 
             estimatedFundamentalMatrix = new EstimatedFundamentalMatrix();
             estimatedFundamentalMatrix.setFundamentalMatrix(fundamentalMatrix);
             estimatedFundamentalMatrix.setViewId1(viewId1);
             estimatedFundamentalMatrix.setViewId2(viewId2);
             estimatedFundamentalMatrix.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;
                     }
                 }
                 estimatedFundamentalMatrix.setQualityScore(fundamentalMatrixQualityScore);
                 estimatedFundamentalMatrix.setInliers(inliers);
             }
 
             // store left/right samples
             estimatedFundamentalMatrix.setLeftSamples(leftSamples);
             estimatedFundamentalMatrix.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.
      * @param viewId2 id of second view.
      * @return true if estimation succeeded, false otherwise.
      */
     private boolean estimatePlanarFundamentalMatrix(
             final List<MatchedSamples> matches, final int viewId1, final int viewId2) {
         if (matches == null) {
             return false;
         }
 
         final var count = matches.size();
         final var leftSamples = new ArrayList<Sample2D>();
         final var rightSamples = new ArrayList<Sample2D>();
         final var leftPoints = new ArrayList<Point2D>();
         final var rightPoints = new ArrayList<Point2D>();
         final var qualityScores = new double[count];
         final double principalPointX;
         final double principalPointY;
         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;
         }
 
         var i = 0;
         for (final var match : matches) {
             final var samples = match.getSamples();
             if (samples.length != NUMBER_OF_VIEWS) {
                 return false;
             }
 
             leftSamples.add(samples[0]);
             rightSamples.add(samples[1]);
 
             final var leftPoint = Point2D.create();
             leftPoint.setInhomogeneousCoordinates(
                     samples[0].getPoint().getInhomX() - principalPointX,
                     samples[0].getPoint().getInhomY() - principalPointY);
             leftPoints.add(leftPoint);
 
             final var rightPoint = Point2D.create();
             rightPoint.setInhomogeneousCoordinates(
                     samples[1].getPoint().getInhomX() - principalPointX,
                     samples[1].getPoint().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:
                 default:
                     final var ransacHomographyEstimator =
                             (RANSACPointCorrespondenceProjectiveTransformation2DRobustEstimator) homographyEstimator;
 
                     ransacHomographyEstimator.setThreshold(configuration.getPlanarHomographyThreshold());
                     ransacHomographyEstimator.setComputeAndKeepInliersEnabled(
                             configuration.getPlanarHomographyComputeAndKeepInliers());
                     ransacHomographyEstimator.setComputeAndKeepResidualsEnabled(
                             configuration.getPlanarHomographyComputeAndKeepResiduals());
                     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();
 
             estimatedFundamentalMatrix = new EstimatedFundamentalMatrix();
             estimatedFundamentalMatrix.setFundamentalMatrix(fundamentalMatrix);
             estimatedFundamentalMatrix.setViewId1(viewId1);
             estimatedFundamentalMatrix.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;
                     }
                 }
                 estimatedFundamentalMatrix.setQualityScore(fundamentalMatrixQualityScore);
                 estimatedFundamentalMatrix.setInliers(inliers);
             }
 
             // store left/right samples
             estimatedFundamentalMatrix.setLeftSamples(leftSamples);
             estimatedFundamentalMatrix.setRightSamples(rightSamples);
 
             return true;
         } catch (final Exception e) {
             return false;
         }
     }
 
     /**
      * 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 = estimatedFundamentalMatrix.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 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);
 
             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 = estimatedFundamentalMatrix.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);
 
             estimatedCamera1 = new EstimatedCamera();
             estimatedCamera1.setCamera(camera1);
 
             estimatedCamera2 = new EstimatedCamera();
             estimatedCamera2.setCamera(camera2);
 
             // 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 = estimatedFundamentalMatrix.getLeftSamples();
             final var samples2 = estimatedFundamentalMatrix.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
             List<Point2D> correctedPoints1;
             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);
 
             reconstructedPoints = 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);
                 reconstructedPoint.setInlier(front1 && front2);
 
                 reconstructedPoints.add(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 = estimatedFundamentalMatrix.getFundamentalMatrix();
 
         // use inlier points used for fundamental matrix estimation
         final var samples1 = estimatedFundamentalMatrix.getLeftSamples();
         final var samples2 = estimatedFundamentalMatrix.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();
 
             estimatedCamera1 = new EstimatedCamera();
             estimatedCamera1.setCamera(camera1);
 
             estimatedCamera2 = new EstimatedCamera();
             estimatedCamera2.setCamera(camera2);
 
             // store points
             final var triangulatedPoints = estimator.getTriangulatedPoints();
             final var validTriangulatedPoints = estimator.getValidTriangulatedPoints();
 
             reconstructedPoints = new ArrayList<>();
             final var size = triangulatedPoints.size();
             for (var i = 0; i < size; i++) {
                 final var reconstructedPoint = new ReconstructedPoint3D();
                 reconstructedPoint.setPoint(triangulatedPoints.get(i));
                 reconstructedPoint.setInlier(validTriangulatedPoints.get(i));
                 reconstructedPoints.add(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 = estimatedFundamentalMatrix.getFundamentalMatrix();
 
         // use all points used for fundamental matrix estimation
         final var samples1 = estimatedFundamentalMatrix.getLeftSamples();
         final var samples2 = estimatedFundamentalMatrix.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();
 
             estimatedCamera1 = new EstimatedCamera();
             estimatedCamera1.setCamera(camera1);
 
             estimatedCamera2 = new EstimatedCamera();
             estimatedCamera2.setCamera(camera2);
 
             // store points
             final var triangulatedPoints = estimator.getTriangulatedPoints();
             final var validTriangulatedPoints = estimator.getValidTriangulatedPoints();
 
             reconstructedPoints = new ArrayList<>();
             final var size = triangulatedPoints.size();
             for (var i = 0; i < size; i++) {
                 final var reconstructedPoint = new ReconstructedPoint3D();
                 reconstructedPoint.setPoint(triangulatedPoints.get(i));
                 reconstructedPoint.setInlier(validTriangulatedPoints.get(i));
                 reconstructedPoints.add(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();
         estimatedFundamentalMatrix.setFundamentalMatrix(fixedFundamentalMatrix);
         estimatedFundamentalMatrix.setCovariance(null);
     }
 }