/**
   * Finds a rotation matrix which is the optimal approximation to an arbitrary 3 by 3 matrix.
   * Optimality is specified by the equation below:<br>
   * <br>
   * min ||R-Q||<sup>2</sup><sub>F</sub><br>
   * R<br>
   * where R is the rotation matrix and Q is the matrix being approximated.
   *
   * <p>
   *
   * <p>The technique used is based on SVD and is described in Appendix C of "A Flexible New
   * Technique for Camera Calibration" Technical Report, updated 2002.
   *
   * <p>
   *
   * <p>Both origin and R can be the same instance.
   *
   * @param orig Input approximate rotation matrix. Not modified.
   * @param R (Optional) Storage for the approximated rotation matrix. Modified.
   * @return Rotation matrix
   */
  public static DenseMatrix64F approximateRotationMatrix(DenseMatrix64F orig, DenseMatrix64F R) {
    R = checkDeclare3x3(R);

    SingularValueDecomposition<DenseMatrix64F> svd =
        DecompositionFactory.svd(orig.numRows, orig.numCols, true, true, false);

    if (!svd.decompose(orig)) throw new RuntimeException("SVD Failed");

    CommonOps.mult(svd.getU(null, false), svd.getV(null, true), R);

    // svd does not guarantee that U anv V have positive determinants.
    float det = (float) CommonOps.det(R);

    if (det < 0) CommonOps.scale(-1, R);

    return R;
  }
  /**
   * Computes a basis (the principle components) from the most dominant eigenvectors.
   *
   * @param numComponents Number of vectors it will use to describe the data. Typically much smaller
   *     than the number of elements in the input vector.
   */
  public void computeBasis(int numComponents) {
    if (numComponents > A.getNumCols())
      throw new IllegalArgumentException("More components requested that the data's length.");
    if (sampleIndex != A.getNumRows())
      throw new IllegalArgumentException("Not all the data has been added");
    if (numComponents > sampleIndex)
      throw new IllegalArgumentException(
          "More data needed to compute the desired number of components");

    this.numComponents = numComponents;

    // compute the mean of all the samples
    for (int i = 0; i < A.getNumRows(); i++) {
      for (int j = 0; j < mean.length; j++) {
        mean[j] += A.get(i, j);
      }
    }
    for (int j = 0; j < mean.length; j++) {
      mean[j] /= A.getNumRows();
    }

    // subtract the mean from the original data
    for (int i = 0; i < A.getNumRows(); i++) {
      for (int j = 0; j < mean.length; j++) {
        A.set(i, j, A.get(i, j) - mean[j]);
      }
    }

    // Compute SVD and save time by not computing U
    SingularValueDecomposition<DenseMatrix64F> svd =
        DecompositionFactory.svd(A.numRows, A.numCols, false, true, false);
    if (!svd.decompose(A)) throw new RuntimeException("SVD failed");

    V_t = svd.getV(null, true);
    DenseMatrix64F W = svd.getW(null);

    // Singular values are in an arbitrary order initially
    SingularOps.descendingOrder(null, false, W, V_t, true);

    // strip off unneeded components and find the basis
    V_t.reshape(numComponents, mean.length, true);
  }
/**
 * Parametrization for Bundle Adjustment with known calibration where the rotation matrix is encoded
 * using {@link Rodrigues} coordinates.
 *
 * @author Peter Abeles
 */
public class CalibPoseAndPointRodriguesCodec implements ModelCodec<CalibratedPoseAndPoint> {
  // number of camera views
  int numViews;
  // number of points in world coordinates
  int numPoints;
  // number of views with unknown extrinsic parameters
  int numViewsUnknown;

  // indicates if a view is known or not
  boolean knownView[];

  // storage
  Rodrigues rotation = new Rodrigues();
  DenseMatrix64F R = new DenseMatrix64F(3, 3);

  // used to make sure the rotation matrix is in SO(3)
  SingularValueDecomposition<DenseMatrix64F> svd =
      DecompositionFactory.svd(3, 3, true, true, false);

  /** Specify the number of views and points it can expected */
  public void configure(int numViews, int numPoints, int numViewsUnknown, boolean[] knownView) {
    if (numViews < knownView.length)
      throw new IllegalArgumentException("Number of views is less than knownView length");

    this.numViews = numViews;
    this.numPoints = numPoints;
    this.numViewsUnknown = numViewsUnknown;
    this.knownView = knownView;
  }

  @Override
  public void decode(double[] param, CalibratedPoseAndPoint outputModel) {

    int paramIndex = 0;

    // first decode the transformation
    for (int i = 0; i < numViews; i++) {
      // don't decode if it is already known
      if (knownView[i]) continue;

      Se3_F64 se = outputModel.getWorldToCamera(i);

      rotation.setParamVector(param[paramIndex++], param[paramIndex++], param[paramIndex++]);

      RotationMatrixGenerator.rodriguesToMatrix(rotation, se.getR());

      Vector3D_F64 T = se.getT();
      T.x = param[paramIndex++];
      T.y = param[paramIndex++];
      T.z = param[paramIndex++];
    }

    // now decode the points
    for (int i = 0; i < numPoints; i++) {
      Point3D_F64 p = outputModel.getPoint(i);
      p.x = param[paramIndex++];
      p.y = param[paramIndex++];
      p.z = param[paramIndex++];
    }
  }

  @Override
  public void encode(CalibratedPoseAndPoint model, double[] param) {
    int paramIndex = 0;

    // first decode the transformation
    for (int i = 0; i < numViews; i++) {
      // don't encode if it is already known
      if (knownView[i]) continue;

      Se3_F64 se = model.getWorldToCamera(i);

      // force the "rotation matrix" to be an exact rotation matrix
      // otherwise Rodrigues will have issues with the noise
      if (!svd.decompose(se.getR())) throw new RuntimeException("SVD failed");

      DenseMatrix64F U = svd.getU(null, false);
      DenseMatrix64F V = svd.getV(null, false);

      CommonOps.multTransB(U, V, R);

      // extract Rodrigues coordinates
      RotationMatrixGenerator.matrixToRodrigues(R, rotation);

      param[paramIndex++] = rotation.unitAxisRotation.x * rotation.theta;
      param[paramIndex++] = rotation.unitAxisRotation.y * rotation.theta;
      param[paramIndex++] = rotation.unitAxisRotation.z * rotation.theta;

      Vector3D_F64 T = se.getT();

      param[paramIndex++] = T.x;
      param[paramIndex++] = T.y;
      param[paramIndex++] = T.z;
    }

    for (int i = 0; i < numPoints; i++) {
      Point3D_F64 p = model.getPoint(i);

      param[paramIndex++] = p.x;
      param[paramIndex++] = p.y;
      param[paramIndex++] = p.z;
    }
  }

  @Override
  public int getParamLength() {
    return numViewsUnknown * 6 + numPoints * 3;
  }
}
 public TrifocalExtractEpipoles() {
   svd = DecompositionFactory.svd(3, 3, true, true, true);
   svd = new SafeSvd(svd);
 }
  /**
   * Creates a new solver targeted at the specified matrix size.
   *
   * @param maxRows The expected largest matrix it might have to process. Can be larger.
   * @param maxCols The expected largest matrix it might have to process. Can be larger.
   */
  public SolvePseudoInverseSvd(int maxRows, int maxCols) {

    svd = DecompositionFactory.svd(maxRows, maxCols, true, true, true);
  }