/*
    * Copyright (C) 2017 Alberto Irurueta Carro (alberto@irurueta.com)
    *
    * Licensed under the Apache License, Version 2.0 (the "License");
    * you may not use this file except in compliance with the License.
    * You may obtain a copy of the License at
    *
    *         http://www.apache.org/licenses/LICENSE-2.0
    *
   * Unless required by applicable law or agreed to in writing, software
   * distributed under the License is distributed on an "AS IS" BASIS,
   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   * See the License for the specific language governing permissions and
   * limitations under the License.
   */
  package com.irurueta.ar.sfm;
  
  import com.irurueta.algebra.Matrix;
  import com.irurueta.algebra.WrongSizeException;
  import com.irurueta.ar.epipolar.Corrector;
  import com.irurueta.ar.epipolar.CorrectorType;
  import com.irurueta.ar.epipolar.EssentialMatrix;
  import com.irurueta.ar.epipolar.FundamentalMatrix;
  import com.irurueta.geometry.CameraException;
  import com.irurueta.geometry.PinholeCamera;
  import com.irurueta.geometry.PinholeCameraIntrinsicParameters;
  import com.irurueta.geometry.Point2D;
  import com.irurueta.geometry.Point3D;
  import com.irurueta.geometry.Rotation3D;
  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 and provided intrinsic
   * parameters on left and right views (which can be obtained by offline
   * calibration) to compute the essential matrix and choose the best combination
   * of rotation and translation on estimated cameras so that triangulated 3D
   * points obtained from provided matched 2D points are located in front of
   * the estimated cameras.
   */
  @SuppressWarnings("DuplicatedCode")
  public class EssentialMatrixInitialCamerasEstimator 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;
  
      /**
       * Intrinsic parameters to be used for estimated left camera.
       */
      private PinholeCameraIntrinsicParameters leftIntrinsic;
  
      /**
       * Intrinsic parameters to be used for estimated right camera.
       */
      private PinholeCameraIntrinsicParameters rightIntrinsic;
  
      /**
       * 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 EssentialMatrixInitialCamerasEstimator() {
         super();
     }
 
     /**
      * Constructor.
      *
      * @param fundamentalMatrix fundamental matrix relating two views.
      */
     public EssentialMatrixInitialCamerasEstimator(final FundamentalMatrix fundamentalMatrix) {
         super(fundamentalMatrix);
     }
 
     /**
      * Constructor.
      *
      * @param leftIntrinsic  intrinsic parameters to be used for estimated left
      *                       camera.
      * @param rightIntrinsic intrinsic parameters to be used for estimated right
      *                       camera.
      */
     public EssentialMatrixInitialCamerasEstimator(
             final PinholeCameraIntrinsicParameters leftIntrinsic,
             final PinholeCameraIntrinsicParameters rightIntrinsic) {
         super();
         this.leftIntrinsic = leftIntrinsic;
         this.rightIntrinsic = rightIntrinsic;
     }
 
     /**
      * Constructor.
      *
      * @param fundamentalMatrix fundamental matrix relating two views.
      * @param leftIntrinsic     intrinsic parameters to be used for estimated left
      *                          camera.
      * @param rightIntrinsic    intrinsic parameters to be used for estimated right
      *                          camera.
      */
     public EssentialMatrixInitialCamerasEstimator(
             final FundamentalMatrix fundamentalMatrix,
             final PinholeCameraIntrinsicParameters leftIntrinsic,
             final PinholeCameraIntrinsicParameters rightIntrinsic) {
         super(fundamentalMatrix);
         this.leftIntrinsic = leftIntrinsic;
         this.rightIntrinsic = rightIntrinsic;
     }
 
     /**
      * 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 EssentialMatrixInitialCamerasEstimator(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 EssentialMatrixInitialCamerasEstimator(
             final FundamentalMatrix fundamentalMatrix, final List<Point2D> leftPoints,
             final List<Point2D> rightPoints) {
         super(fundamentalMatrix);
         internalSetLeftAndRightPoints(leftPoints, rightPoints);
     }
 
     /**
      * Constructor.
      *
      * @param leftIntrinsic  intrinsic parameters to be used for estimated left
      *                       camera.
      * @param rightIntrinsic intrinsic parameters to be used for estimated right
      *                       camera.
      * @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 EssentialMatrixInitialCamerasEstimator(
             final PinholeCameraIntrinsicParameters leftIntrinsic,
             final PinholeCameraIntrinsicParameters rightIntrinsic,
             final List<Point2D> leftPoints, final List<Point2D> rightPoints) {
         this(leftIntrinsic, rightIntrinsic);
         internalSetLeftAndRightPoints(leftPoints, rightPoints);
     }
 
     /**
      * Constructor.
      *
      * @param fundamentalMatrix fundamental matrix relating two views.
      * @param leftIntrinsic     intrinsic parameters to be used for estimated left
      *                          camera.
      * @param rightIntrinsic    intrinsic parameters to be used for estimated right
      *                          camera.
      * @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 EssentialMatrixInitialCamerasEstimator(
             final FundamentalMatrix fundamentalMatrix,
             final PinholeCameraIntrinsicParameters leftIntrinsic,
             final PinholeCameraIntrinsicParameters rightIntrinsic,
             final List<Point2D> leftPoints, final List<Point2D> rightPoints) {
         this(fundamentalMatrix, leftIntrinsic, rightIntrinsic);
         internalSetLeftAndRightPoints(leftPoints, rightPoints);
     }
 
     /**
      * Constructor.
      *
      * @param listener listener to handle events raised by this instance.
      */
     public EssentialMatrixInitialCamerasEstimator(final InitialCamerasEstimatorListener listener) {
         super(listener);
     }
 
     /**
      * Constructor.
      *
      * @param fundamentalMatrix fundamental matrix relating two views.
      * @param listener          listener to handle events raised by this instance.
      */
     public EssentialMatrixInitialCamerasEstimator(
             final FundamentalMatrix fundamentalMatrix, final InitialCamerasEstimatorListener listener) {
         super(fundamentalMatrix, listener);
     }
 
     /**
      * Constructor.
      *
      * @param leftIntrinsic  intrinsic parameters to be used for estimated left
      *                       camera.
      * @param rightIntrinsic intrinsic parameters to be used for estimated right
      *                       camera.
      * @param listener       listener to handle events raised by this instance.
      */
     public EssentialMatrixInitialCamerasEstimator(
             final PinholeCameraIntrinsicParameters leftIntrinsic,
             final PinholeCameraIntrinsicParameters rightIntrinsic,
             final InitialCamerasEstimatorListener listener) {
         super(listener);
         this.leftIntrinsic = leftIntrinsic;
         this.rightIntrinsic = rightIntrinsic;
     }
 
     /**
      * Constructor.
      *
      * @param fundamentalMatrix fundamental matrix relating two views.
      * @param leftIntrinsic     intrinsic parameters to be used for estimated left
      *                          camera.
      * @param rightIntrinsic    intrinsic parameters to be used for estimated right
      *                          camera.
      * @param listener          listener to handle events raised by this instance.
      */
     public EssentialMatrixInitialCamerasEstimator(
             final FundamentalMatrix fundamentalMatrix,
             final PinholeCameraIntrinsicParameters leftIntrinsic,
             final PinholeCameraIntrinsicParameters rightIntrinsic,
             final InitialCamerasEstimatorListener listener) {
         super(fundamentalMatrix, listener);
         this.leftIntrinsic = leftIntrinsic;
         this.rightIntrinsic = rightIntrinsic;
     }
 
     /**
      * 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 EssentialMatrixInitialCamerasEstimator(
             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 EssentialMatrixInitialCamerasEstimator(
             final FundamentalMatrix fundamentalMatrix,
             final List<Point2D> leftPoints,
             final List<Point2D> rightPoints,
             final InitialCamerasEstimatorListener listener) {
         super(fundamentalMatrix, listener);
         internalSetLeftAndRightPoints(leftPoints, rightPoints);
     }
 
     /**
      * Constructor.
      *
      * @param leftIntrinsic  intrinsic parameters to be used for estimated left
      *                       camera.
      * @param rightIntrinsic intrinsic parameters to be used for estimated right
      *                       camera.
      * @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 EssentialMatrixInitialCamerasEstimator(
             final PinholeCameraIntrinsicParameters leftIntrinsic,
             final PinholeCameraIntrinsicParameters rightIntrinsic,
             final List<Point2D> leftPoints,
             final List<Point2D> rightPoints,
             final InitialCamerasEstimatorListener listener) {
         this(leftIntrinsic, rightIntrinsic, listener);
         internalSetLeftAndRightPoints(leftPoints, rightPoints);
     }
 
     /**
      * Constructor.
      *
      * @param fundamentalMatrix fundamental matrix relating two views.
      * @param leftIntrinsic     intrinsic parameters to be used for estimated left
      *                          camera.
      * @param rightIntrinsic    intrinsic parameters to be used for estimated right
      *                          camera.
      * @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 EssentialMatrixInitialCamerasEstimator(
             final FundamentalMatrix fundamentalMatrix,
             final PinholeCameraIntrinsicParameters leftIntrinsic,
             final PinholeCameraIntrinsicParameters rightIntrinsic,
             final List<Point2D> leftPoints,
             final List<Point2D> rightPoints,
             final InitialCamerasEstimatorListener listener) {
         this(fundamentalMatrix, leftIntrinsic, rightIntrinsic, listener);
         internalSetLeftAndRightPoints(leftPoints, rightPoints);
     }
 
     /**
      * Returns method used by this estimator.
      *
      * @return method used by this estimator.
      */
     @Override
     public InitialCamerasEstimatorMethod getMethod() {
         return InitialCamerasEstimatorMethod.ESSENTIAL_MATRIX;
     }
 
     /**
      * Indicates if estimator is ready.
      *
      * @return true if estimator is ready, false otherwise.
      */
     @Override
     public boolean isReady() {
         return fundamentalMatrix != null && leftIntrinsic != null && rightIntrinsic != 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.
      */
     @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();
             }
 
             generateInitialMetricCamerasFromEssentialMatrix(fundamentalMatrix, leftIntrinsic, rightIntrinsic,
                     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 intrinsic parameters to be used for estimated left camera.
      *
      * @return intrinsic parameters to be used for estimated left camera.
      */
     public PinholeCameraIntrinsicParameters getLeftIntrinsic() {
         return leftIntrinsic;
     }
 
     /**
      * Sets intrinsic parameters to be used for estimated left camera.
      *
      * @param leftIntrinsic intrinsic parameters to be used for estimated left
      *                      camera.
      * @throws LockedException if estimator is locked.
      */
     public void setLeftIntrinsic(
             final PinholeCameraIntrinsicParameters leftIntrinsic) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         this.leftIntrinsic = leftIntrinsic;
     }
 
     /**
      * Gets intrinsic parameters to be used for estimated right camera.
      *
      * @return intrinsic parameters to be used for estimated right camera.
      */
     public PinholeCameraIntrinsicParameters getRightIntrinsic() {
         return rightIntrinsic;
     }
 
     /**
      * Sets intrinsic parameters to be used for estimated right camera.
      *
      * @param rightIntrinsic intrinsic parameters to be used for estimated right
      *                       camera.
      * @throws LockedException if estimator is locked.
      */
     public void setRightIntrinsic(
             final PinholeCameraIntrinsicParameters rightIntrinsic) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         this.rightIntrinsic = rightIntrinsic;
     }
 
     /**
      * Sets intrinsic parameters to be used for estimated left and right
      * cameras.
      *
      * @param leftIntrinsic  intrinsic parameters to be used for estimated left
      *                       camera.
      * @param rightIntrinsic intrinsic parameters to be used for estimated right
      *                       camera.
      * @throws LockedException if estimator is locked.
      */
     public void setLeftAndRightIntrinsics(
             final PinholeCameraIntrinsicParameters leftIntrinsic,
             final PinholeCameraIntrinsicParameters rightIntrinsic) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         this.leftIntrinsic = leftIntrinsic;
         this.rightIntrinsic = rightIntrinsic;
     }
 
     /**
      * Sets the same intrinsic parameters to be used for both estimated left
      * and right cameras.
      *
      * @param intrinsic intrinsic parameters to be used for both cameras.
      * @throws LockedException if estimator is locked.
      */
     public void setIntrinsicsForBoth(final PinholeCameraIntrinsicParameters intrinsic) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         leftIntrinsic = rightIntrinsic = intrinsic;
     }
 
     /**
      * 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
      * computing the essential matrix from provided fundamental matrix and
      * intrinsic parameters of left and right cameras, and choosing 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 leftIntrinsic     intrinsic parameters to be set on left view.
      *                          This can be used when cameras have been previously calibrated.
      * @param rightIntrinsic    intrinsic parameters to be set on right view.
      *                          This can be used when cameras have been previously calibrated.
      * @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 generateInitialMetricCamerasFromEssentialMatrix(
             final FundamentalMatrix fundamentalMatrix,
             final PinholeCameraIntrinsicParameters leftIntrinsic,
             final PinholeCameraIntrinsicParameters rightIntrinsic,
             final List<Point2D> leftPoints, final List<Point2D> rightPoints,
             final PinholeCamera leftCamera, final PinholeCamera rightCamera)
             throws InitialCamerasEstimationFailedException {
         return generateInitialMetricCamerasFromEssentialMatrix(
                 fundamentalMatrix, leftIntrinsic, rightIntrinsic, leftPoints,
                 rightPoints, Corrector.DEFAULT_TYPE, leftCamera, rightCamera);
     }
 
     /**
      * Generates a pair of metric cameras (up to an arbitrary space) by
      * computing the essential matrix from provided fundamental matrix and
      * intrinsic parameters of left and right cameras, and choosing 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 leftIntrinsic     intrinsic parameters to be set on left view.
      *                          This can be used when cameras have been previously calibrated.
      * @param rightIntrinsic    intrinsic parameters to be set on right view.
      *                          This can be used when cameras have been previously calibrated.
      * @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 generateInitialMetricCamerasFromEssentialMatrix(
             final FundamentalMatrix fundamentalMatrix,
             final PinholeCameraIntrinsicParameters leftIntrinsic,
             final PinholeCameraIntrinsicParameters rightIntrinsic,
             final List<Point2D> leftPoints, final List<Point2D> rightPoints,
             final CorrectorType correctorType, final PinholeCamera leftCamera,
             final PinholeCamera rightCamera) throws InitialCamerasEstimationFailedException {
         return generateInitialMetricCamerasFromEssentialMatrix(
                 fundamentalMatrix, leftIntrinsic, rightIntrinsic, leftPoints,
                 rightPoints, correctorType, leftCamera, rightCamera, null,
                 null);
     }
 
     /**
      * Generates a pair of metric cameras (up to an arbitrary space) by
      * computing the essential matrix from provided fundamental matrix and
      * intrinsic parameters of left and right cameras, and choosing 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 leftIntrinsic           intrinsic parameters to be set on left view.
      *                                This can be used when cameras have been previously calibrated.
      * @param rightIntrinsic          intrinsic parameters to be set on right view.
      *                                This can be used when cameras have been previously calibrated.
      * @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 generateInitialMetricCamerasFromEssentialMatrix(
             final FundamentalMatrix fundamentalMatrix,
             final PinholeCameraIntrinsicParameters leftIntrinsic,
             final PinholeCameraIntrinsicParameters rightIntrinsic,
             final List<Point2D> leftPoints, final List<Point2D> rightPoints,
             final PinholeCamera leftCamera, final PinholeCamera rightCamera,
             final List<Point3D> triangulatedPoints, final BitSet validTriangulatedPoints)
             throws InitialCamerasEstimationFailedException {
 
         return generateInitialMetricCamerasFromEssentialMatrix(
                 fundamentalMatrix, leftIntrinsic, rightIntrinsic, leftPoints,
                 rightPoints, Corrector.DEFAULT_TYPE, leftCamera, rightCamera,
                 triangulatedPoints, validTriangulatedPoints);
     }
 
     /**
      * Generates a pair of metric cameras (up to an arbitrary space) by
      * computing the essential matrix from provided fundamental matrix and
      * intrinsic parameters of left and right cameras, and choosing 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 leftIntrinsic           intrinsic parameters to be set on left view.
      *                                This can be used when cameras have been previously calibrated.
      * @param rightIntrinsic          intrinsic parameters to be set on right view.
      *                                This can be used when cameras have been previously calibrated.
      * @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 generateInitialMetricCamerasFromEssentialMatrix(
             final FundamentalMatrix fundamentalMatrix,
             final PinholeCameraIntrinsicParameters leftIntrinsic,
             final PinholeCameraIntrinsicParameters rightIntrinsic,
             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 {
 
         if (leftPoints.size() != rightPoints.size()) {
             throw new IllegalArgumentException(
                     "left and right points must have the same size");
         }
 
         final List<Point2D> correctedLeftPoints;
         final List<Point2D> correctedRightPoints;
         final Rotation3D rotation1;
         final Rotation3D rotation2;
         final Point2D translation1;
         final Point2D translation2;
         try {
             final var essential = new EssentialMatrix(fundamentalMatrix, leftIntrinsic, rightIntrinsic);
 
             essential.computePossibleRotationAndTranslations();
 
             rotation1 = essential.getFirstPossibleRotation();
             translation1 = essential.getFirstPossibleTranslation();
 
             rotation2 = essential.getSecondPossibleRotation();
             translation2 = essential.getSecondPossibleTranslation();
 
             if (correctorType != null) {
                 // use corrector
                 final var corrector = Corrector.create(leftPoints, rightPoints, fundamentalMatrix, correctorType);
                 corrector.correct();
 
                 correctedLeftPoints = corrector.getLeftCorrectedPoints();
                 correctedRightPoints = corrector.getRightCorrectedPoints();
             } else {
                 // don't use corrector
                 correctedLeftPoints = leftPoints;
                 correctedRightPoints = rightPoints;
             }
         } catch (final Exception e) {
             throw new InitialCamerasEstimationFailedException(e);
         }
 
         if (triangulatedPoints != null) {
             triangulatedPoints.clear();
         }
         if (validTriangulatedPoints != null) {
             validTriangulatedPoints.clear();
         }
         int numValidTriangulatedPoints;
 
         final var numPoints = correctedLeftPoints.size();
         var skip = false;
 
         // obtain 1st pair of possible cameras and their corresponding
         // triangulated points
         try {
             numValidTriangulatedPoints = computeCamerasAndTriangulation(rotation1, translation1,
                     leftIntrinsic, rightIntrinsic, correctedLeftPoints, correctedRightPoints, leftCamera, rightCamera,
                     triangulatedPoints, validTriangulatedPoints);
         } catch (final Exception e) {
             numValidTriangulatedPoints = 0;
         }
 
         if (numValidTriangulatedPoints >= numPoints) {
             // all points are valid, hence, we can set current pair of
             // cameras as the best result
             skip = true;
         }
 
         final var attemptLeftCamera = new PinholeCamera();
         final var attemptRightCamera = new PinholeCamera();
         List<Point3D> attemptTriangulatedPoints = null;
         if (triangulatedPoints != null) {
             attemptTriangulatedPoints = new ArrayList<>();
         }
         BitSet attemptValidTriangulatedPoints = null;
         if (validTriangulatedPoints != null) {
             attemptValidTriangulatedPoints = new BitSet(numPoints);
         }
         int attemptNumValidTriangulatedPoints;
 
         if (!skip) {
             // obtain 2nd pair of possible cameras and their corresponding
             // triangulated points
             try {
                 attemptNumValidTriangulatedPoints = computeCamerasAndTriangulation(rotation1, translation2,
                         leftIntrinsic, rightIntrinsic, correctedLeftPoints, correctedRightPoints, attemptLeftCamera,
                         attemptRightCamera, attemptTriangulatedPoints, attemptValidTriangulatedPoints);
             } catch (final Exception e) {
                 attemptNumValidTriangulatedPoints = 0;
             }
 
             if (attemptNumValidTriangulatedPoints >= numPoints) {
                 // all points are valid, hence, we can set current pair of
                 // cameras as the best result
                 skip = true;
             }
 
             if (attemptNumValidTriangulatedPoints > numValidTriangulatedPoints) {
                 // a better solution containing more valid points has been found
 
                 // keep better solution
                 updateBestSolutionData(leftCamera, rightCamera, triangulatedPoints, validTriangulatedPoints,
                         attemptLeftCamera, attemptRightCamera, attemptTriangulatedPoints,
                         attemptValidTriangulatedPoints);
                 numValidTriangulatedPoints = attemptNumValidTriangulatedPoints;
             }
         }
 
         if (!skip) {
             // obtain 3rd pair of possible cameras and their corresponding
             // triangulated points
             try {
                 attemptNumValidTriangulatedPoints = computeCamerasAndTriangulation(rotation2, translation1,
                         leftIntrinsic, rightIntrinsic, correctedLeftPoints, correctedRightPoints, attemptLeftCamera,
                         attemptRightCamera, attemptTriangulatedPoints, attemptValidTriangulatedPoints);
             } catch (final Exception e) {
                 attemptNumValidTriangulatedPoints = 0;
             }
 
             if (attemptNumValidTriangulatedPoints >= numPoints) {
                 // all points are valid, hence, we can set current pair of
                 // cameras as the best result
                 skip = true;
             }
 
             if (attemptNumValidTriangulatedPoints > numValidTriangulatedPoints) {
                 // a better solution containing more valid points has been found
 
                 // keep better solution
                 updateBestSolutionData(leftCamera, rightCamera, triangulatedPoints, validTriangulatedPoints,
                         attemptLeftCamera, attemptRightCamera, attemptTriangulatedPoints,
                         attemptValidTriangulatedPoints);
                 numValidTriangulatedPoints = attemptNumValidTriangulatedPoints;
             }
         }
 
         if (!skip) {
             // obtain 4th pair of possible cameras and their corresponding
             // triangulated points
             try {
                 attemptNumValidTriangulatedPoints = computeCamerasAndTriangulation(rotation2, translation2,
                         leftIntrinsic, rightIntrinsic, correctedLeftPoints, correctedRightPoints, attemptLeftCamera,
                         attemptRightCamera, attemptTriangulatedPoints, attemptValidTriangulatedPoints);
             } catch (final Exception e) {
                 attemptNumValidTriangulatedPoints = 0;
             }
 
             if (attemptNumValidTriangulatedPoints > numValidTriangulatedPoints) {
                 // a better solution containing more valid points has been found
 
                 // keep better solution
                 updateBestSolutionData(leftCamera, rightCamera, triangulatedPoints, validTriangulatedPoints,
                         attemptLeftCamera, attemptRightCamera, attemptTriangulatedPoints,
                         attemptValidTriangulatedPoints);
                 numValidTriangulatedPoints = attemptNumValidTriangulatedPoints;
             }
         }
 
         if (numValidTriangulatedPoints == 0) {
             throw new InitialCamerasEstimationFailedException("no valid points found");
         }
 
         return numValidTriangulatedPoints;
     }
 
     /**
      * 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;
     }
 
     /**
      * Updates data for best solution found so far.
      *
      * @param leftCamera                     instance where best found left camera will be stored.
      * @param rightCamera                    instance where best found right camera will be stored.
      * @param triangulatedPoints             instance where triangulated points for best
      *                                       found solution will be stored or null if points don't need to
      *                                       be kept.
      * @param validTriangulatedPoints        instance where valid triangulated points
      *                                       for best found solution will be stored or null if such data
      *                                       doesn't need to be kept.
      * @param attemptLeftCamera              estimated left camera to be copied into the best
      *                                       solution.
      * @param attemptRightCamera             estimated right camera to be copied into the best
      *                                       solution.
      * @param attemptTriangulatedPoints      triangulated points to be copied into the
      *                                       best solution, or null if nothing needs to be copied.
      * @param attemptValidTriangulatedPoints valid triangulated points to be
      *                                       copied into the best solution, or null if nothing needs to
      *                                       be copied.
      * @throws InitialCamerasEstimationFailedException if something fails.
      */
     private static void updateBestSolutionData(
             final PinholeCamera leftCamera,
             final PinholeCamera rightCamera,
             final List<Point3D> triangulatedPoints,
             final BitSet validTriangulatedPoints,
             final PinholeCamera attemptLeftCamera,
             final PinholeCamera attemptRightCamera,
             final List<Point3D> attemptTriangulatedPoints,
             final BitSet attemptValidTriangulatedPoints)
             throws InitialCamerasEstimationFailedException {
 
         try {
             leftCamera.setInternalMatrix(attemptLeftCamera.getInternalMatrix());
             rightCamera.setInternalMatrix(attemptRightCamera.getInternalMatrix());
             if (triangulatedPoints != null && attemptTriangulatedPoints != null) {
                 triangulatedPoints.clear();
                 triangulatedPoints.addAll(attemptTriangulatedPoints);
             }
             if (validTriangulatedPoints != null && attemptValidTriangulatedPoints != null) {
                 validTriangulatedPoints.clear();
                 validTriangulatedPoints.or(attemptValidTriangulatedPoints);
             }
         } catch (final WrongSizeException e) {
             throw new InitialCamerasEstimationFailedException(e);
         }
     }
 
     /**
      * Computes a pair of cameras for provided rotation and translation
      * using provided intrinsic parameters.
      * This method also triangulates proved matched 2D points and determines
      * how many of them lie in front of both estimated cameras.
      *
      * @param rotation                rotation between estimated left and right cameras.
      * @param translation             translation between estimated left and right cameras.
      * @param leftIntrinsic           intrinsic parameters to set on estimated left
      *                                camera.
      * @param rightIntrinsic          intrinsic parameters to set on estimated right
      *                                camera.
      * @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 estimatedLeftCamera     instance where estimated left camera will be
      *                                stored.
      * @param estimatedRightCamera    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 WrongSizeException            never occurs.
      * @throws CameraException               if any of the estimated cameras become
      *                                       numerically unstable.
      * @throws LockedException               never occurs.
      * @throws NotReadyException             never occurs.
      * @throws Point3DTriangulationException if points cannot be triangulated
      *                                       because of numerical instabilities.
      */
     private static int computeCamerasAndTriangulation(
             final Rotation3D rotation,
             final Point2D translation,
             final PinholeCameraIntrinsicParameters leftIntrinsic,
             final PinholeCameraIntrinsicParameters rightIntrinsic,
             final List<Point2D> leftPoints,
             final List<Point2D> rightPoints,
             final PinholeCamera estimatedLeftCamera,
             final PinholeCamera estimatedRightCamera,
             final List<Point3D> triangulatedPoints,
             final BitSet validTriangulatedPoints) throws WrongSizeException, CameraException, LockedException,
             NotReadyException, Point3DTriangulationException {
 
         if (triangulatedPoints != null) {
             triangulatedPoints.clear();
         }
         if (validTriangulatedPoints != null) {
             validTriangulatedPoints.clear();
         }
         var numValidTriangulatedPoints = 0;
 
         final var leftIntrinsicMatrix = leftIntrinsic.getInternalMatrix();
         final var rightIntrinsicMatrix = rightIntrinsic.getInternalMatrix();
 
         final var rotationMatrix = rotation.asInhomogeneousMatrix();
 
         // 1st camera
 
         // set camera as a canonical matrix
         final var tmp = Matrix.identity(PinholeCamera.PINHOLE_CAMERA_MATRIX_ROWS,
                 PinholeCamera.PINHOLE_CAMERA_MATRIX_COLS);
 
         // add intrinsic parameters
         leftIntrinsicMatrix.multiply(tmp);
 
         // set internal matrix, normalize and fix camera sign
         estimatedLeftCamera.setInternalMatrix(leftIntrinsicMatrix);
         estimatedLeftCamera.normalize();
         estimatedLeftCamera.fixCameraSign();
 
         // 2nd camera
 
         // set left 3x3 minor containing rotation
         tmp.setSubmatrix(0, 0, 2, 2, rotationMatrix);
 
         // set last column containing translation
         translation.normalize();
         tmp.setElementAt(0, 3, translation.getHomX());
         tmp.setElementAt(1, 3, translation.getHomY());
         tmp.setElementAt(2, 3, translation.getHomW());
 
         // add intrinsic parameters
         rightIntrinsicMatrix.multiply(tmp);
 
         // set internal matrix, normalize and fix camera sign
         estimatedRightCamera.setInternalMatrix(rightIntrinsicMatrix);
         estimatedRightCamera.normalize();
         estimatedRightCamera.fixCameraSign();
 
         // set cameras on triangulator
         final var triangulator = SinglePoint3DTriangulator.create();
 
         final var numPoints = leftPoints.size();
         Point2D leftPoint;
         Point2D rightPoint;
         final var points = new ArrayList<Point2D>();
         final var cameras = new ArrayList<PinholeCamera>();
         Point3D triangulatedPoint;
         boolean frontLeft;
         boolean frontRight;
         for (var i = 0; i < numPoints; i++) {
             leftPoint = leftPoints.get(i);
             rightPoint = rightPoints.get(i);
 
             points.clear();
             points.add(leftPoint);
             points.add(rightPoint);
 
             cameras.clear();
             cameras.add(estimatedLeftCamera);
             cameras.add(estimatedRightCamera);
 
             triangulator.setPointsAndCameras(points, cameras);
             triangulatedPoint = triangulator.triangulate();
             if (triangulatedPoints != null) {
                 triangulatedPoints.add(triangulatedPoint);
             }
 
             // check that triangulated point is in front of both cameras
             frontLeft = estimatedLeftCamera.isPointInFrontOfCamera(triangulatedPoint);
             frontRight = estimatedRightCamera.isPointInFrontOfCamera(triangulatedPoint);
 
             if (frontLeft && frontRight) {
                 // point is valid because it is in front of both cameras
                 if (validTriangulatedPoints != null) {
                     validTriangulatedPoints.set(i);
                 }
                 numValidTriangulatedPoints++;
             } else {
                 // point is not valid
                 if (validTriangulatedPoints != null) {
                     validTriangulatedPoints.clear(i);
                 }
             }
         }
 
         return numValidTriangulatedPoints;
     }
 }