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);
}
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);
}
}