public AngleLocalization(
      List<PolygonObstacle> cSpaceObstacles,
      Rectangle2D.Double cSpaceWorld,
      Point2D.Double robotStart,
      Fiducial[] fiducialPairs) {

    this.cSpaceObstacles = cSpaceObstacles;
    this.cSpaceWorld = cSpaceWorld;
    this.robotStart = robotStart;
    this.fiducialPairs = fiducialPairs;

    particles = new ArrayList<OdoPoint>();
    probabilities = new ArrayList<Double>();

    double delta_x = 0;
    double delta_y = 0;
    double delta_theta = 0;
    for (int i = 0; i < NUM_PARTICLES; i++) {
      delta_x = cSpaceWorld.getWidth() * INITIAL_NOISE * (1 - 2 * Math.random());
      delta_y = cSpaceWorld.getHeight() * INITIAL_NOISE * (1 - 2 * Math.random());
      delta_theta = 2 * Math.PI * INITIAL_NOISE * (1 - 2 * Math.random());
      particles.add(new OdoPoint(robotStart.x + delta_x, robotStart.y + delta_y, delta_theta));
      probabilities.add(1.0 / NUM_PARTICLES);
    }

    prev_odo_x = robotStart.x;
    prev_odo_y = robotStart.y;
    prev_odo_theta = 0;
    localPoint = new OdoPoint(robotStart.x, robotStart.y, 0);
  }
Beispiel #2
0
  public Localization(
      List<PolygonObstacle> cSpaceObstacles,
      Rectangle2D.Double cSpaceWorld,
      Point2D.Double robotStart,
      Map<int[], Point2D.Double> fiducialPairs) {

    this.cSpaceObstacles = cSpaceObstacles;
    this.cSpaceWorld = cSpaceWorld;
    this.robotStart = robotStart;
    this.fiducialPairs = fiducialPairs;

    particles = new ArrayList<Point2D.Double>();
    probabilities = new ArrayList<Double>();

    double delta_x = 0;
    double delta_y = 0;
    for (int i = 0; i < NUM_PARTICLES; i++) {
      delta_x = cSpaceWorld.getWidth() * INITIAL_NOISE * (2 * Math.random() - 1);
      delta_y = cSpaceWorld.getHeight() * INITIAL_NOISE * (2 * Math.random() - 1);
      particles.add(new Point2D.Double(robotStart.x + delta_x, robotStart.y + delta_y));
      probabilities.add(1.0 / NUM_PARTICLES);
    }
  }