/*
    * 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.epipolar.estimators;
  
  import com.irurueta.algebra.AlgebraException;
  import com.irurueta.algebra.Matrix;
  import com.irurueta.algebra.SingularValueDecomposer;
  import com.irurueta.geometry.EuclideanTransformation3D;
  import com.irurueta.geometry.InvalidRotationMatrixException;
  import com.irurueta.geometry.MatrixRotation3D;
  import com.irurueta.geometry.PinholeCameraIntrinsicParameters;
  import com.irurueta.geometry.Transformation2D;
  import com.irurueta.geometry.estimators.LockedException;
  import com.irurueta.geometry.estimators.NotReadyException;
  
  import java.util.ArrayList;
  import java.util.List;
  
  /**
   * Decomposes a 2D homography to extract its internal geometry structure. There
   * are four possible solutions, with two that are physically possible. The
   * physically possible solution can be found by imposing a positive depth
   * constraint (reconstructed points must lie in front of the cameras).
   * An homography matrix is defined as H = (R + (1/d)*T*N<sup>T</sup>), where R
   * is a 3x3 rotation matrix, d is the distance of the plane, N is the plane's
   * normal, T is the translation vector. The decomposition works by computing the
   * SVD of H<sup>T</sup>H and then following the procedure defined in
   * O. Faugeras, Motion and structure from motion in a piecewise planar
   * environment.
   */
  @SuppressWarnings("DuplicatedCode")
  public class HomographyDecomposer {
  
      /**
       * Number of inhomogeneous coordinates in 3D.
       */
      public static final int NUM_COORDS_3D = 3;
  
      /**
       * Threshold to determine that two singular values are equal.
       */
      public static final double EQUAL_SINGULAR_VALUE_THRESHOLD = 1e-12;
  
      /**
       * 2D transformation relating two views (left view to right view).
       */
      private Transformation2D homography;
  
      /**
       * Intrinsic parameters to be used on left view.
       */
      private PinholeCameraIntrinsicParameters leftIntrinsics;
  
      /**
       * Intrinsic parameters to be used on right view.
       */
      private PinholeCameraIntrinsicParameters rightIntrinsics;
  
      /**
       * Listener to handle events raised by this instance.
       */
      private HomographyDecomposerListener listener;
  
      /**
       * Indicates whether decomposer is locked while computing decomposition.
       */
      private boolean locked;
  
      /**
       * Constructor.
       */
      public HomographyDecomposer() {
      }
  
      /**
       * Constructor.
       *
       * @param homography      2D transformation relating two views (left view to
       *                        right view).
       * @param leftIntrinsics  intrinsic parameters to be used on left view.
       * @param rightIntrinsics intrinsic parameters to be used on right view.
       */
      public HomographyDecomposer(final Transformation2D homography,
                                  final PinholeCameraIntrinsicParameters leftIntrinsics,
                                  final PinholeCameraIntrinsicParameters rightIntrinsics) {
          this.homography = homography;
         this.leftIntrinsics = leftIntrinsics;
         this.rightIntrinsics = rightIntrinsics;
     }
 
     /**
      * Constructor.
      *
      * @param homography      2D transformation relating two views (left view to
      *                        right view).
      * @param leftIntrinsics  intrinsic parameters to be used on left view.
      * @param rightIntrinsics intrinsic parameters to be used on right view.
      * @param listener        listener to attend events generated by this instance.
      */
     public HomographyDecomposer(final Transformation2D homography,
                                 final PinholeCameraIntrinsicParameters leftIntrinsics,
                                 final PinholeCameraIntrinsicParameters rightIntrinsics,
                                 final HomographyDecomposerListener listener) {
         this(homography, leftIntrinsics, rightIntrinsics);
         this.listener = listener;
     }
 
     /**
      * Gets 2D transformation relating two views (left view to right view).
      *
      * @return 2D transformation relating two views.
      */
     public Transformation2D getHomography() {
         return homography;
     }
 
     /**
      * Sets 2D transformation relating two views (left view to right view).
      *
      * @param homography 2D transformation relating two views.
      * @throws LockedException if estimator is locked.
      */
     public void setHomography(final Transformation2D homography) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         this.homography = homography;
     }
 
     /**
      * Gets intrinsic parameters to be used on left view.
      *
      * @return intrinsic parameters to be used on left view.
      */
     public PinholeCameraIntrinsicParameters getLeftIntrinsics() {
         return leftIntrinsics;
     }
 
     /**
      * Sets intrinsic parameters to be used on left view.
      *
      * @param leftIntrinsics intrinsic parameters to be used on left view.
      * @throws LockedException if estimator is locked.
      */
     public void setLeftIntrinsics(final PinholeCameraIntrinsicParameters leftIntrinsics) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         this.leftIntrinsics = leftIntrinsics;
     }
 
     /**
      * Gets intrinsic parameters to be used on right view.
      *
      * @return intrinsic parameters to be used on right view.
      */
     public PinholeCameraIntrinsicParameters getRightIntrinsics() {
         return rightIntrinsics;
     }
 
     /**
      * Sets intrinsic parameters to be used on right view.
      *
      * @param rightIntrinsics intrinsic parameters to be used on right view.
      * @throws LockedException if estimator is locked.
      */
     public void setRightIntrinsics(final PinholeCameraIntrinsicParameters rightIntrinsics) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         this.rightIntrinsics = rightIntrinsics;
     }
 
     /**
      * Gets listener to handle events raised by this instance.
      *
      * @return listener to handle events raised by this instance.
      */
     public HomographyDecomposerListener getListener() {
         return listener;
     }
 
     /**
      * Sets listener to handle events raised by this instance.
      *
      * @param listener listener to handle events raised by this instance.
      */
     public void setListener(final HomographyDecomposerListener listener) {
         this.listener = listener;
     }
 
     /**
      * Indicates whether estimator is locked while computing decomposition.
      *
      * @return true if decomposer is locked, false otherwise.
      */
     public boolean isLocked() {
         return locked;
     }
 
     /**
      * Indicates whether decomposer is ready to start the decomposition when all
      * required data has been provided.
      *
      * @return true if decomposer is ready, false otherwise.
      */
     public boolean isReady() {
         return homography != null && leftIntrinsics != null && rightIntrinsics != null;
     }
 
     /**
      * Decomposes homography into possible solutions containing possible 3D
      * rotation, 3D translation and normal and distance of the plane relating
      * two views via provided homography.
      *
      * @return possible solutions.
      * @throws LockedException               if decomposer is locked.
      * @throws NotReadyException             if decomposer is not ready.
      * @throws HomographyDecomposerException if decomposition fails for some
      *                                       other reason (i.e. numerical instabilities).
      */
     public List<HomographyDecomposition> decompose() throws LockedException, NotReadyException,
             HomographyDecomposerException {
         final var result = new ArrayList<HomographyDecomposition>();
         decompose(result);
         return result;
     }
 
     /**
      * Decomposes homography into possible solutions containing possible 3D
      * rotation, 3D translation and normal and distance of the plane relating
      * two views via provided homography.
      *
      * @param result instance where possible solutions will be stored.
      * @throws LockedException               if decomposer is locked.
      * @throws NotReadyException             if decomposer is not ready.
      * @throws HomographyDecomposerException if decomposition fails for some
      *                                       other reason (i.e. numerical instabilities).
      */
     public void decompose(final List<HomographyDecomposition> result) throws LockedException, NotReadyException,
             HomographyDecomposerException {
         if (isLocked()) {
             throw new LockedException();
         }
 
         if (!isReady()) {
             throw new NotReadyException();
         }
 
         try {
             locked = true;
             result.clear();
 
             if (listener != null) {
                 listener.onDecomposeStart(this);
             }
 
             final var h = computeNormalizedCoordinatesHomographyMatrix();
 
             // Homography matrix H can be expressed as:
             // H = d*R + t*n^T
             // where d is the distance to the plane used to relate both views, R
             // is a rotation relating both views so that R*R^T = I and
             // det(R) = 1, t is the translation relating both views and n is the
             // normal of the plane relating both views.
 
             // By using SVD decomposition, we get:
             // H = U*A'*V^T
 
             // where U and V are orthonormal matrices and A' is a diagonal matrix
             // containing singular values in decreasing order d1 >= d2 >= d3
 
             // Hence A' can also be expressed similarly to H as:
             // A' = d'*R' + t'*n'^T, and the relation to d, R, t and n is as
             // follows:
             // H = U*(d'*R' + t'*n'^T)*V^T = d'*U*R'*V^T + U*t'*n'^T*V^T
             // d = d'
             // R = U*R'*V^T
             // t = U*t'
             // n^T = n'T*V^T --> n = V*n'
 
             final var svdDecomposer = new SingularValueDecomposer(h);
             svdDecomposer.decompose();
             final var u = svdDecomposer.getU();
             final var singularValues = svdDecomposer.getSingularValues();
             final var v = svdDecomposer.getV();
 
             final var n = new ArrayList<double[]>();
             final var r = new ArrayList<Matrix>();
             final var t = new ArrayList<double[]>();
             final var d = new ArrayList<Double>();
             final var numSolutions = decomposeAllFromSingularValues(singularValues, n, r, t, d);
 
             final var transV = v.transposeAndReturnNew();
             final var translationMatrix = new Matrix(NUM_COORDS_3D, 1);
             final var planeNormalMatrix = new Matrix(NUM_COORDS_3D, 1);
 
             for (var i = 0; i < numSolutions; i++) {
                 // undo U, V decomposition
 
                 // R = U*R'*V^T
                 final var denormalizedR = new Matrix(u);
                 denormalizedR.multiply(r.get(i));
                 denormalizedR.multiply(transV);
 
                 // t = U*t'
                 translationMatrix.fromArray(t.get(i), true);
                 final var denormalizedTranslationMatrix = u.multiplyAndReturnNew(translationMatrix);
 
                 // n = V*n'
                 planeNormalMatrix.fromArray(n.get(i));
                 final var denormalizedPlaneNormalMatrix = v.multiplyAndReturnNew(planeNormalMatrix);
 
                 final var denormalizedPlaneDistance = d.get(i);
 
                 // rotation
                 final var denormalizedRotation = new MatrixRotation3D(denormalizedR);
                 final var denormalizedTranslation = denormalizedTranslationMatrix.getBuffer();
                 final var denormalizedPlaneNormal = denormalizedPlaneNormalMatrix.getBuffer();
 
                 // set rotation and translation
                 final var transformation = new EuclideanTransformation3D(denormalizedRotation, denormalizedTranslation);
 
                 result.add(new HomographyDecomposition(transformation, denormalizedPlaneNormal,
                         denormalizedPlaneDistance));
             }
         } catch (final InvalidRotationMatrixException | AlgebraException e) {
             throw new HomographyDecomposerException(e);
         } finally {
             if (listener != null) {
                 listener.onDecomposeEnd(this, result);
             }
             locked = false;
         }
     }
 
     /**
      * Decompose solutions from singular values.
      *
      * @param singularValues input singular values.
      * @param n              list containing possible plane normals.
      * @param r              list containing possible camera rotations.
      * @param t              list containing possible camera translations.
      * @param d              list of distances to plane.
      * @return number of solutions.
      * @throws HomographyDecomposerException if decomposition fails (i.e. numerical instabilities, etc).
      */
     private int decomposeAllFromSingularValues(
             final double[] singularValues,
             final List<double[]> n,
             final List<Matrix> r,
             final List<double[]> t,
             final List<Double> d)
             throws HomographyDecomposerException {
 
         n.clear();
         r.clear();
         t.clear();
         d.clear();
 
         if (areThreeDifferentSingularValues(singularValues)) {
             // Three different singular values
             final var n1 = new double[NUM_COORDS_3D];
             final var n2 = new double[NUM_COORDS_3D];
             final var n3 = new double[NUM_COORDS_3D];
             final var n4 = new double[NUM_COORDS_3D];
             Matrix r1 = null;
             Matrix r2 = null;
             Matrix r3 = null;
             Matrix r4 = null;
             try {
                 r1 = new Matrix(NUM_COORDS_3D, NUM_COORDS_3D);
                 r2 = new Matrix(NUM_COORDS_3D, NUM_COORDS_3D);
                 r3 = new Matrix(NUM_COORDS_3D, NUM_COORDS_3D);
                 r4 = new Matrix(NUM_COORDS_3D, NUM_COORDS_3D);
             } catch (final AlgebraException ignore) {
                 // never thrown
             }
 
             final var t1 = new double[NUM_COORDS_3D];
             final var t2 = new double[NUM_COORDS_3D];
             final var t3 = new double[NUM_COORDS_3D];
             final var t4 = new double[NUM_COORDS_3D];
 
             final var planeDistance1 = decomposeFromSingularValues(singularValues, n1, r1, t1, true,
                     true);
             final var planeDistance2 = decomposeFromSingularValues(singularValues, n2, r2, t2, false,
                     true);
             final var planeDistance3 = decomposeFromSingularValues(singularValues, n3, r3, t3, true,
                     false);
             final var planeDistance4 = decomposeFromSingularValues(singularValues, n4, r4, t4, false,
                     false);
 
             n.add(n1);
             n.add(n2);
             n.add(n3);
             n.add(n4);
 
             r.add(r1);
             r.add(r2);
             r.add(r3);
             r.add(r4);
 
             t.add(t1);
             t.add(t2);
             t.add(t3);
             t.add(t4);
 
             d.add(planeDistance1);
             d.add(planeDistance2);
             d.add(planeDistance3);
             d.add(planeDistance4);
 
             return n.size();
 
         } else if (areTwoEqualSingularValues(singularValues)) {
             // Two different singular values
             final var n1 = new double[NUM_COORDS_3D];
             final var n2 = new double[NUM_COORDS_3D];
             Matrix r1 = null;
             Matrix r2 = null;
             try {
                 r1 = new Matrix(NUM_COORDS_3D, NUM_COORDS_3D);
                 r2 = new Matrix(NUM_COORDS_3D, NUM_COORDS_3D);
             } catch (final AlgebraException ignore) {
                 // never thrown
             }
 
             final var t1 = new double[NUM_COORDS_3D];
             final var t2 = new double[NUM_COORDS_3D];
 
             final var planeDistance1 = decomposeFromSingularValues(singularValues, n1, r1, t1, true,
                     true);
             final var planeDistance2 = decomposeFromSingularValues(singularValues, n2, r2, t2, true,
                     false);
 
             n.add(n1);
             n.add(n2);
 
             r.add(r1);
             r.add(r2);
 
             t.add(t1);
             t.add(t2);
 
             d.add(planeDistance1);
             d.add(planeDistance2);
 
             return n.size();
 
         } else {
             // Three equal singular values
             throw new HomographyDecomposerException("undefined plane normal");
         }
     }
 
     /**
      * Determines whether there are three different singular values or not.
      *
      * @param singularValues singular values to be checked.
      * @return true if there are three different singular values, false
      * otherwise.
      */
     private static boolean areThreeDifferentSingularValues(final double[] singularValues) {
         final var d1 = singularValues[0];
         final var d2 = singularValues[1];
         final var d3 = singularValues[2];
 
         return (Math.abs(d1 - d2) > EQUAL_SINGULAR_VALUE_THRESHOLD)
                 && (Math.abs(d2 - d3) > EQUAL_SINGULAR_VALUE_THRESHOLD);
     }
 
     /**
      * Determines whether there are two equal singular values or not.
      *
      * @param singularValues singular values to be checked.
      * @return true if there are two equal singular values, false otherwise.
      */
     private static boolean areTwoEqualSingularValues(final double[] singularValues) {
         final var d1 = singularValues[0];
         final var d2 = singularValues[1];
         final var d3 = singularValues[2];
 
         return ((Math.abs(d1 - d2) <= EQUAL_SINGULAR_VALUE_THRESHOLD)
                 && (Math.abs(d2 - d3) > EQUAL_SINGULAR_VALUE_THRESHOLD))
                 || ((Math.abs(d1 - d2) > EQUAL_SINGULAR_VALUE_THRESHOLD)
                 && (Math.abs(d2 - d3) <= EQUAL_SINGULAR_VALUE_THRESHOLD));
     }
 
     /**
      * Decomposes one possible solution using provided singular values and signs
      *
      * @param singularValues singular values to use for
      * @param n              array where plane normal will be store.
      * @param r              matrix where rotation will be stored.
      * @param t              array where translation will be stored.
      * @param positive1      sign of 1st coordinate of plane normal.
      * @param positive3      sign of 2nd coordinate of plane normal.
      * @return plane distance.
      * @throws HomographyDecomposerException if decomposition is undetermined
      *                                       when all three singular values are equal.
      */
     private double decomposeFromSingularValues(
             final double[] singularValues, final double[] n, final Matrix r, final double[] t, final boolean positive1,
             final boolean positive3) throws HomographyDecomposerException {
         final var d1 = singularValues[0];
         final var d2 = singularValues[1];
         final var d3 = singularValues[2];
 
         if (areThreeDifferentSingularValues(singularValues)) {
             // Three different singular values
             if (d2 > 0.0) {
                 // Three different singular values d1 != d2 != d3 and d'= d2 > 0
                 return decomposeFromThreeDifferentSingularValuesPositive(d1, d2, d3, n, r, t, positive1, positive3);
             } else {
                 // Three different singular values and d' = d2 < 0
                 return decomposeFromThreeDifferentSingularValuesNegative(d1, d2, d3, n, r, t, positive1, positive3);
             }
         }
         if (areTwoEqualSingularValues(singularValues)) {
             // Two different singular values
             if (d2 > 0.0) {
                 // Two different singular values d1 = d2 != d3 or d1 != d2 = d3
                 // and d2 > 0
                 return decomposeFromTwoDifferentSingularValuesPositive(d1, d2, d3, n, r, t, positive3);
             } else {
                 // Two different singular values d1 = d2 != d3 or d1 != d2 = d3
                 // and d2 < 0
                 return decomposeFromTwoDifferentSingularValuesNegative(d1, d2, d3, n, r, t, positive3);
             }
         } else {
             // Three equal singular values
             throw new HomographyDecomposerException("undefined plane normal");
         }
     }
 
     /**
      * Generates one homography decomposition using provided singular values and
      * assuming that there are two equal singular values and that d2 is
      * negative.
      *
      * @param d1        1st singular value.
      * @param d2        2nd singular value.
      * @param d3        3rd singular value.
      * @param n         plane normal.
      * @param r         rotation.
      * @param t         translation.
      * @param positive3 true to assume positive x3, false otherwise.
      * @return distance to plane.
      */
     private double decomposeFromTwoDifferentSingularValuesNegative(
             final double d1, final double d2, final double d3, final double[] n, final Matrix r, final double[] t,
             final boolean positive3) {
 
         // fill plane normal solution
         n[0] = 0.0;
         n[1] = 0.0;
         n[2] = positive3 ? 1.0 : -1.0;
 
         // compute rotation
 
         // fill rotation matrix
         r.setElementAt(0, 0, -1.0);
         r.setElementAt(1, 0, 0.0);
         r.setElementAt(2, 0, 0.0);
 
         r.setElementAt(0, 1, 0.0);
         r.setElementAt(1, 1, -1.0);
         r.setElementAt(2, 1, 0.0);
 
         r.setElementAt(0, 2, 0.0);
         r.setElementAt(1, 2, 0.0);
         r.setElementAt(2, 2, 1.0);
 
         // compute translation
         final var sum = d3 + d1;
         t[0] = 0.0;
         t[1] = 0.0;
         t[2] = sum * n[2];
 
         // plane distance
         return d2;
     }
 
     /**
      * Generates one homography decomposition using provided singular values and
      * assuming that there are two equal singular values and that d2 is
      * positive.
      *
      * @param d1        1st singular value.
      * @param d2        2nd singular value.
      * @param d3        3rd singular value.
      * @param n         plane normal.
      * @param r         rotation.
      * @param t         translation.
      * @param positive3 true to assume positive x3, false otherwise.
      * @return distance to plane.
      */
     private double decomposeFromTwoDifferentSingularValuesPositive(
             final double d1, final double d2, final double d3, final double[] n, final Matrix r, final double[] t,
             final boolean positive3) {
 
         // fill plane normal solution
         n[0] = 0.0;
         n[1] = 0.0;
         n[2] = positive3 ? 1.0 : -1.0;
 
         // compute rotation
 
         //fill rotation matrix
         r.setElementAt(0, 0, 1.0);
         r.setElementAt(1, 0, 0.0);
         r.setElementAt(2, 0, 0.0);
 
         r.setElementAt(0, 1, 0.0);
         r.setElementAt(1, 1, 1.0);
         r.setElementAt(2, 1, 0.0);
 
         r.setElementAt(0, 2, 0.0);
         r.setElementAt(1, 2, 0.0);
         r.setElementAt(2, 2, 1.0);
 
         // compute translation
         final var diff = d1 - d3;
         t[0] = 0.0;
         t[1] = 0.0;
         t[2] = -diff * n[2];
 
         // plane distance
         return d2;
     }
 
     /**
      * Generates one homography decomposition using provided singular values and
      * assuming that there are three different singular values and that d2 is
      * negative.
      *
      * @param d1        1st singular value.
      * @param d2        2nd singular value.
      * @param d3        3rd singular value.
      * @param n         plane normal.
      * @param r         rotation.
      * @param t         translation.
      * @param positive1 true to assume positive x1, false otherwise.
      * @param positive3 true to assume positive x3, false otherwise.
      * @return distance to plane.
      */
     private double decomposeFromThreeDifferentSingularValuesNegative(
             final double d1, final double d2, final double d3, final double[] n, final Matrix r, final double[] t,
             final boolean positive1, final boolean positive3) {
         final var d1Sqr = d1 * d1;
         final var d2Sqr = d2 * d2;
         final var d3Sqr = d3 * d3;
 
         // compute plane normal
         final var denom = d1Sqr - d3Sqr;
 
         var x1 = Math.sqrt((d1Sqr - d2Sqr) / denom);
         if (!positive1) {
             x1 = -x1;
         }
 
         final var x2 = 0.0;
 
         var x3 = Math.sqrt((d2Sqr - d3Sqr) / denom);
         if (!positive3) {
             x3 = -x3;
         }
 
         // fill plane normal solution
         n[0] = x1;
         n[1] = x2;
         n[2] = x3;
 
         // compute rotation
         final var x1Sqr = x1 * x1;
         final var x3Sqr = x3 * x3;
 
         final var sinTheta = (d1 + d3) * x1 * x3 / d2;
         final var cosTheta = (d3 * x1Sqr - d1 * x3Sqr) / d2;
 
         // fill rotation matrix
         r.setElementAt(0, 0, cosTheta);
         r.setElementAt(1, 0, 0.0);
         r.setElementAt(2, 0, sinTheta);
 
         r.setElementAt(0, 1, 0.0);
         r.setElementAt(1, 1, 1.0);
         r.setElementAt(2, 1, 0.0);
 
         r.setElementAt(0, 2, -sinTheta);
         r.setElementAt(1, 2, 0.0);
         r.setElementAt(2, 2, cosTheta);
 
         // compute translation
         final var sum = d1 + d3;
         t[0] = sum * x1;
         t[1] = 0.0;
         t[2] = sum * x3;
 
         // plane distance
         return d2;
     }
 
     /**
      * Generates one homography decomposition using provided singular values and
      * assuming that there are three different singular values and that d2 is
      * positive.
      *
      * @param d1        1st singular value.
      * @param d2        2nd singular value.
      * @param d3        3rd singular value.
      * @param n         plane normal.
      * @param r         rotation.
      * @param t         translation.
      * @param positive1 true to assume positive x1, false otherwise.
      * @param positive3 true to assume positive x3, false otherwise.
      * @return distance to plane.
      */
     private double decomposeFromThreeDifferentSingularValuesPositive(
             final double d1, final double d2, final double d3, final double[] n, final Matrix r, final double[] t,
             final boolean positive1, final boolean positive3) {
         final var d1Sqr = d1 * d1;
         final var d2Sqr = d2 * d2;
         final var d3Sqr = d3 * d3;
 
         // compute plane normal
         final var denom = d1Sqr - d3Sqr;
 
         var x1 = Math.sqrt((d1Sqr - d2Sqr) / denom);
         if (!positive1) {
             x1 = -x1;
         }
 
         final var x2 = 0.0;
 
         var x3 = Math.sqrt((d2Sqr - d3Sqr) / denom);
         if (!positive3) {
             x3 = -x3;
         }
 
         // fill plane normal solution
         n[0] = x1;
         n[1] = x2;
         n[2] = x3;
 
         // compute rotation
         final var x1Sqr = x1 * x1;
         final var x3Sqr = x3 * x3;
 
         final var sinTheta = (d1 - d3) * x1 * x3 / d2;
         final var cosTheta = (d1 * x3Sqr + d3 * x1Sqr) / d2;
 
         // fill rotation matrix
         r.setElementAt(0, 0, cosTheta);
         r.setElementAt(1, 0, 0.0);
         r.setElementAt(2, 0, sinTheta);
 
         r.setElementAt(0, 1, 0.0);
         r.setElementAt(1, 1, 1.0);
         r.setElementAt(2, 1, 0.0);
 
         r.setElementAt(0, 2, -sinTheta);
         r.setElementAt(1, 2, 0.0);
         r.setElementAt(2, 2, cosTheta);
 
 
         // compute translation
         final var diff = d1 - d3;
         t[0] = diff * x1;
         t[1] = 0.0;
         t[2] = -diff * x3;
 
         // plane distance
         return d2;
     }
 
     /**
      * Computes homography matrix in terms of normalized point coordinates by
      * taking into account intrinsic camera parameters on left and right views.
      *
      * @return normalized homography matrix
      * @throws AlgebraException if there are numerical instabilities.
      */
     private Matrix computeNormalizedCoordinatesHomographyMatrix() throws AlgebraException {
         // we know that point p1 in the left view is related to point p2
         // in the right view by homography G so that:
         // p2 = G*p1
         // where:
         // p1 = K1*m1
         // p2 = K2*m2
         // where K1 and K2 are intrinsic parameters on left and right views
         // and m1, m2 are normalized point coordinates on left and right
         // views.
         // Hence:
         // K2*m2 = G*K1*m1 --> m2 = K2^-1*G*K1*m1
         // and so we obtain:
         // H = K2^-1*G*K1, which is an homography in normalized coordinates
 
         final var k1 = leftIntrinsics.getInternalMatrix();
         final var invK2 = rightIntrinsics.getInverseInternalMatrix();
         final var g = homography.asMatrix();
 
         // compute H = K2^-1*G*K1
         g.multiply(k1);
         invK2.multiply(g);
         return invK2;
     }
 }