public double getChi2(Graph g) {
    double chi2 = Double.MAX_VALUE;

    for (int i = 0; i < components.length; i++) {
      chi2 = Math.min(chi2, -2 * Math.log(weights[i]) + components[i].getChi2(g));
    }

    return chi2;
  }
Exemple #2
0
  // incorporates exponential model of probability of motion, weight less likely motions less
  private double[] constructW(double[] Uxyzrpy) {
    assert (Uxyzrpy.length == Vxyzrpy.length) : "Warning expected 6 velocity";

    double[] W = new double[6]; // weighting matrix

    for (int i = 0; i < Uxyzrpy.length; i++) {
      double u = Uxyzrpy[i];
      double v = Vxyzrpy[i];
      double Dv = Math.abs(u - v);

      if (i <= 2) {
        // handle velocities close to 0
        if ((Math.abs(u) <= LV_THRESH) && (Math.abs(v) <= LV_THRESH)) {
          // use exponential model to come up with weights
          W[i] = MathUtil.exp(-ALPHA * Dv);
        } // if our change in velocity was within acceptable bounds
        else if ((Dv / u) <= DELTAV_THRESH) {
          // use exponential model to come up with weights
          W[i] = MathUtil.exp(-ALPHA * Dv);
        } else {
          W[i] = 0; // throw out bad changes
        }
      } else {
        // handle angular velocities close to 0
        if ((Math.abs(u) <= LR_THRESH) && (Math.abs(v) <= LR_THRESH)) {
          // use exponential model to come up with weights
          W[i] = MathUtil.exp(-BETA * Dv);
        } // if our change in velocity was within acceptable bounds
        else if ((Dv / u) <= DELTAR_THRESH) {
          // use exponential model to come up with weights
          W[i] = MathUtil.exp(-BETA * Dv);
        } else {
          W[i] = 0; // throw out bad changes
        }
      }
    }
    return W;
  }
  public Linearization linearize(Graph g, Linearization lin) {
    double bestChi2 = Double.MAX_VALUE;
    GEdge bestEdge = null;
    int besti = -1;

    for (int i = 0; i < components.length; i++) {
      double thisChi2 = -2 * Math.log(weights[i]) + components[i].getChi2(g);
      if (thisChi2 < bestChi2) {
        bestChi2 = thisChi2;
        bestEdge = components[i];
        besti = i;
      }
      System.out.printf("%d %15f\n", i, thisChi2);
    }

    System.out.println("besti: " + besti);
    return bestEdge.linearize(g, lin);
  }