コード例 #1
0
ファイル: IMU.java プロジェクト: pdaquino/EECS568
  // if we don't have anything to filter, just use Dead Reckoning
  public double[][] estimate() {
    // use new velocity estimates to estimate xyzrpy
    double[] Exyzrpy = LinAlg.scale(Vxyzrpy, T);

    // convert back to RBT
    double[][] Erbt = LinAlg.xyzrpyToMatrix(Exyzrpy);
    // XXX there is a 2nd version of the function above which one is better
    return Erbt;
  }
コード例 #2
0
ファイル: IMU.java プロジェクト: pdaquino/EECS568
  // incorporate new information and return new estimate
  public double[][] estimate(double[][] Urbt) {
    // what rigid body transformation occured over the last frame
    double[] DUxyzrpy = LinAlg.matrixToXyzrpy(Urbt);

    // convert to velocity
    double[] Uxyzrpy;
    if (T != 0) {
      Uxyzrpy = LinAlg.scale(DUxyzrpy, 1 / T);
    } else {
      Uxyzrpy = new double[6];
    }
    /*
    System.out.println("Our observation velocity");
    LinAlg.print(Uxyzrpy);

    System.out.println("Our best estimate of velocity");
    LinAlg.print(Vxyzrpy);
     */
    // calculate relative weighting
    double[] W = constructW(Uxyzrpy);
    /*
    System.out.println("Our weighting matrix");
    LinAlg.print(W);
     */
    // incorporatenew information
    Vxyzrpy = weightedSum(Vxyzrpy, Uxyzrpy, W);
    /*
    System.out.println("After in corporating information");
    LinAlg.print(Vxyzrpy);
     */
    // use new velocity estimates to estimate xyzrpy
    double[] Exyzrpy = LinAlg.scale(Vxyzrpy, T);

    // convert back to RBT
    double[][] Erbt = LinAlg.xyzrpyToMatrix(Exyzrpy);
    // XXX there is a 2nd version of the function above which one is better
    return Erbt;
  }
コード例 #3
0
ファイル: VzOFF.java プロジェクト: kenkeiras/april-tags
  /** Call with one or more OFF model paths * */
  public static void main(String args[]) {
    JFrame f = new JFrame("VzOFF " + args[0]);
    f.setLayout(new BorderLayout());

    VisWorld vw = new VisWorld();
    VisLayer vl = new VisLayer(vw);
    VisCanvas vc = new VisCanvas(vl);

    VzMesh.Style defaultMeshStyle = new VzMesh.Style(Color.cyan);

    ArrayList<VzOFF> models = new ArrayList<VzOFF>();

    for (int i = 0; i < args.length; i++) {
      if (args[i].endsWith(".off")) {
        try {
          models.add(new VzOFF(args[i], defaultMeshStyle));
          System.out.printf("Loaded: %20s (%5.2f%%)\n", args[i], i * 100.0 / args.length);
        } catch (IOException ex) {
          System.out.println("ex: " + ex);
        }
      } else {
        System.out.printf("Ignoring file with wrong suffix: " + args[i]);
      }
    }

    if (models.size() == 0) {
      System.out.println("No models specified\n");
      return;
    }

    int rows = (int) Math.sqrt(models.size());
    int cols = models.size() / rows + 1;

    //        VzGrid.addGrid(vw);

    VisWorld.Buffer vb = vw.getBuffer("models");
    for (int y = 0; y < rows; y++) {
      for (int x = 0; x < cols; x++) {
        int idx = y * cols + x;
        if (idx >= models.size()) break;

        VzOFF model = models.get(idx);

        double mx =
            Math.max(
                model.xyz_max[2] - model.xyz_min[2],
                Math.max(model.xyz_max[1] - model.xyz_min[1], model.xyz_max[0] - model.xyz_min[0]));

        vb.addBack(
            new VisChain(
                LinAlg.translate(x + .5, rows - (y + .5), 0),
                new VzRectangle(1, 1, new VzLines.Style(Color.white, 3)),
                new VisChain(
                    LinAlg.translate(0, .4, 0),
                    new VzText(
                        VzText.ANCHOR.CENTER,
                        String.format("<<sansserif-20,scale=.003>>%s", baseName(model.path)))),
                LinAlg.scale(.5, .5, .5),
                LinAlg.scale(1.0 / mx, 1.0 / mx, 1.0 / mx),
                LinAlg.translate(
                    -(model.xyz_max[0] + model.xyz_min[0]) / 2.0,
                    -(model.xyz_max[1] + model.xyz_min[1]) / 2.0,
                    -(model.xyz_max[2] + model.xyz_min[2]) / 2.0),
                model));
      }
    }

    vb.swap();

    vl.cameraManager.fit2D(new double[] {0, 0, 0}, new double[] {cols, rows, 0}, true);
    ((DefaultCameraManager) vl.cameraManager).interfaceMode = 3.0;

    f.add(vc);
    f.setSize(600, 400);
    f.setVisible(true);
  }