/**
   * Extracts the epipoles from the trifocal tensor. Extracted epipoles will have a norm of 1 as an
   * artifact of using SVD.
   *
   * @param tensor Input: Trifocal tensor. Not Modified
   * @param e2 Output: Epipole in image 2. Homogeneous coordinates. Modified
   * @param e3 Output: Epipole in image 3. Homogeneous coordinates. Modified
   */
  public void process(TrifocalTensor tensor, Point3D_F64 e2, Point3D_F64 e3) {
    svd.decompose(tensor.T1);
    SingularOps.nullVector(svd, true, v1);
    SingularOps.nullVector(svd, false, u1);

    svd.decompose(tensor.T2);
    SingularOps.nullVector(svd, true, v2);
    SingularOps.nullVector(svd, false, u2);

    svd.decompose(tensor.T3);
    SingularOps.nullVector(svd, true, v3);
    SingularOps.nullVector(svd, false, u3);

    for (int i = 0; i < 3; i++) {
      U.set(i, 0, u1.get(i));
      U.set(i, 1, u2.get(i));
      U.set(i, 2, u3.get(i));

      V.set(i, 0, v1.get(i));
      V.set(i, 1, v2.get(i));
      V.set(i, 2, v3.get(i));
    }

    svd.decompose(U);
    SingularOps.nullVector(svd, false, tempE);
    e2.set(tempE.get(0), tempE.get(1), tempE.get(2));

    svd.decompose(V);
    SingularOps.nullVector(svd, false, tempE);
    e3.set(tempE.get(0), tempE.get(1), tempE.get(2));
  }
  @DeployableTestMethod(estimatedDuration = 0.0)
  @Test(timeout = 30000)
  public void testConstrainedSimple() {
    int objectiveSize = 3;
    int solutionSize = 3;
    int constraintSize = 1;

    DenseMatrix64F a = new DenseMatrix64F(objectiveSize, solutionSize);
    CommonOps.setIdentity(a);

    DenseMatrix64F b = new DenseMatrix64F(objectiveSize, 1);
    b.zero();

    DenseMatrix64F c = new DenseMatrix64F(constraintSize, solutionSize);
    CommonOps.setIdentity(c);

    DenseMatrix64F d = new DenseMatrix64F(constraintSize, 1);
    d.set(0, 0, -1.0);

    QuadraticProgram quadraticProgram = new QuadraticProgram(a, b, c, d);

    DenseMatrix64F initialGuess = new DenseMatrix64F(solutionSize, 1);
    initialGuess.set(0, 0, -10.0);
    initialGuess.set(1, 0, -10.0);
    initialGuess.set(2, 0, -10.0);
    ActiveSearchSolutionInfo solutionInfo = solve(quadraticProgram, initialGuess);

    assertTrue(solutionInfo.isConverged());

    DenseMatrix64F expectedResult = new DenseMatrix64F(solutionSize, 1);
    expectedResult.set(0, 0, d.get(0, 0));
    expectedResult.set(1, 0, 0.0);
    expectedResult.set(2, 0, 0.0);
    assertTrue(MatrixFeatures.isEquals(expectedResult, solutionInfo.getSolution(), 1e-12));
  }
  private void compute(ValkyrieJointInterface[] jnt_data) {
    double q1 = jnt_data[1].getPosition();
    double q2 = jnt_data[0].getPosition();

    double cq1 = Math.cos(q1);
    double sq1 = Math.sin(q1);

    double cq2 = Math.cos(q2);
    double sq2 = Math.sin(q2);

    double c2q1 = Math.cos(2.0 * q1);
    double c2q2 = Math.cos(2.0 * q2);
    double s2q1 = Math.sin(2.0 * q1);

    double a1 = computeALeft();
    double b1 = computeBLeft(cq1, sq1, cq2, sq2);
    double c1 = computeCLeft(cq1, sq1, cq2, sq2);

    double a2 = computeARight();
    double b2 = computeBRight(cq1, sq1, cq2, sq2);
    double c2 = computeCRight(cq1, sq1, cq2, sq2);

    double Db1Dq1 = computeDb1Dq1(cq1, sq1, sq2);
    double Db1Dq2 = computeDb1Dq2(cq1, cq2, sq2);
    double Dc1Dq1 = computeDc1Dq1(cq1, sq1, sq2, c2q1, c2q2, s2q1);
    double Dc1Dq2 = computeDc1Dq2(cq1, sq1, cq2, sq2, c2q1, s2q1);

    double Db2Dq1 = computeDb2Dq1(cq1, sq1, sq2);
    double Db2Dq2 = computeDb1Dq2(cq1, cq2, sq2);
    double Dc2Dq1 = computeDc2Dq1(cq1, sq1, sq2, c2q1, c2q2, s2q1);
    double Dc2Dq2 = computeDc2Dq2(cq1, sq1, cq2, sq2, c2q1);

    double x1 = computePushrodPosition(a1, b1, c1);
    double x2 = computePushrodPosition(a2, b2, c2);
    pushrodPositions.set(0, 0, x1);
    pushrodPositions.set(1, 0, x2);

    double velocityMap00 = computeVelocityMapCoefficient(Db1Dq1, a1, b1, Dc1Dq1, c1);
    double velocityMap01 = computeVelocityMapCoefficient(Db1Dq2, a1, b1, Dc1Dq2, c1);
    double velocityMap10 = computeVelocityMapCoefficient(Db2Dq1, a2, b2, Dc2Dq1, c2);
    double velocityMap11 = computeVelocityMapCoefficient(Db2Dq2, a2, b2, Dc2Dq2, c2);
    jacobian.set(0, 0, velocityMap00);
    jacobian.set(0, 1, velocityMap01);
    jacobian.set(1, 0, velocityMap10);
    jacobian.set(1, 1, velocityMap11);

    double pushrodForceToJointTorqueMap00 =
        computePushrodForceToJointTorqueMap00(cq1, sq1, sq2, x1);
    double pushrodForceToJointTorqueMap01 =
        computePushrodForceToJointTorqueMap01(cq1, sq1, sq2, x2);
    double pushrodForceToJointTorqueMap10 =
        computePushrodForceToJointTorqueMap10(cq1, sq1, cq2, sq2, x1);
    double pushrodForceToJointTorqueMap11 =
        computePushrodForceToJointTorqueMap11(cq1, sq1, cq2, sq2, x2);
    jacobianTranspose.set(0, 0, pushrodForceToJointTorqueMap00);
    jacobianTranspose.set(0, 1, pushrodForceToJointTorqueMap01);
    jacobianTranspose.set(1, 0, pushrodForceToJointTorqueMap10);
    jacobianTranspose.set(1, 1, pushrodForceToJointTorqueMap11);
  }
  /**
   * Sets the values in the specified matrix to a rotation matrix about the z-axis.
   *
   * @param ang the angle it rotates a point by in radians.
   * @param r A 3 by 3 matrix. Is modified.
   */
  public static void setRotZ(float ang, DenseMatrix64F r) {
    float c = (float) Math.cos(ang);
    float s = (float) Math.sin(ang);

    r.set(0, 0, c);
    r.set(0, 1, -s);
    r.set(1, 0, s);
    r.set(1, 1, c);
    r.set(2, 2, 1);
  }
  /**
   * Sets the values in the specified matrix to a rotation matrix about the x-axis.
   *
   * @param ang the angle it rotates a point by in radians.
   * @param R (Output) Storage for rotation matrix. Modified.
   */
  public static void setRotX(float ang, DenseMatrix64F R) {
    float c = (float) Math.cos(ang);
    float s = (float) Math.sin(ang);

    R.set(0, 0, 1);
    R.set(1, 1, c);
    R.set(1, 2, -s);
    R.set(2, 1, s);
    R.set(2, 2, c);
  }
Ejemplo n.º 6
0
  /**
   * Adds a new sample of the raw data to internal data structure for later processing. All the
   * samples must be added before computeBasis is called.
   *
   * @param sampleData Sample from original raw data.
   */
  public void addSample(double[] sampleData) {
    if (A.getNumCols() != sampleData.length)
      throw new IllegalArgumentException("Unexpected sample size");
    if (sampleIndex >= A.getNumRows()) throw new IllegalArgumentException("Too many samples");

    for (int i = 0; i < sampleData.length; i++) {
      A.set(sampleIndex, i, sampleData[i]);
    }
    sampleIndex++;
  }
Ejemplo n.º 7
0
 public static DenseMatrix64F numDiff2d(MatrixChains chains, double[] angles, double epsilon) {
   int l = angles.length;
   DenseMatrix64F togo = new DenseMatrix64F(l, l);
   for (int i = 0; i < l; ++i) {
     for (int j = 0; j < l; ++j) {
       togo.set(i, j, numDiff(chains, angles, i, j, epsilon));
     }
   }
   return togo;
 }
Ejemplo n.º 8
0
  private void initPower(DenseMatrix64F A) {
    if (A.numRows != A.numCols) throw new IllegalArgumentException("A must be a square matrix.");

    if (seed != null) {
      q0.set(seed);
    } else {
      for (int i = 0; i < A.numRows; i++) {
        q0.data[i] = 1;
      }
    }
  }
  @Override
  public void actuatorToJointEffort(LinearActuator[] act_data, ValkyrieJointInterface[] jnt_data) {
    compute(jnt_data);
    pushrodForces.set(0, 0, act_data[0].getEffort());
    pushrodForces.set(1, 0, act_data[1].getEffort());

    //      CommonOps.multTransA(jacobian, pushrodForces, jointTorques);
    CommonOps.mult(jacobianTranspose, pushrodForces, jointTorques);

    jnt_data[1].setEffort(jointTorques.get(0, 0));
    jnt_data[0].setEffort(jointTorques.get(1, 0));
  }
  public static long fillManual(DenseMatrix64F mat, int numTrials) {
    long prev = System.currentTimeMillis();

    for (int i = 0; i < numTrials; i++) {
      final int size = mat.getNumElements();

      for (int j = 0; j < size; j++) {
        mat.set(j, 2);
      }
    }

    long curr = System.currentTimeMillis();
    return curr - prev;
  }
Ejemplo n.º 11
0
  /**
   * Compute rectification transforms for the stereo pair given a fundamental matrix and its
   * observations.
   *
   * @param F Fundamental matrix
   * @param observations Observations used to compute F
   * @param width Width of first image.
   * @param height Height of first image.
   */
  public void process(DenseMatrix64F F, List<AssociatedPair> observations, int width, int height) {

    int centerX = width / 2;
    int centerY = height / 2;

    MultiViewOps.extractEpipoles(F, epipole1, epipole2);
    checkEpipoleInside(width, height);

    // compute the transform H which will send epipole2 to infinity
    SimpleMatrix R = rotateEpipole(epipole2, centerX, centerY);
    SimpleMatrix T = translateToOrigin(centerX, centerY);
    SimpleMatrix G = computeG(epipole2, centerX, centerY);

    SimpleMatrix H = G.mult(R).mult(T);

    // Find the two matching transforms
    SimpleMatrix Hzero = computeHZero(F, epipole2, H);

    SimpleMatrix Ha = computeAffineH(observations, H.getMatrix(), Hzero.getMatrix());

    rect1.set(Ha.mult(Hzero).getMatrix());
    rect2.set(H.getMatrix());
  }
  @Override
  public void actuatorToJointVelocity(
      LinearActuator[] act_data, ValkyrieJointInterface[] jnt_data) {
    compute(jnt_data);
    pushrodVelocities.set(0, 0, act_data[0].getVelocity());
    pushrodVelocities.set(1, 0, act_data[1].getVelocity());

    DenseMatrix64F jacobianInverse = new DenseMatrix64F(2, 2);
    CommonOps.invert(jacobian, jacobianInverse);
    CommonOps.mult(jacobianInverse, pushrodVelocities, jointVelocites);

    jnt_data[1].setVelocity(-jointVelocites.get(0, 0));
    jnt_data[0].setVelocity(-jointVelocites.get(1, 0));
  }
  // ValkyrieJointInterface[] jnt_data = [ pitch, roll ]
  @Override
  public void jointToActuatorEffort(LinearActuator[] act_data, ValkyrieJointInterface[] jnt_data) {
    compute(jnt_data);
    jointTorques.set(0, 0, jnt_data[1].getDesiredEffort());
    jointTorques.set(1, 0, jnt_data[0].getDesiredEffort());

    DenseMatrix64F jacobianTransposeInverse = new DenseMatrix64F(2, 2);
    CommonOps.invert(jacobianTranspose, jacobianTransposeInverse);

    CommonOps.mult(jacobianTransposeInverse, jointTorques, pushrodForces);

    act_data[0].setEffortCommand(pushrodForces.get(0, 0));
    act_data[1].setEffortCommand(pushrodForces.get(1, 0));
  }
Ejemplo n.º 14
0
  /**
   * Loads the word-vector pairs stored in the file whose path is
   * <code>filename</code> The file can be a plain text file or a .gz archive.
   * <p>
   * The expected format is: </br>
   * number_of_vectors space_dimensionality</br>
   * word_i [TAB] 1.0 [TAB] 0 [TAB] vector values comma
   * separated </code> </br>
   * </br>
   * Example: </br>
   * </br>
   * <code> 3 5</br>
   * dog::n [TAB] 1.0 [TAB] 0 [TAB] 2.1,4.1,1.4,2.3,0.9</br>
   * cat::n [TAB] 1.0 [TAB] 0 [TAB] 3.2,4.3,1.2,2.2,0.8</br>
   * mouse::n [TAB] 1.0 [TAB] 0 [TAB] 2.4,4.4,2.4,1.3,0.92</br>
   *
   *
   * @param filename
   *            the path of the file containing the word-vector pairs
   * @throws IOException
   */
  private void populate(String filename) throws IOException {
    BufferedReader br = null;
    GZIPInputStream gzis = null;
    if (filename.endsWith(".gz")) {
      gzis = new GZIPInputStream(new FileInputStream(filename));
      InputStreamReader reader = new InputStreamReader(gzis, "UTF8");
      br = new BufferedReader(reader);
    } else {
      br = new BufferedReader(new InputStreamReader(new FileInputStream(filename), "UTF8"));
    }
    String line;
    ArrayList<String> split;
    String label;
    String[] vSplit;

    Pattern iPattern = Pattern.compile(",");
    float[] v = null;

    while ((line = br.readLine()) != null) {
      if (!line.contains("\t")) continue;
      float norm2 = 0;
      split = mySplit(line);
      label = split.get(0);
      vSplit = iPattern.split(split.get(3), 0);
      if (v == null) v = new float[vSplit.length];
      for (int i = 0; i < v.length; i++) {
        v[i] = Float.parseFloat(vSplit[i]);
        norm2 += v[i] * v[i];
      }
      float norm = (float) Math.sqrt(norm2);
      for (int i = 0; i < v.length; i++) {
        v[i] /= norm;
      }

      DenseMatrix64F featureVector = new DenseMatrix64F(1, v.length);
      for (int i = 0; i < v.length; i++) {
        featureVector.set(0, i, (double) v[i]);
      }

      DenseVector denseFeatureVector = new DenseVector(featureVector);

      addWordVector(label, denseFeatureVector);
    }
    if (filename.endsWith(".gz")) {
      gzis.close();
    }
    br.close();
  }
Ejemplo n.º 15
0
  // Produce 2nd derivative of objective function at specified angle setting
  public static DenseMatrix64F get2d(MatrixChains chain, double[] angles) {
    if (angles == null) // yingdan
    return null;

    int dim = angles.length;
    DenseMatrix64F togo = new DenseMatrix64F(dim, dim);
    for (int i = 0; i < dim; ++i) {
      for (int j = 0; j < dim; ++j) {
        double v = df2by(chain, angles, i, j);
        if (chain.regularise > 0 && i == j) {
          v += 2 * chain.regularise;
        }
        togo.set(i, j, v);
      }
    }
    return togo;
  }
Ejemplo n.º 16
0
  public static OptimisationReport NiuDunLaFuSunStep(
      MatrixChains chain, double[] angles, boolean printStep) {
    // see: http://code.google.com/p/efficient-java-matrix-library/wiki/SolvingLinearSystems
    int l = angles.length;
    DenseMatrix64F J = get2d(chain, angles);

    DenseMatrix64F grad = get1d(chain, angles);
    DenseMatrix64F propose = vector(l);

    boolean solved = CommonOps.solve(J, grad, propose);
    for (int i = 0; i < l; ++i) {
      if (Double.isNaN(propose.data[i])) {
        solved = false;
      }
    }
    OptimisationReport togo = new OptimisationReport();

    try {
      if (!solved) {
        throw new RuntimeException("Failed to solve update equation");
      }
      CommonOps.scale(-1, propose);

      if (printStep) {
        EigenDecomposition<DenseMatrix64F> decomp = DecompositionFactory.eigSymm(l, false);
        boolean posed = decomp.decompose(J);
        if (!posed) {
          throw new RuntimeException("Failed to decompose matrix");
        }
        double[] eigs = eigs(decomp);
        System.out.println("Computed eigenvalues " + printVec(eigs));
        togo.eigenvalues = eigs;
      }
      tryProposal(chain, propose, grad, angles, printStep, togo);
    } catch (Exception e) {
      System.out.println(
          "Failed to find suitable proposal from Newton step - trying gradient step");
      propose.set(grad);
      CommonOps.scale(-1, propose);
      tryProposal(chain, propose, grad, angles, printStep, togo);
    }
    return togo;
  }
Ejemplo n.º 17
0
  /**
   * 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);
  }
  @Test
  public void _solveVectorInternal() {
    int width = 10;
    DenseMatrix64F LU = RandomMatrices.createRandom(width, width, rand);

    double a[] = new double[] {1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
    double b[] = new double[] {1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
    for (int i = 0; i < width; i++) LU.set(i, i, 1);

    TriangularSolver.solveL(LU.data, a, width);
    TriangularSolver.solveU(LU.data, a, width);

    DebugDecompose alg = new DebugDecompose(width);
    for (int i = 0; i < width; i++) alg.getIndx()[i] = i;
    alg.setLU(LU);

    alg._solveVectorInternal(b);

    for (int i = 0; i < width; i++) {
      assertEquals(a[i], b[i], 1e-6);
    }
  }
  /**
   * Converts a unit quaternion into a rotation matrix.
   *
   * <p>Equations is taken from: Paul J. Besl and Neil D. McKay, "A Method for Registration of 3-D
   * Shapes" IEEE Transactions on Pattern Analysis and Machine Intelligence, Vol 14, No. 2, Feb.
   * 1992
   *
   * @param quat Unit quaternion.
   * @param R Storage for rotation matrix. If null a new matrix is created. Modified.
   * @return Rotation matrix
   */
  public static DenseMatrix64F quaternionToMatrix(Quaternion_F32 quat, DenseMatrix64F R) {
    R = checkDeclare3x3(R);

    final float q0 = quat.w;
    final float q1 = quat.x;
    final float q2 = quat.y;
    final float q3 = quat.z;

    R.set(0, 0, q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3);
    R.set(0, 1, 2.0f * (q1 * q2 - q0 * q3));
    R.set(0, 2, 2.0f * (q1 * q3 + q0 * q2));

    R.set(1, 0, 2.0f * (q1 * q2 + q0 * q3));
    R.set(1, 1, q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3);
    R.set(1, 2, 2.0f * (q2 * q3 - q0 * q1));

    R.set(2, 0, 2.0f * (q1 * q3 - q0 * q2));
    R.set(2, 1, 2.0f * (q2 * q3 + q0 * q1));
    R.set(2, 2, q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3);

    return R;
  }
 public void setDesiredPosition(DenseMatrix64F q, int[] indices) {
   if (indices.length != 7)
     throw new RuntimeException("Unexpected number of indices: " + Arrays.toString(indices));
   desiredConfiguration.reshape(7, 1);
   for (int i = 0; i < indices.length; i++) desiredConfiguration.set(i, 0, q.get(indices[i], 0));
 }
 /**
  * Complete the information held in this using other. Does not overwrite the data already set in
  * this.
  */
 public void completeWith(RootJointDesiredConfigurationData other) {
   if (!hasDesiredConfiguration()) desiredConfiguration.set(other.desiredConfiguration);
   if (!hasDesiredVelocity()) desiredVelocity.set(other.desiredVelocity);
   if (!hasDesiredAcceleration()) desiredAcceleration.set(other.desiredAcceleration);
 }
Ejemplo n.º 22
0
 @Override
 public TDoubleArrayList[] values(
     Coordinate pos,
     List<? extends Coordinate> nodes,
     TDoubleArrayList[] distSquares,
     TDoubleArrayList infRads,
     TDoubleArrayList[] results) {
   if (null == distSquares) {
     distSquares = this.distSquaresCache;
     euclideanDistSqFun.setPosition(pos);
     euclideanDistSqFun.sqValues(nodes, distSquares);
   }
   int ndsNum = nodes.size();
   weightFunction.values(distSquares, infRads, nodesWeightsCache);
   results = ShapeFunctionUtils2D.init4Output(results, diffOrder, ndsNum);
   int diffDim = CommonUtils.len2DBase(diffOrder);
   int baseDim = basesFunction.getDim();
   Coordinate zero = new Coordinate(0, 0, 0);
   Coordinate radCoord = new Coordinate(0, 0, 0);
   for (int i = 0; i < diffDim; i++) {
     As[i].zero();
     ArrayList<TDoubleArrayList> tB = Bs.get(i);
     for (int j = 0; j < baseDim; j++) {
       TDoubleArrayList v = tB.get(j);
       v.resetQuick();
       v.ensureCapacity(ndsNum);
       v.fill(0, ndsNum, 0);
     }
   }
   basesFunction.setDiffOrder(0);
   for (int diffDimIdx = 0; diffDimIdx < diffDim; diffDimIdx++) {
     TDoubleArrayList weights_d = nodesWeightsCache[diffDimIdx];
     DenseMatrix64F A_d = As[diffDimIdx];
     ArrayList<TDoubleArrayList> B_d = Bs.get(diffDimIdx);
     double[] tp = ps_arr[0];
     int ndIdx = 0;
     for (Coordinate nd : nodes) {
       if (areBasesRelative) {
         GeometryMath.minus(nd, pos, radCoord);
         basesFunction.values(radCoord, ps_arr);
       } else {
         basesFunction.values(nd, ps_arr);
       }
       double weight_d = weights_d.getQuick(ndIdx);
       for (int i = 0; i < baseDim; i++) {
         double p_i = tp[i];
         for (int j = 0; j < baseDim; j++) {
           double p_ij = p_i * tp[j];
           A_d.add(i, j, weight_d * p_ij);
         }
         B_d.get(i).set(ndIdx, weight_d * p_i);
       }
       ndIdx++;
     }
   }
   basesFunction.setDiffOrder(diffOrder);
   if (areBasesRelative) {
     basesFunction.values(zero, ps_arr);
   } else {
     basesFunction.values(pos, ps_arr);
   }
   A_bak.set(A);
   luSolver.setA(A_bak);
   tv.set(p);
   luSolver.solve(tv, gamma);
   //            CommonOps.solve(A, p, gamma);
   ShapeFunctionUtils2D.multAddTo(gamma, B, results[0]);
   if (diffOrder < 1) {
     return results;
   }
   tv.zero();
   CommonOps.mult(-1, A_x, gamma, tv);
   CommonOps.add(p_x, tv, tv);
   luSolver.solve(tv, gamma_x);
   //            CommonOps.solve(A, tv, gamma_x);
   tv.zero();
   CommonOps.mult(-1, A_y, gamma, tv);
   CommonOps.add(p_y, tv, tv);
   luSolver.solve(tv, gamma_y);
   //            CommonOps.solve(A, tv, gamma_y);
   ShapeFunctionUtils2D.multAddTo(gamma_x, B, results[1]);
   ShapeFunctionUtils2D.multAddTo(gamma, B_x, results[1]);
   ShapeFunctionUtils2D.multAddTo(gamma_y, B, results[2]);
   ShapeFunctionUtils2D.multAddTo(gamma, B_y, results[2]);
   return results;
 }
 public void setDesiredAcceleration(DenseMatrix64F qdd, int[] indices) {
   if (indices.length != 6)
     throw new RuntimeException("Unexpected number of indices: " + Arrays.toString(indices));
   desiredAcceleration.reshape(6, 1);
   for (int i = 0; i < indices.length; i++) desiredAcceleration.set(i, 0, qdd.get(indices[i], 0));
 }
Ejemplo n.º 24
0
 public void add(Double value) {
   if (currPtr < dimensions) {
     values.set(currPtr, 0, value);
     currPtr++;
   }
 }
Ejemplo n.º 25
0
 public void set(TrifocalTensor a) {
   T1.set(a.T1);
   T2.set(a.T2);
   T3.set(a.T3);
 }
Ejemplo n.º 26
0
Archivo: CPM.java Proyecto: fpl/s1tbx
  private void estimateCPM() {

    logger.info("Start EJML Estimation");

    numIterations = 0;
    boolean estimationDone = false;

    DenseMatrix64F eL_hat = null;
    DenseMatrix64F eP_hat = null;
    DenseMatrix64F rhsL = null;
    DenseMatrix64F rhsP = null;

    // normalize master coordinates for stability -- only master!
    TDoubleArrayList yMasterNorm = new TDoubleArrayList();
    TDoubleArrayList xMasterNorm = new TDoubleArrayList();
    for (int i = 0; i < yMaster.size(); i++) {
      yMasterNorm.add(PolyUtils.normalize2(yMaster.getQuick(i), normWin.linelo, normWin.linehi));
      xMasterNorm.add(PolyUtils.normalize2(xMaster.getQuick(i), normWin.pixlo, normWin.pixhi));
    }

    // helper variables
    int winL;
    int winP;
    int maxWSum_idx = 0;

    while (!estimationDone) {

      String codeBlockMessage = "LS ESTIMATION PROCEDURE";
      StopWatch stopWatch = new StopWatch();
      StopWatch clock = new StopWatch();
      clock.start();
      stopWatch.setTag(codeBlockMessage);
      stopWatch.start();

      logger.info("Start iteration: {}" + numIterations);

      /** Remove identified outlier from previous estimation */
      if (numIterations != 0) {
        logger.info(
            "Removing observation {}, idxList {},  from observation vector."
                + index.getQuick(maxWSum_idx)
                + maxWSum_idx);
        index.removeAt(maxWSum_idx);
        yMasterNorm.removeAt(maxWSum_idx);
        xMasterNorm.removeAt(maxWSum_idx);
        yOffset.removeAt(maxWSum_idx);
        xOffset.removeAt(maxWSum_idx);

        // only for outlier removal
        yMaster.removeAt(maxWSum_idx);
        xMaster.removeAt(maxWSum_idx);
        ySlave.removeAt(maxWSum_idx);
        xSlave.removeAt(maxWSum_idx);
        coherence.removeAt(maxWSum_idx);

        // also take care of slave pins
        slaveGCPList.remove(maxWSum_idx);

        //                if (demRefinement) {
        //                    ySlaveGeometry.removeAt(maxWSum_idx);
        //                    xSlaveGeometry.removeAt(maxWSum_idx);
        //                }

      }

      /** Check redundancy */
      numObservations = index.size(); // Number of points > threshold
      if (numObservations < numUnknowns) {
        logger.severe(
            "coregpm: Number of windows > threshold is smaller than parameters solved for.");
        throw new ArithmeticException(
            "coregpm: Number of windows > threshold is smaller than parameters solved for.");
      }

      // work with normalized values
      DenseMatrix64F A =
          new DenseMatrix64F(
              SystemOfEquations.constructDesignMatrix_loop(
                  yMasterNorm.toArray(), xMasterNorm.toArray(), cpmDegree));

      logger.info("TIME FOR SETUP of SYSTEM : {}" + stopWatch.lap("setup"));

      RowD1Matrix64F Qy_1; // vector
      double meanValue;
      switch (cpmWeight) {
        case "linear":
          logger.info("Using sqrt(coherence) as weights");
          Qy_1 = DenseMatrix64F.wrap(numObservations, 1, coherence.toArray());
          // Normalize weights to avoid influence on estimated var.factor
          logger.info("Normalizing covariance matrix for LS estimation");
          meanValue = CommonOps.elementSum(Qy_1) / numObservations;
          CommonOps.divide(meanValue, Qy_1); // normalize vector
          break;
        case "quadratic":
          logger.info("Using coherence as weights.");
          Qy_1 = DenseMatrix64F.wrap(numObservations, 1, coherence.toArray());
          CommonOps.elementMult(Qy_1, Qy_1);
          // Normalize weights to avoid influence on estimated var.factor
          meanValue = CommonOps.elementSum(Qy_1) / numObservations;
          logger.info("Normalizing covariance matrix for LS estimation.");
          CommonOps.divide(meanValue, Qy_1); // normalize vector
          break;
        case "bamler":
          // TODO: see Bamler papers IGARSS 2000 and 2004
          logger.warning("Bamler weighting method NOT IMPLEMENTED, falling back to None.");
          Qy_1 = onesEJML(numObservations);
          break;
        case "none":
          logger.info("No weighting.");
          Qy_1 = onesEJML(numObservations);
          break;
        default:
          Qy_1 = onesEJML(numObservations);
          break;
      }

      logger.info("TIME FOR SETUP of VC diag matrix: {}" + stopWatch.lap("diag VC matrix"));

      /** tempMatrix_1 matrices */
      final DenseMatrix64F yL_matrix = DenseMatrix64F.wrap(numObservations, 1, yOffset.toArray());
      final DenseMatrix64F yP_matrix = DenseMatrix64F.wrap(numObservations, 1, xOffset.toArray());
      logger.info("TIME FOR SETUP of TEMP MATRICES: {}" + stopWatch.lap("Temp matrices"));

      /** normal matrix */
      final DenseMatrix64F N =
          new DenseMatrix64F(numUnknowns, numUnknowns); // = A_transpose.mmul(Qy_1_diag.mmul(A));

      /*
                  // fork/join parallel implementation
                  RowD1Matrix64F result = A.copy();
                  DiagXMat dd = new DiagXMat(Qy_1, A, 0, A.numRows, result);
                  ForkJoinPool pool = new ForkJoinPool();
                  pool.invoke(dd);
                  CommonOps.multAddTransA(A, dd.result, N);
      */

      CommonOps.multAddTransA(A, diagxmat(Qy_1, A), N);
      DenseMatrix64F Qx_hat = N.copy();

      logger.info("TIME FOR SETUP of NORMAL MATRIX: {}" + stopWatch.lap("Normal matrix"));

      /** right hand sides */
      // azimuth
      rhsL = new DenseMatrix64F(numUnknowns, 1); // A_transpose.mmul(Qy_1_diag.mmul(yL_matrix));
      CommonOps.multAddTransA(1d, A, diagxmat(Qy_1, yL_matrix), rhsL);
      // range
      rhsP = new DenseMatrix64F(numUnknowns, 1); // A_transpose.mmul(Qy_1_diag.mmul(yP_matrix));
      CommonOps.multAddTransA(1d, A, diagxmat(Qy_1, yP_matrix), rhsP);
      logger.info("TIME FOR SETUP of RightHand Side: {}" + stopWatch.lap("Right-hand-side"));

      LinearSolver<DenseMatrix64F> solver = LinearSolverFactory.leastSquares(100, 100);
      /** compute solution */
      if (!solver.setA(Qx_hat)) {
        throw new IllegalArgumentException("Singular Matrix");
      }
      solver.solve(rhsL, rhsL);
      solver.solve(rhsP, rhsP);
      logger.info("TIME FOR SOLVING of System: {}" + stopWatch.lap("Solving System"));

      /** inverting of Qx_hat for stability check */
      solver.invert(Qx_hat);

      logger.info("TIME FOR INVERSION OF N: {}" + stopWatch.lap("Inversion of N"));

      /** test inversion and check stability: max(abs([N*inv(N) - E)) ?= 0 */
      DenseMatrix64F tempMatrix_1 = new DenseMatrix64F(N.numRows, N.numCols);
      CommonOps.mult(N, Qx_hat, tempMatrix_1);
      CommonOps.subEquals(
          tempMatrix_1, CommonOps.identity(tempMatrix_1.numRows, tempMatrix_1.numCols));
      double maxDeviation = CommonOps.elementMaxAbs(tempMatrix_1);
      if (maxDeviation > .01) {
        logger.severe(
            "COREGPM: maximum deviation N*inv(N) from unity = {}. This is larger than 0.01"
                + maxDeviation);
        throw new IllegalStateException("COREGPM: maximum deviation N*inv(N) from unity)");
      } else if (maxDeviation > .001) {
        logger.warning(
            "COREGPM: maximum deviation N*inv(N) from unity = {}. This is between 0.01 and 0.001"
                + maxDeviation);
      }
      logger.info("TIME FOR STABILITY CHECK: {}" + stopWatch.lap("Stability Check"));

      logger.info("Coeffs in Azimuth direction: {}" + rhsL.toString());
      logger.info("Coeffs in Range direction: {}" + rhsP.toString());
      logger.info("Max Deviation: {}" + maxDeviation);
      logger.info("System Quality: {}" + solver.quality());

      /** some other stuff if the scale is okay */
      DenseMatrix64F Qe_hat = new DenseMatrix64F(numObservations, numObservations);
      DenseMatrix64F tempMatrix_2 = new DenseMatrix64F(numObservations, numUnknowns);

      CommonOps.mult(A, Qx_hat, tempMatrix_2);
      CommonOps.multTransB(-1, tempMatrix_2, A, Qe_hat);
      scaleInputDiag(Qe_hat, Qy_1);

      // solution: Azimuth
      DenseMatrix64F yL_hat = new DenseMatrix64F(numObservations, 1);
      eL_hat = new DenseMatrix64F(numObservations, 1);
      CommonOps.mult(A, rhsL, yL_hat);
      CommonOps.sub(yL_matrix, yL_hat, eL_hat);

      // solution: Range
      DenseMatrix64F yP_hat = new DenseMatrix64F(numObservations, 1);
      eP_hat = new DenseMatrix64F(numObservations, 1);
      CommonOps.mult(A, rhsP, yP_hat);
      CommonOps.sub(yP_matrix, yP_hat, eP_hat);

      logger.info("TIME FOR DATA preparation for TESTING: {}" + stopWatch.lap("Testing Setup"));

      /** overal model test (variance factor) */
      double overAllModelTest_L = 0;
      double overAllModelTest_P = 0;

      for (int i = 0; i < numObservations; i++) {
        overAllModelTest_L += FastMath.pow(eL_hat.get(i), 2) * Qy_1.get(i);
        overAllModelTest_P += FastMath.pow(eP_hat.get(i), 2) * Qy_1.get(i);
      }

      overAllModelTest_L =
          (overAllModelTest_L / FastMath.pow(SIGMA_L, 2)) / (numObservations - numUnknowns);
      overAllModelTest_P =
          (overAllModelTest_P / FastMath.pow(SIGMA_P, 2)) / (numObservations - numUnknowns);

      logger.info("Overall Model Test Lines: {}" + overAllModelTest_L);
      logger.info("Overall Model Test Pixels: {}" + overAllModelTest_P);

      logger.info("TIME FOR OMT: {}" + stopWatch.lap("OMT"));

      /** ---------------------- DATASNOPING ----------------------------------- * */
      /** Assumed Qy diag */

      /** initialize */
      DenseMatrix64F wTest_L = new DenseMatrix64F(numObservations, 1);
      DenseMatrix64F wTest_P = new DenseMatrix64F(numObservations, 1);

      for (int i = 0; i < numObservations; i++) {
        wTest_L.set(i, eL_hat.get(i) / (Math.sqrt(Qe_hat.get(i, i)) * SIGMA_L));
        wTest_P.set(i, eP_hat.get(i) / (Math.sqrt(Qe_hat.get(i, i)) * SIGMA_P));
      }

      /** find maxima's */
      // azimuth
      winL = absArgmax(wTest_L);
      double maxWinL = Math.abs(wTest_L.get(winL));
      logger.info(
          "maximum wtest statistic azimuth = {} for window number: {} "
              + maxWinL
              + index.getQuick(winL));

      // range
      winP = absArgmax(wTest_P);
      double maxWinP = Math.abs(wTest_P.get(winP));
      logger.info(
          "maximum wtest statistic range = {} for window number: {} "
              + maxWinP
              + index.getQuick(winP));

      /** use summed wTest in Azimuth and Range direction for outlier detection */
      DenseMatrix64F wTestSum = new DenseMatrix64F(numObservations);
      for (int i = 0; i < numObservations; i++) {
        wTestSum.set(i, FastMath.pow(wTest_L.get(i), 2) + FastMath.pow(wTest_P.get(i), 2));
      }

      maxWSum_idx = absArgmax(wTest_P);
      double maxWSum = wTest_P.get(winP);
      logger.info(
          "Detected outlier: summed sqr.wtest = {}; observation: {}"
              + maxWSum
              + index.getQuick(maxWSum_idx));

      /** Test if we are estimationDone yet */
      // check on number of observations
      if (numObservations <= numUnknowns) {
        logger.warning("NO redundancy!  Exiting iterations.");
        estimationDone = true; // cannot remove more than this
      }

      // check on test k_alpha
      if (Math.max(maxWinL, maxWinP) <= criticalValue) {
        // all tests accepted?
        logger.info("All outlier tests accepted! (final solution computed)");
        estimationDone = true;
      }

      if (numIterations >= maxIterations) {
        logger.info("max. number of iterations reached (exiting loop).");
        estimationDone = true; // we reached max. (or no max_iter specified)
      }

      /** Only warn if last iteration has been estimationDone */
      if (estimationDone) {
        if (overAllModelTest_L > 10) {
          logger.warning(
              "COREGPM: Overall Model Test, Lines = {} is larger than 10. (Suggest model or a priori sigma not correct.)"
                  + overAllModelTest_L);
        }
        if (overAllModelTest_P > 10) {
          logger.warning(
              "COREGPM: Overall Model Test, Pixels = {} is larger than 10. (Suggest model or a priori sigma not correct.)"
                  + overAllModelTest_P);
        }

        /** if a priori sigma is correct, max wtest should be something like 1.96 */
        if (Math.max(maxWinL, maxWinP) > 200.0) {
          logger.warning(
              "Recommendation: remove window number: {} and re-run step COREGPM.  max. wtest is: {}."
                  + index.get(winL)
                  + Math.max(maxWinL, maxWinP));
        }
      }

      logger.info("TIME FOR wTestStatistics: {}" + stopWatch.lap("WTEST"));
      logger.info("Total Estimation TIME: {}" + clock.getElapsedTime());

      numIterations++; // update counter here!
    } // only warn when iterating

    yError = eL_hat.getData();
    xError = eP_hat.getData();

    yCoef = rhsL.getData();
    xCoef = rhsP.getData();
  }
 public void set(RootJointDesiredConfigurationDataReadOnly other) {
   desiredConfiguration.set(other.getDesiredConfiguration());
   desiredVelocity.set(other.getDesiredVelocity());
   desiredAcceleration.set(other.getDesiredAcceleration());
 }
  public void setDesiredConfiguration(DenseMatrix64F q) {
    if (q.getNumRows() != 7 || q.getNumCols() != 1)
      throw new RuntimeException("Unexpected size: " + q);

    desiredConfiguration.set(q);
  }
  public void setDesiredVelocity(DenseMatrix64F qd) {
    if (qd.getNumRows() != 6 || qd.getNumCols() != 1)
      throw new RuntimeException("Unexpected size: " + qd);

    desiredVelocity.set(qd);
  }
  public void setDesiredAcceleration(DenseMatrix64F qdd) {
    if (qdd.getNumRows() != 6 || qdd.getNumCols() != 1)
      throw new RuntimeException("Unexpected size: " + qdd);

    desiredAcceleration.set(qdd);
  }