@Override
  public String toString() {
    StringBuilder builder = new StringBuilder();
    builder.append("Jacobian. jacobianFrame = " + jacobianFrame + ". Joints:\n");

    for (InverseDynamicsJoint joint : unitTwistMap.keySet()) {
      builder.append(joint.getClass().getSimpleName() + " " + joint.getName() + "\n");
    }

    builder.append("\n");
    builder.append(jacobian.toString());

    return builder.toString();
  }
Exemple #2
0
  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();
  }