/*
    * 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.KruppaDualImageOfAbsoluteConicEstimator;
  import com.irurueta.ar.epipolar.Corrector;
  import com.irurueta.ar.epipolar.CorrectorType;
  import com.irurueta.ar.epipolar.FundamentalMatrix;
  import com.irurueta.geometry.PinholeCamera;
  import com.irurueta.geometry.Point2D;
  import com.irurueta.geometry.Point3D;
  import com.irurueta.geometry.estimators.LockedException;
  import com.irurueta.geometry.estimators.NotReadyException;
  
  import java.util.ArrayList;
  import java.util.BitSet;
  import java.util.List;
  
  /**
   * Estimates an initial pair of cameras in the metric stratum (up to an
   * arbitrary scale) using a given fundamental matrix to obtain the Dual Image
   * of Absolute Conic by solving Kruppa equations to obtain the Essential matrix,
   * so that once it is computed it can be used to determine best pair of camera
   * poses and translations by triangulating a set of matched points and checking
   * that their triangulation lies in front of cameras.
   */
  public class DualImageOfAbsoluteConicInitialCamerasEstimator extends InitialCamerasEstimator {
  
      /**
       * Indicates whether matched 2D points must be triangulated by default.
       */
      public static final boolean DEFAULT_TRIANGULATE_POINTS = false;
  
      /**
       * Indicates whether triangulated points must be marked as valid (i.e. when
       * they lie in front of both of the estimated cameras) or not.
       */
      public static final boolean DEFAULT_MARK_VALID_TRIANGULATED_POINTS = false;
  
      /**
       * Aspect ratio of intrinsic parameters of cameras.
       * Typically, this value is 1.0 if vertical coordinates increase upwards,
       * or -1.0 if it is the opposite.
       */
      private double aspectRatio = KruppaDualImageOfAbsoluteConicEstimator.DEFAULT_FOCAL_DISTANCE_ASPECT_RATIO;
  
      /**
       * Horizontal coordinate of principal point. This value should be the
       * coordinates of the center of an image assuming that the coordinates start
       * on the top-left or bottom-left corner. Using a value close to zero
       * will produce inaccurate results.
       */
      private double principalPointX;
  
      /**
       * Vertical coordinate of principal point. This value should be the
       * coordinates of the center of an image assuming that the coordinates start
       * on the top-left or bottom-left corner. Using a value close to zero will
       * produce inaccurate results.
       */
      private double principalPointY;
  
      /**
       * Matched 2D points on left view.
       */
      private List<Point2D> leftPoints;
  
      /**
       * Matched 2D points on right view.
       */
      private List<Point2D> rightPoints;
  
      /**
       * Type of corrector to use to triangulate matched points or null if no
       * corrector needs to be used.
       */
      private CorrectorType correctorType = Corrector.DEFAULT_TYPE;
  
      /**
       * Indicates whether matched 2D points need to be triangulated.
       */
      private boolean triangulatePoints = DEFAULT_TRIANGULATE_POINTS;
  
      /**
       * Marks which of the triangulated points are marked as valid (lie in front
       * of both of the estimated cameras) and which ones aren't.
      */
     private boolean markValidTriangulatedPoints = DEFAULT_MARK_VALID_TRIANGULATED_POINTS;
 
     /**
      * Contains triangulated points.
      */
     private List<Point3D> triangulatedPoints;
 
     /**
      * Contains booleans indicating whether triangulated points are valid (i.e.
      * lie in front of both estimated cameras) or not.
      */
     private BitSet validTriangulatedPoints;
 
     /**
      * Constructor.
      */
     public DualImageOfAbsoluteConicInitialCamerasEstimator() {
         super();
     }
 
     /**
      * Constructor.
      *
      * @param fundamentalMatrix fundamental matrix relating two views.
      */
     public DualImageOfAbsoluteConicInitialCamerasEstimator(final FundamentalMatrix fundamentalMatrix) {
         super(fundamentalMatrix);
     }
 
     /**
      * Constructor.
      *
      * @param leftPoints  matched 2D points on left view.
      * @param rightPoints matched 2D points on right view.
      * @throws IllegalArgumentException if provided lists don't have the same
      *                                  size.
      */
     public DualImageOfAbsoluteConicInitialCamerasEstimator(
             final List<Point2D> leftPoints, final List<Point2D> rightPoints) {
         super();
         internalSetLeftAndRightPoints(leftPoints, rightPoints);
     }
 
     /**
      * Constructor.
      *
      * @param fundamentalMatrix fundamental matrix relating two views.
      * @param leftPoints        matched 2D points on left view.
      * @param rightPoints       matched 2D points on right view.
      * @throws IllegalArgumentException if provided lists don't have the same
      *                                  size.
      */
     public DualImageOfAbsoluteConicInitialCamerasEstimator(
             final FundamentalMatrix fundamentalMatrix, final List<Point2D> leftPoints,
             final List<Point2D> rightPoints) {
         super(fundamentalMatrix);
         internalSetLeftAndRightPoints(leftPoints, rightPoints);
     }
 
     /**
      * Constructor.
      *
      * @param listener listener to handle events raised by this instance.
      */
     public DualImageOfAbsoluteConicInitialCamerasEstimator(final InitialCamerasEstimatorListener listener) {
         super(listener);
     }
 
     /**
      * Constructor.
      *
      * @param fundamentalMatrix fundamental matrix relating two views.
      * @param listener          listener to handle events raised by this instance.
      */
     public DualImageOfAbsoluteConicInitialCamerasEstimator(
             final FundamentalMatrix fundamentalMatrix, final InitialCamerasEstimatorListener listener) {
         super(fundamentalMatrix, listener);
     }
 
     /**
      * Constructor.
      *
      * @param leftPoints  matched 2D points on left view.
      * @param rightPoints matched 2D points on right view.
      * @param listener    listener to handle events raised by this instance.
      * @throws IllegalArgumentException if provided lists don't have the same
      *                                  size.
      */
     public DualImageOfAbsoluteConicInitialCamerasEstimator(
             final List<Point2D> leftPoints, final List<Point2D> rightPoints,
             final InitialCamerasEstimatorListener listener) {
         super(listener);
         internalSetLeftAndRightPoints(leftPoints, rightPoints);
     }
 
     /**
      * Constructor.
      *
      * @param fundamentalMatrix fundamental matrix relating two views.
      * @param leftPoints        matched 2D points on left view.
      * @param rightPoints       matched 2D points on right view.
      * @param listener          listener to handle events raised by this instance.
      * @throws IllegalArgumentException if provided lists don't have the same
      *                                  size.
      */
     public DualImageOfAbsoluteConicInitialCamerasEstimator(
             final FundamentalMatrix fundamentalMatrix,
             final List<Point2D> leftPoints,
             final List<Point2D> rightPoints,
             final InitialCamerasEstimatorListener listener) {
         super(fundamentalMatrix, listener);
         internalSetLeftAndRightPoints(leftPoints, rightPoints);
     }
 
     /**
      * Returns method used by this estimator.
      *
      * @return method used by this estimator.
      */
     @Override
     public InitialCamerasEstimatorMethod getMethod() {
         return InitialCamerasEstimatorMethod.DUAL_IMAGE_OF_ABSOLUTE_CONIC;
     }
 
     /**
      * Indicates if estimator is ready.
      *
      * @return true if estimator is ready, false otherwise.
      */
     @Override
     public boolean isReady() {
         return fundamentalMatrix != null && leftPoints != null && rightPoints != null
                 && leftPoints.size() == rightPoints.size();
     }
 
     /**
      * Estimates cameras.
      *
      * @throws LockedException                         if estimator is locked.
      * @throws NotReadyException                       if estimator is not ready.
      * @throws InitialCamerasEstimationFailedException if estimation of cameras
      *                                                 fails for some reason, typically due to numerical
      *                                                 instabilities.
      */
     @SuppressWarnings("DuplicatedCode")
     @Override
     public void estimate() throws LockedException, NotReadyException, InitialCamerasEstimationFailedException {
         if (isLocked()) {
             throw new LockedException();
         }
 
         if (!isReady()) {
             throw new NotReadyException();
         }
 
         try {
             locked = true;
 
             if (listener != null) {
                 listener.onStart(this);
             }
 
             if (triangulatePoints) {
                 triangulatedPoints = new ArrayList<>();
             } else {
                 triangulatedPoints = null;
             }
 
             final var nPoints = leftPoints.size();
             if (markValidTriangulatedPoints) {
                 validTriangulatedPoints = new BitSet(nPoints);
             } else {
                 validTriangulatedPoints = null;
             }
 
             if (estimatedLeftCamera == null) {
                 estimatedLeftCamera = new PinholeCamera();
             }
             if (estimatedRightCamera == null) {
                 estimatedRightCamera = new PinholeCamera();
             }
 
             generateInitialMetricCamerasUsingDIAC(fundamentalMatrix, principalPointX, principalPointY, aspectRatio,
                     leftPoints, rightPoints, correctorType, estimatedLeftCamera, estimatedRightCamera,
                     triangulatedPoints, validTriangulatedPoints);
 
             if (listener != null) {
                 listener.onFinish(this, estimatedLeftCamera, estimatedRightCamera);
             }
         } catch (final InitialCamerasEstimationFailedException e) {
             if (listener != null) {
                 listener.onFail(this, e);
             }
             throw e;
         } finally {
             locked = false;
         }
     }
 
     /**
      * Gets aspect ratio of intrinsic parameters of cameras.
      * Typically, this value is 1.0 if vertical coordinates increase upwards,
      * or -1.0 if it is the opposite.
      *
      * @return aspect ratio of intrinsic parameters of cameras.
      */
     public double getAspectRatio() {
         return aspectRatio;
     }
 
     /**
      * Sets aspect ratio of intrinsic parameters of cameras.
      * Typically, this value is 1.0 if vertical coordinates increase upwards,
      * or -1.0 if it is the opposite.
      *
      * @param aspectRatio aspect ratio of intrinsic parameters of cameras.
      * @throws LockedException if estimator is locked.
      */
     public void setAspectRatio(final double aspectRatio) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         this.aspectRatio = aspectRatio;
     }
 
     /**
      * Gets horizontal coordinate of principal point. This value should be the
      * coordinates of the center of an image assuming that the coordinates start
      * on the top-left or bottom-left corner. Using a value close to zero
      * will produce inaccurate results.
      *
      * @return horizontal coordinate of principal point.
      */
     public double getPrincipalPointX() {
         return principalPointX;
     }
 
     /**
      * Sets horizontal coordinate of principal point. This value should be the
      * coordinates of the center of an image assuming that the coordinates start
      * on the top-left or bottom-left corner. Using a value close to zero
      * will produce inaccurate results.
      *
      * @param principalPointX horizontal coordinate of principal point.
      * @throws LockedException if estimator is locked.
      */
     public void setPrincipalPointX(final double principalPointX) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         this.principalPointX = principalPointX;
     }
 
     /**
      * Gets vertical coordinate of principal point. This value should be the
      * coordinates of the center of an image assuming that the coordinates start
      * on the top-left or bottom-left corner. Using a value close to zero will
      * produce inaccurate results.
      *
      * @return vertical coordinate of principal point.
      */
     public double getPrincipalPointY() {
         return principalPointY;
     }
 
     /**
      * Sets vertical coordinate of principal point. This value should be the
      * coordinates of the center of an image assuming that the coordinates start
      * on the top-left or bottom-left corner. Using a value close to zero will
      * produce inaccurate results.
      *
      * @param principalPointY vertical coordinate of principal point.
      * @throws LockedException if estimator is locked.
      */
     public void setPrincipalPointY(final double principalPointY) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         this.principalPointY = principalPointY;
     }
 
     /**
      * Sets horizontal and vertical coordinates of principal point. This value
      * should be the coordinates of the center of an image assuming that the
      * coordinates start on the top-left or bottom-left corner. Using a value
      * close to zero will produce inaccurate results.
      *
      * @param principalPointX horizontal coordinate of principal point.
      * @param principalPointY vertical coordinate of principal point.
      * @throws LockedException if estimator is locked.
      */
     public void setPrincipalPoint(final double principalPointX, final double principalPointY) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         this.principalPointX = principalPointX;
         this.principalPointY = principalPointY;
     }
 
     /**
      * Gets matched 2D points on left view.
      *
      * @return matched 2D points on left view.
      */
     public List<Point2D> getLeftPoints() {
         return leftPoints;
     }
 
     /**
      * Sets matched 2D points on left view.
      *
      * @param leftPoints matched 2D points on left view.
      * @throws LockedException if estimator is locked.
      */
     public void setLeftPoints(final List<Point2D> leftPoints) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         this.leftPoints = leftPoints;
     }
 
     /**
      * Gets matched 2D points on right view.
      *
      * @return matched 2D points on right view.
      */
     public List<Point2D> getRightPoints() {
         return rightPoints;
     }
 
     /**
      * Sets matched 2D points on right view.
      *
      * @param rightPoints matched 2D points on right view.
      * @throws LockedException if estimator is locked.
      */
     public void setRightPoints(final List<Point2D> rightPoints) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         this.rightPoints = rightPoints;
     }
 
     /**
      * Sets matched 2D points on left and right views.
      *
      * @param leftPoints  matched 2D points on left view.
      * @param rightPoints matched 2D points on right view.
      * @throws LockedException          if estimator is locked.
      * @throws IllegalArgumentException if provided lists don't have the same
      *                                  size.
      */
     public void setLeftAndRightPoints(final List<Point2D> leftPoints, final List<Point2D> rightPoints)
             throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         internalSetLeftAndRightPoints(leftPoints, rightPoints);
     }
 
     /**
      * Gets type of corrector to use to triangulate matched points or null if
      * no corrector needs to be used.
      *
      * @return type of corrector to use.
      */
     public CorrectorType getCorrectorType() {
         return correctorType;
     }
 
     /**
      * Sets type of corrector to use to triangulate matched points or null if
      * no corrector needs to be used.
      *
      * @param correctorType type of corrector to use.
      * @throws LockedException if estimator is locked.
      */
     public void setCorrectorType(final CorrectorType correctorType) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         this.correctorType = correctorType;
     }
 
     /**
      * Indicates whether matched 2D points need to be triangulated or not.
      *
      * @return true if 2D points need to be triangulated, false otherwise.
      */
     public boolean arePointsTriangulated() {
         return triangulatePoints;
     }
 
     /**
      * Specifies whether matched 2D points need to be triangulated or not.
      *
      * @param triangulatePoints true if 2D points need to be triangulated, false
      *                          otherwise.
      * @throws LockedException if estimator is locked.
      */
     public void setPointsTriangulated(final boolean triangulatePoints) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         this.triangulatePoints = triangulatePoints;
     }
 
     /**
      * Indicates which triangulated points are marked as valid (lie in front
      * of both of the estimated cameras) and which ones aren't.
      *
      * @return true to mark valid and invalid triangulated points, false
      * otherwise.
      */
     public boolean areValidTriangulatedPointsMarked() {
         return markValidTriangulatedPoints;
     }
 
     /**
      * Specifies whether triangulated points are marked as valid (lie in front
      * of both of the estimated cameras) and which ones aren't.
      *
      * @param markValidTriangulatedPoints true to mark valid and invalid
      *                                    triangulated points, false otherwise.
      * @throws LockedException if estimator is locked.
      */
     public void setValidTriangulatedPointsMarked(final boolean markValidTriangulatedPoints) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         this.markValidTriangulatedPoints = markValidTriangulatedPoints;
     }
 
     /**
      * Gets triangulated points, if available.
      *
      * @return triangulated points or null.
      */
     public List<Point3D> getTriangulatedPoints() {
         return triangulatedPoints;
     }
 
     /**
      * Gets bitset indicating which of the triangulated points are valid and
      * which ones aren't.
      *
      * @return bitset indicating validity of triangulated points or null if not
      * available.
      */
     public BitSet getValidTriangulatedPoints() {
         return validTriangulatedPoints;
     }
 
     /**
      * Generates a pair of metric cameras (up to an arbitrary space) by
      * estimating the intrinsic parameters of the views by solving the Kruppa
      * equations to obtain the Dual Image of Absolute Conic (DIAC).
      * The estimated intrinsic parameters can later be used to find the
      * essential matrix (assuming that both views have the same intrinsic
      * parameters), and the essential matrix along with provided matched 2D
      * points can be used to determine the best pair of camera pose and
      * translation that yields the largest number of triangulated points laying
      * in front of both of the estimated cameras.
      * This method uses default corrector type, does not keep triangulated
      * points or valid triangulated points, and uses default aspect ratio (1.0).
      *
      * @param fundamentalMatrix fundamental matrix relating both left and right
      *                          views.
      * @param principalPointX   horizontal coordinate of principal point. This
      *                          value should be the coordinates of the center of an image assuming that
      *                          the coordinates start on the top-left or bottom-left corner. Using a
      *                          value close to zero will produce inaccurate results.
      * @param principalPointY   vertical coordinate of principal point. This
      *                          value should be the coordinates of the center of an image assuming that
      *                          the coordinates start on the top-left or bottom-left corner. Using a
      *                          value close to zero will produce inaccurate results.
      * @param leftPoints        points on left view matched with points on right view,
      *                          so they can be triangulated using estimated cameras. Both lists of points
      *                          must have the same size.
      * @param rightPoints       points on right view matched with points on left view,
      *                          so they can be triangulated using estimated cameras. Both lists of points
      *                          must have the same size.
      * @param leftCamera        instance where estimated left camera will be stored.
      * @param rightCamera       instance where estimated right camera will be stored.
      * @return number of valid triangulated points which lie in front of the two
      * estimated cameras.
      * @throws InitialCamerasEstimationFailedException if estimation of cameras
      *                                                 fails for some reason, typically due to numerical
      *                                                 instabilities.
      * @throws IllegalArgumentException                if provided lists of left and right
      *                                                 points don't have the same size.
      */
     public static int generateInitialMetricCamerasUsingDIAC(
             final FundamentalMatrix fundamentalMatrix, final double principalPointX,
             final double principalPointY, final List<Point2D> leftPoints,
             final List<Point2D> rightPoints, final PinholeCamera leftCamera,
             final PinholeCamera rightCamera) throws InitialCamerasEstimationFailedException {
 
         return generateInitialMetricCamerasUsingDIAC(fundamentalMatrix, principalPointX, principalPointY,
                 leftPoints, rightPoints, Corrector.DEFAULT_TYPE, leftCamera, rightCamera);
     }
 
     /**
      * Generates a pair of metric cameras (up to an arbitrary space) by
      * estimating the intrinsic parameters of the views by solving the Kruppa
      * equations to obtain the Dual Image of Absolute Conic (DIAC).
      * The estimated intrinsic parameters can later be used to find the
      * essential matrix (assuming that both views have the same intrinsic
      * parameters), and the essential matrix along with provided matched 2D
      * points can be used to determine the best pair of camera pose and
      * translation that yields the largest number of triangulated points laying
      * in front of both of the estimated cameras.
      * This method does not keep triangulated points or valid triangulated
      * points and uses default aspect ratio (1.0).
      *
      * @param fundamentalMatrix fundamental matrix relating both left and right
      *                          views.
      * @param principalPointX   horizontal coordinate of principal point. This
      *                          value should be the coordinates of the center of an image assuming that
      *                          the coordinates start on the top-left or bottom-left corner. Using a
      *                          value close to zero will produce inaccurate results.
      * @param principalPointY   vertical coordinate of principal point. This
      *                          value should be the coordinates of the center of an image assuming that
      *                          the coordinates start on the top-left or bottom-left corner. Using a
      *                          value close to zero will produce inaccurate results.
      * @param leftPoints        points on left view matched with points on right view,
      *                          so they can be triangulated using estimated cameras. Both lists of points
      *                          must have the same size.
      * @param rightPoints       points on right view matched with points on left view,
      *                          so they can be triangulated using estimated cameras. Both lists of points
      *                          must have the same size.
      * @param correctorType     corrector type to be used to correct 2D points, so
      *                          they follow the epipolar geometry defined by provided fundamental matrix
      *                          so that error on triangulated points is reduced. If null, no corrector
      *                          will be used.
      * @param leftCamera        instance where estimated left camera will be stored.
      * @param rightCamera       instance where estimated right camera will be stored.
      * @return number of valid triangulated points which lie in front of the two
      * estimated cameras.
      * @throws InitialCamerasEstimationFailedException if estimation of
      *                                                 cameras fails for some reason, typically due to
      *                                                 numerical instabilities.
      * @throws IllegalArgumentException                if provided lists of left and right
      *                                                 points don't have the same size.
      */
     public static int generateInitialMetricCamerasUsingDIAC(
             final FundamentalMatrix fundamentalMatrix, final double principalPointX,
             final double principalPointY, final List<Point2D> leftPoints,
             final List<Point2D> rightPoints, final CorrectorType correctorType,
             final PinholeCamera leftCamera, final PinholeCamera rightCamera)
             throws InitialCamerasEstimationFailedException {
 
         return generateInitialMetricCamerasUsingDIAC(fundamentalMatrix, principalPointX, principalPointY,
                 leftPoints, rightPoints, correctorType, leftCamera, rightCamera, null,
                 null);
     }
 
     /**
      * Generates a pair of metric cameras (up to an arbitrary space) by
      * estimating the intrinsic parameters of the views by solving the Kruppa
      * equations to obtain the Dual Image of Absolute Conic (DIAC).
      * The estimated intrinsic parameters can later be used to find the
      * essential matrix (assuming that both views have the same intrinsic
      * parameters), and the essential matrix along with provided matched 2D
      * points can be used to determine the best pair of camera pose and
      * translation that yields the largest number of triangulated points laying
      * in front of both of the estimated cameras.
      * This method uses default corrector type and default aspect ratio (1.0).
      *
      * @param fundamentalMatrix       fundamental matrix relating both left and right
      *                                views.
      * @param principalPointX         horizontal coordinate of principal point. This
      *                                value should be the coordinates of the center of an image assuming
      *                                that the coordinates start on the top-left or bottom-left corner.
      *                                Using a value close to zero will produce inaccurate results.
      * @param principalPointY         vertical coordinate of principal point. This
      *                                value should be the coordinates of the center of an image assuming
      *                                that the coordinates start on the top-left or bottom-left corner.
      *                                Using a value close to zero will produce inaccurate results.
      * @param leftPoints              points on left view matched with points on right view,
      *                                so they can be triangulated using estimated cameras. Both lists of
      *                                points must have the same size.
      * @param rightPoints             points on right view matched with points on left view,
      *                                so they can be triangulated using estimated cameras. Both lists of
      *                                points must have the same size.
      * @param leftCamera              instance where estimated left camera will be stored.
      * @param rightCamera             instance where estimated right camera will be stored.
      * @param triangulatedPoints      instance where triangulated 3D points will be
      *                                stored or null if triangulated points don't need to be kept.
      * @param validTriangulatedPoints instance which indicates which
      *                                triangulated 3D points are considered valid because they lie in
      *                                front of both cameras or null if such data doesn't need to be kept.
      * @return number of valid triangulated points which lie in front of the two
      * estimated cameras.
      * @throws InitialCamerasEstimationFailedException if estimation of cameras
      *                                                 fails for some reason, typically due to numerical
      *                                                 instabilities.
      * @throws IllegalArgumentException                if provided lists of left and right
      *                                                 points don't have the same size.
      */
     public static int generateInitialMetricCamerasUsingDIAC(
             final FundamentalMatrix fundamentalMatrix, final double principalPointX,
             final double principalPointY, final List<Point2D> leftPoints,
             final List<Point2D> rightPoints, final PinholeCamera leftCamera,
             final PinholeCamera rightCamera, final List<Point3D> triangulatedPoints,
             final BitSet validTriangulatedPoints)
             throws InitialCamerasEstimationFailedException {
 
         return generateInitialMetricCamerasUsingDIAC(fundamentalMatrix, principalPointX, principalPointY,
                 leftPoints, rightPoints, Corrector.DEFAULT_TYPE, leftCamera, rightCamera, triangulatedPoints,
                 validTriangulatedPoints);
     }
 
     /**
      * Generates a pair of metric cameras (up to an arbitrary space) by
      * estimating the intrinsic parameters of the views by solving the Kruppa
      * equations to obtain the Dual Image of Absolute Conic (DIAC).
      * The estimated intrinsic parameters can later be used to find the
      * essential matrix (assuming that both views have the same intrinsic
      * parameters), and the essential matrix along with provided matched 2D
      * points can be used to determine the best pair of camera pose and
      * translation that yields the largest number of triangulated points laying
      * in front of both of the estimated cameras.
      * This method uses default aspect ratio (1.0).
      *
      * @param fundamentalMatrix       fundamental matrix relating both left and right
      *                                views.
      * @param principalPointX         horizontal coordinate of principal point. This
      *                                value should be the coordinates of the center of an image assuming
      *                                that the coordinates start on the top-left or bottom-left corner.
      *                                Using a value close to zero will produce inaccurate results.
      * @param principalPointY         vertical coordinate of principal point. This
      *                                value should be the coordinates of the center of an image assuming
      *                                that the coordinates start on the top-left or bottom-left corner.
      *                                Using a value close to zero will produce inaccurate results.
      * @param leftPoints              points on left view matched with points on right view,
      *                                so they can be triangulated using estimated cameras. Both lists of
      *                                points must have the same size.
      * @param rightPoints             points on right view matched with points on left view,
      *                                so they can be triangulated using estimated cameras. Both lists of
      *                                points must have the same size.
      * @param correctorType           corrector type to be used to correct 2D points, so
      *                                they follow the epipolar geometry defined by provided fundamental
      *                                matrix so that error on triangulated points is reduced. If null, no
      *                                corrector will be used.
      * @param leftCamera              instance where estimated left camera will be stored.
      * @param rightCamera             instance where estimated right camera will be stored.
      * @param triangulatedPoints      instance where triangulated 3D points will be
      *                                stored or null if triangulated points don't need to be kept.
      * @param validTriangulatedPoints instance which indicates which
      *                                triangulated 3D points are considered valid because they lie in
      *                                front of both cameras or null if such data doesn't need to be kept.
      * @return number of valid triangulated points which lie in front of the two
      * estimated cameras.
      * @throws InitialCamerasEstimationFailedException if estimation of cameras
      *                                                 fails for some reason, typically due to numerical
      *                                                 instabilities.
      * @throws IllegalArgumentException                if provided lists of left and right
      *                                                 points don't have the same size.
      */
     public static int generateInitialMetricCamerasUsingDIAC(
             final FundamentalMatrix fundamentalMatrix, final double principalPointX,
             final double principalPointY, final List<Point2D> leftPoints,
             final List<Point2D> rightPoints, final CorrectorType correctorType,
             final PinholeCamera leftCamera, final PinholeCamera rightCamera,
             final List<Point3D> triangulatedPoints, final BitSet validTriangulatedPoints)
             throws InitialCamerasEstimationFailedException {
 
         return generateInitialMetricCamerasUsingDIAC(fundamentalMatrix,
                 principalPointX, principalPointY,
                 KruppaDualImageOfAbsoluteConicEstimator.DEFAULT_FOCAL_DISTANCE_ASPECT_RATIO, leftPoints, rightPoints,
                 correctorType, leftCamera, rightCamera, triangulatedPoints, validTriangulatedPoints);
     }
 
     /**
      * Generates a pair of metric cameras (up to an arbitrary space) by
      * estimating the intrinsic parameters of the views by solving the Kruppa
      * equations to obtain the Dual Image of Absolute Conic (DIAC).
      * The estimated intrinsic parameters can later be used to find the
      * essential matrix (assuming that both views have the same intrinsic
      * parameters), and the essential matrix along with provided matched 2D
      * points can be used to determine the best pair of camera pose and
      * translation that yields the largest number of triangulated points laying
      * in front of both of the estimated cameras.
      * This method uses default corrector type and does not keep triangulated
      * points or valid triangulated points.
      *
      * @param fundamentalMatrix fundamental matrix relating both left and right
      *                          views.
      * @param principalPointX   horizontal coordinate of principal point. This
      *                          value should be the coordinates of the center of an image assuming that
      *                          the coordinates start on the top-left or bottom-left corner. Using a
      *                          value close to zero will produce inaccurate results.
      * @param principalPointY   vertical coordinate of principal point. This
      *                          value should be the coordinates of the center of an image assuming that
      *                          the coordinates start on the top-left or bottom-left corner. Using a
      *                          value close to zero will produce inaccurate results.
      * @param aspectRatio       aspect ratio for estimated intrinsic parameters. This
      *                          is typically 1.0 if vertical coordinates increase upwards or -1.0 if it
      *                          is the opposite.
      * @param leftPoints        points on left view matched with points on right view,
      *                          so they can be triangulated using estimated cameras. Both lists of points
      *                          must have the same size.
      * @param rightPoints       points on right view matched with points on left view,
      *                          so they can be triangulated using estimated cameras. Both lists of points
      *                          must have the same size.
      * @param leftCamera        instance where estimated left camera will be stored.
      * @param rightCamera       instance where estimated right camera will be stored.
      * @return number of valid triangulated points which lie in front of the two
      * estimated cameras.
      * @throws InitialCamerasEstimationFailedException if estimation of cameras
      *                                                 fails for some reason, typically due to numerical
      *                                                 instabilities.
      * @throws IllegalArgumentException                if provided lists of left and right
      *                                                 points don't have the same size.
      */
     public static int generateInitialMetricCamerasUsingDIAC(
             final FundamentalMatrix fundamentalMatrix, final double principalPointX,
             final double principalPointY, final double aspectRatio,
             final List<Point2D> leftPoints, final List<Point2D> rightPoints,
             final PinholeCamera leftCamera, final PinholeCamera rightCamera)
             throws InitialCamerasEstimationFailedException {
 
         return generateInitialMetricCamerasUsingDIAC(fundamentalMatrix, principalPointX, principalPointY, aspectRatio,
                 leftPoints, rightPoints, Corrector.DEFAULT_TYPE, leftCamera, rightCamera);
     }
 
     /**
      * Generates a pair of metric cameras (up to an arbitrary space) by
      * estimating the intrinsic parameters of the views by solving the Kruppa
      * equations to obtain the Dual Image of Absolute Conic (DIAC).
      * The estimated intrinsic parameters can later be used to find the
      * essential matrix (assuming that both views have the same intrinsic
      * parameters), and the essential matrix along with provided matched 2D
      * points can be used to determine the best pair of camera pose and
      * translation that yields the largest number of triangulated points laying
      * in front of both of the estimated cameras.
      * This method does not keep triangulated points or valid triangulated
      * points.
      *
      * @param fundamentalMatrix fundamental matrix relating both left and right
      *                          views.
      * @param principalPointX   horizontal coordinate of principal point. This
      *                          value should be the coordinates of the center of an image assuming that
      *                          the coordinates start on the top-left or bottom-left corner. Using a
      *                          value close to zero will produce inaccurate results.
      * @param principalPointY   vertical coordinate of principal point. This
      *                          value should be the coordinates of the center of an image assuming that
      *                          the coordinates start on the top-left or bottom-left corner. Using a
      *                          value close to zero will produce inaccurate results.
      * @param aspectRatio       aspect ratio for estimated intrinsic parameters. This
      *                          is typically 1.0 if vertical coordinates increase upwards or -1.0 if it
      *                          is the opposite.
      * @param leftPoints        points on left view matched with points on right view,
      *                          so they can be triangulated using estimated cameras. Both lists of points
      *                          must have the same size.
      * @param rightPoints       points on right view matched with points on left view,
      *                          so they can be triangulated using estimated cameras. Both lists of points
      *                          must have the same size.
      * @param correctorType     corrector type to be used to correct 2D points, so
      *                          they follow the epipolar geometry defined by provided fundamental matrix
      *                          so that error on triangulated points is reduced. If null, no corrector
      *                          will be used.
      * @param leftCamera        instance where estimated left camera will be stored.
      * @param rightCamera       instance where estimated right camera will be stored.
      * @return number of valid triangulated points which lie in front of the two
      * estimated cameras.
      * @throws InitialCamerasEstimationFailedException if estimation of
      *                                                 cameras fails for some reason, typically due to
      *                                                 numerical instabilities.
      * @throws IllegalArgumentException                if provided lists of left and right
      *                                                 points don't have the same size.
      */
     public static int generateInitialMetricCamerasUsingDIAC(
             final FundamentalMatrix fundamentalMatrix, final double principalPointX,
             final double principalPointY, final double aspectRatio,
             final List<Point2D> leftPoints, final List<Point2D> rightPoints,
             final CorrectorType correctorType, final PinholeCamera leftCamera,
             final PinholeCamera rightCamera) throws InitialCamerasEstimationFailedException {
 
         return generateInitialMetricCamerasUsingDIAC(fundamentalMatrix, principalPointX, principalPointY, aspectRatio,
                 leftPoints, rightPoints, correctorType, leftCamera, rightCamera, null,
                 null);
     }
 
     /**
      * Generates a pair of metric cameras (up to an arbitrary space) by
      * estimating the intrinsic parameters of the views by solving the Kruppa
      * equations to obtain the Dual Image of Absolute Conic (DIAC).
      * The estimated intrinsic parameters can later be used to find the
      * essential matrix (assuming that both views have the same intrinsic
      * parameters), and the essential matrix along with provided matched 2D
      * points can be used to determine the best pair of camera pose and
      * translation that yields the largest number of triangulated points laying
      * in front of both of the estimated cameras.
      * This method uses default corrector type.
      *
      * @param fundamentalMatrix       fundamental matrix relating both left and right
      *                                views.
      * @param principalPointX         horizontal coordinate of principal point. This
      *                                value should be the coordinates of the center of an image assuming
      *                                that the coordinates start on the top-left or bottom-left corner.
      *                                Using a value close to zero will produce inaccurate results.
      * @param principalPointY         vertical coordinate of principal point. This
      *                                value should be the coordinates of the center of an image assuming
      *                                that the coordinates start on the top-left or bottom-left corner.
      *                                Using a value close to zero will produce inaccurate results.
      * @param aspectRatio             aspect ratio for estimated intrinsic parameters. This
      *                                is typically 1.0 if vertical coordinates increase upwards or -1.0 if
      *                                it is the opposite.
      * @param leftPoints              points on left view matched with points on right view,
      *                                so they can be triangulated using estimated cameras. Both lists of
      *                                points must have the same size.
      * @param rightPoints             points on right view matched with points on left view,
      *                                so they can be triangulated using estimated cameras. Both lists of
      *                                points must have the same size.
      * @param leftCamera              instance where estimated left camera will be stored.
      * @param rightCamera             instance where estimated right camera will be stored.
      * @param triangulatedPoints      instance where triangulated 3D points will be
      *                                stored or null if triangulated points don't need to be kept.
      * @param validTriangulatedPoints instance which indicates which
      *                                triangulated 3D points are considered valid because they lie in
      *                                front of both cameras or null if such data doesn't need to be kept.
      * @return number of valid triangulated points which lie in front of the two
      * estimated cameras.
      * @throws InitialCamerasEstimationFailedException if estimation of cameras
      *                                                 fails for some reason, typically due to numerical
      *                                                 instabilities.
      * @throws IllegalArgumentException                if provided lists of left and right
      *                                                 points don't have the same size.
      */
     public static int generateInitialMetricCamerasUsingDIAC(
             final FundamentalMatrix fundamentalMatrix, final double principalPointX,
             final double principalPointY, final double aspectRatio,
             final List<Point2D> leftPoints, final List<Point2D> rightPoints,
             final PinholeCamera leftCamera, final PinholeCamera rightCamera,
             final List<Point3D> triangulatedPoints, final BitSet validTriangulatedPoints)
             throws InitialCamerasEstimationFailedException {
 
         return generateInitialMetricCamerasUsingDIAC(fundamentalMatrix, principalPointX, principalPointY, aspectRatio,
                 leftPoints, rightPoints, Corrector.DEFAULT_TYPE, leftCamera, rightCamera, triangulatedPoints,
                 validTriangulatedPoints);
     }
 
     /**
      * Generates a pair of metric cameras (up to an arbitrary space) by
      * estimating the intrinsic parameters of the views by solving the Kruppa
      * equations to obtain the Dual Image of Absolute Conic (DIAC).
      * The estimated intrinsic parameters can later be used to find the
      * essential matrix (assuming that both views have the same intrinsic
      * parameters), and the essential matrix along with provided matched 2D
      * points can be used to determine the best pair of camera pose and
      * translation that yields the largest number of triangulated points laying
      * in front of both of the estimated cameras.
      *
      * @param fundamentalMatrix       fundamental matrix relating both left and right
      *                                views.
      * @param principalPointX         horizontal coordinate of principal point. This
      *                                value should be the coordinates of the center of an image assuming
      *                                that the coordinates start on the top-left or bottom-left corner.
      *                                Using a value close to zero will produce inaccurate results.
      * @param principalPointY         vertical coordinate of principal point. This
      *                                value should be the coordinates of the center of an image assuming
      *                                that the coordinates start on the top-left or bottom-left corner.
      *                                Using a value close to zero will produce inaccurate results.
      * @param aspectRatio             aspect ratio for estimated intrinsic parameters. This
      *                                is typically 1.0 if vertical coordinates increase upwards or -1.0 if
      *                                it is the opposite.
      * @param leftPoints              points on left view matched with points on right view,
      *                                so they can be triangulated using estimated cameras. Both lists of
      *                                points must have the same size.
      * @param rightPoints             points on right view matched with points on left view,
      *                                so they can be triangulated using estimated cameras. Both lists of
      *                                points must have the same size.
      * @param correctorType           corrector type to be used to correct 2D points, so
      *                                they follow the epipolar geometry defined by provided fundamental
      *                                matrix so that error on triangulated points is reduced. If null, no
      *                                corrector will be used.
      * @param leftCamera              instance where estimated left camera will be stored.
      * @param rightCamera             instance where estimated right camera will be stored.
      * @param triangulatedPoints      instance where triangulated 3D points will be
      *                                stored or null if triangulated points don't need to be kept.
      * @param validTriangulatedPoints instance which indicates which
      *                                triangulated 3D points are considered valid because they lie in
      *                                front of both cameras or null if such data doesn't need to be kept.
      * @return number of valid triangulated points which lie in front of the two
      * estimated cameras.
      * @throws InitialCamerasEstimationFailedException if estimation of cameras
      *                                                 fails for some reason, typically due to numerical
      *                                                 instabilities.
      * @throws IllegalArgumentException                if provided lists of left and right
      *                                                 points don't have the same size.
      */
     public static int generateInitialMetricCamerasUsingDIAC(
             final FundamentalMatrix fundamentalMatrix, final double principalPointX,
             final double principalPointY, final double aspectRatio,
             final List<Point2D> leftPoints, final List<Point2D> rightPoints,
             final CorrectorType correctorType, final PinholeCamera leftCamera,
             final PinholeCamera rightCamera, final List<Point3D> triangulatedPoints,
             final BitSet validTriangulatedPoints) throws InitialCamerasEstimationFailedException {
 
         try {
             final KruppaDualImageOfAbsoluteConicEstimator diacEstimator = new KruppaDualImageOfAbsoluteConicEstimator(
                     fundamentalMatrix);
             diacEstimator.setPrincipalPointX(principalPointX);
             diacEstimator.setPrincipalPointY(principalPointY);
             diacEstimator.setFocalDistanceAspectRatioKnown(true);
             diacEstimator.setFocalDistanceAspectRatio(aspectRatio);
 
             final var diac = diacEstimator.estimate();
 
             final var intrinsic = diac.getIntrinsicParameters();
 
             return EssentialMatrixInitialCamerasEstimator.generateInitialMetricCamerasFromEssentialMatrix(
                     fundamentalMatrix, intrinsic, intrinsic, leftPoints, rightPoints, correctorType, leftCamera,
                     rightCamera, triangulatedPoints, validTriangulatedPoints);
 
         } catch (final InitialCamerasEstimationFailedException e) {
             throw e;
         } catch (final Exception e) {
             throw new InitialCamerasEstimationFailedException(e);
         }
     }
 
     /**
      * Internal method to set matched 2D points on left and right views.
      * This method does not check whether the estimator is locked or not, only
      * ensures that provided lists have the same size.
      *
      * @param leftPoints  matched 2D points on left view.
      * @param rightPoints matched 2D points on right view.
      * @throws IllegalArgumentException if provided lists don't have the same
      *                                  size.
      */
     private void internalSetLeftAndRightPoints(final List<Point2D> leftPoints, final List<Point2D> rightPoints) {
         if (leftPoints == null || rightPoints == null || leftPoints.size() != rightPoints.size()) {
             throw new IllegalArgumentException();
         }
         this.leftPoints = leftPoints;
         this.rightPoints = rightPoints;
     }
 }