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 List<Vector2D> getShapeForDrawing() {
List<Vector2D> theList = new ArrayList<Vector2D>();
theList.add(d.getPosition());
for (Node n : Arrays.asList(dt, tv, v)) {
theList.add(n.getPosition());
}
return theList;
}
/**
* Compute the (inward) normal of each node by taking a weighted average of the (inward) surface
* normals of the edges connected to it. Longer edges are given more weight.
*/
private void computeWeightedNormals() {
for (int i = 0, n = nodeList.size(); i < n; i++) {
Node prevNode = nodeList.get(i);
Node curNode = nodeList.get((i + 1) % n);
Node nextNode = nodeList.get((i + 2) % n);
Vector2D prevVector = curNode.getPosition().subtract(prevNode.getPosition()).rotate270();
Vector2D nextVector = nextNode.getPosition().subtract(curNode.getPosition()).rotate270();
normals.put(curNode, prevVector.add(nextVector).normalize());
}
}
public static String getInheritedAttribute(
String name, DOMInput in, List<Map<String, String>> styles) {
List<String> values = in.getInheritedAttribute(name);
for (int i = values.size() - 1; i >= 0; i--) {
if (values.get(i) != null) {
return values.get(i);
}
if (styles.get(i) != null && styles.get(i).containsKey(name)) {
return styles.get(i).get(name);
}
}
return null;
}
/**
* Compute the (inward) normal at each node by averaging the unit direction vectors of the two
* edges connected to it.
*/
private void computeNormals() {
for (int i = 0, n = nodeList.size(); i < n; i++) {
Node prevNode = nodeList.get(i);
Node curNode = nodeList.get((i + 1) % n);
Node nextNode = nodeList.get((i + 2) % n);
Vector2D prevEdge = curNode.getPosition().subtract(prevNode.getPosition());
Vector2D nextEdge = nextNode.getPosition().subtract(curNode.getPosition());
double crossProd = prevEdge.crossMag(nextEdge);
Vector2D normal =
prevEdge.normalize().subtract(nextEdge.normalize()).scaleTo(Math.signum(crossProd));
normals.put(curNode, normal);
}
}
public void updateInfluences() {
computeArea();
pressureShapes.clear();
pressure.update();
dorsal.update();
transversal.update();
ventral.update();
}
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);
}
}
private void addTileOrDescendands(MeshTile tile, List<MeshTile> tileList) {
if (!tile.getSector().intersects(this.levelSet.getSector())) return;
if (tile.getLevelNumber() == this.visibleLevel) {
tileList.add(tile);
return;
}
if (this.levelSet.isFinalLevel(tile.getLevelNumber())) {
return;
}
MeshTile[] subTiles = tile.subdivide(this.levelSet.getLevel(tile.getLevelNumber() + 1));
for (MeshTile subTile : subTiles) {
addTileOrDescendands(subTile, tileList);
}
}
public void navigateRobot() {
System.out.println("Path Index: " + pathIndex);
System.out.println("goalPath Size: " + goalPath.size());
System.out.println("locX, locY, locTheta" + locX + ", " + locY + ", " + locTheta);
System.out.println("goalX, goalY, goalTheta: " + goalX + ", " + goalY + ", " + goalTheta);
if (goalPath.size() > 0 && !done) {
if (pathIndex < goalPath.size() - 1
&& Math.abs(goalX - locX) < 0.2
&& Math.abs(goalY - locY) < 0.2) {
pathIndex++;
}
if (pathIndex == goalPath.size() - 1
&& Math.abs(goalX - locX) < 0.05
&& Math.abs(goalY - locY) < 0.05) {
pathIndex++;
}
if (pathIndex >= goalPath.size()) {
done = true;
setVelocities(0.0, 0.0);
}
goalX = goalPath.get(pathIndex).x;
goalY = goalPath.get(pathIndex).y;
goalTheta = Math.atan2(goalY - locY, goalX - locX);
if (goalTheta < 0) {
goalTheta += 2 * Math.PI;
}
double error = goalTheta - locTheta;
if (error > Math.PI) {
error -= 2 * Math.PI;
} else if (error < -Math.PI) {
error += 2 * Math.PI;
}
if (Math.abs(error) < Math.PI / 8.0) {
setVelocities(0.3, anglePID.update(error));
} else {
setVelocities(0, anglePID.update(error));
}
// setVelocities(0.2, anglePID.update(error));
}
}
// public Point2D.Double localize(double odo_x, double odo_y, double odo_theta, List<Fiducial>
// fiducials){
public Point2D.Double localize(double odo_x, double odo_y, List<Fiducial> fiducials) {
double x_disp = odo_x - prev_odo_x;
double y_disp = odo_y - prev_odo_y;
prev_odo_x = odo_x;
prev_odo_y = odo_y;
// double theta_disp = odo_theta - prev_odo_theta;
// prev_odo_theta = odo_theta;
List<Point2D.Double> new_particles = new ArrayList<Point2D.Double>();
List<Double> new_probs = new ArrayList<Double>();
List<Double> cumulative_probs = new ArrayList<Double>();
Point2D.Double curr_point;
double curr_prob, distance, mapx, mapy;
double probsum = 0;
for (int i = 0; i < NUM_PARTICLES; i++) {
// Motion Update
curr_point = new_particles.get(i);
new_particles.add(new Point2D.Double(curr_point.x + x_disp, curr_point.y + y_disp));
// Sensor Update
curr_prob = 1;
for (Fiducial fid : fiducials) {
mapx = fiducialPairs.get(fid.colors).x;
mapy = fiducialPairs.get(fid.colors).y;
distance =
Math.sqrt(
(curr_point.x + x_disp - mapx) * (curr_point.x + x_disp - mapx)
+ (curr_point.y + y_disp - mapy) * (curr_point.y + y_disp - mapy));
curr_prob *=
(Math.exp(-0.5 * (fid.distance - distance) * (fid.distance - distance) / SIGMA)
/ Math.sqrt(2 * Math.PI * SIGMA * SIGMA));
}
probsum += curr_prob;
new_probs.add(curr_prob);
cumulative_probs.add(probsum);
}
particles.clear();
double rand_val;
int index;
int max_index = 0;
for (int i = 0; i < NUM_PARTICLES; i++) {
rand_val = probsum * Math.random();
index = Math.abs(Collections.binarySearch(cumulative_probs, rand_val));
particles.add(new_particles.get(index));
if (new_probs.get(max_index) < new_probs.get(i)) {
max_index = i;
}
}
localPoint = new_particles.get(max_index);
return localPoint;
}
public OdoPoint localize(
double odo_x, double odo_y, double odo_theta, List<FiducialBlob> fiducials) {
double x_disp = odo_x - prev_odo_x;
double y_disp = odo_y - prev_odo_y;
double theta_disp = odo_theta - prev_odo_theta;
prev_odo_x = odo_x;
prev_odo_y = odo_y;
prev_odo_theta = odo_theta;
localPoint =
new OdoPoint(localPoint.x + x_disp, localPoint.y + y_disp, localPoint.theta + theta_disp);
List<FiducialBlob> revFiducials = new ArrayList<FiducialBlob>();
double mapx, mapy;
Color[] colors = new Color[2];
Fiducial mapFid;
double mapbearing;
double localAngle = localPoint.theta;
localAngle = localAngle - ((int) (localAngle / (2 * Math.PI)));
if (localAngle > Math.PI) {
localAngle -= 2 * Math.PI;
}
List<OdoPoint> new_particles = new ArrayList<OdoPoint>();
List<Double> new_probs = new ArrayList<Double>();
List<Double> cumulative_probs = new ArrayList<Double>();
OdoPoint curr_point;
double curr_prob, distance, fid_distance, fid_bearing;
double probsum = 0;
// Filter out unreasonable fiducials
for (FiducialBlob fid : fiducials) {
colors[0] = fid.colors[0];
colors[1] = fid.colors[1];
if (findFiducial(colors[0], colors[1]) != null) {
mapFid = findFiducial(colors[0], colors[1]);
mapx = mapFid.getPosition().x;
mapy = mapFid.getPosition().y;
mapbearing = Math.atan2(mapy - localPoint.y, mapx - localPoint.x);
distance =
Math.sqrt(
(localPoint.x - mapx) * (localPoint.x - mapx)
+ (localPoint.y - mapy) * (localPoint.y - mapy));
fid_distance = ImageBlob.computeDistance(fid.size, mapFid.getTopSize());
System.out.println(
"Localization: map and camera bearing = " + mapbearing + ", " + localAngle);
System.out.println(
"Localization: map and camera distance = " + distance + ", " + fid_distance);
if ((Math.abs(mapbearing - localAngle) < Math.PI / 6
|| Math.abs(mapbearing - localAngle + 2 * Math.PI) < Math.PI / 6
|| Math.abs(mapbearing - localAngle - 2 * Math.PI) < Math.PI / 6)
&& fid_distance < distance * 1.3
&& distance < fid_distance * 1.3) {
System.out.println("Localization: added fiducial");
revFiducials.add(fid);
} else {
System.out.println("Localization: eliminated fiducial from bearing and distance");
}
} else {
System.out.println("Localization: eliminated fiducial from colors");
}
}
/*// Use Odometry if no fiducials
if (revFiducials.size() == 0){
System.out.println("Localization: no fiducials to localize with");
particles.clear();
OdoPoint curr_point;
for (int i = 0; i < NUM_PARTICLES; i++){
curr_point = particles.get(i);
particles.add(new OdoPoint(curr_point.x + x_disp, curr_point.y + y_disp, curr_point.theta + theta_disp));
}
return localPoint;
}*/
for (int i = 0; i < NUM_PARTICLES; i++) {
// Motion Update
curr_point = particles.get(i);
new_particles.add(
new OdoPoint(
curr_point.x + x_disp, curr_point.y + y_disp, curr_point.theta + theta_disp));
// Sensor Update
curr_prob = 1;
for (FiducialBlob fid : revFiducials) {
colors[0] = fid.colors[0];
colors[1] = fid.colors[1];
mapFid = findFiducial(colors[0], colors[1]);
mapx = mapFid.getPosition().x;
mapy = mapFid.getPosition().y;
distance =
Math.sqrt(
(curr_point.x + x_disp - mapx) * (curr_point.x + x_disp - mapx)
+ (curr_point.y + y_disp - mapy) * (curr_point.y + y_disp - mapy));
fid_distance = ImageBlob.computeDistance(fid.size, mapFid.getTopSize());
curr_prob *=
NORMALIZATION
* (Math.exp(
-0.5 * (fid_distance - distance) * (fid_distance - distance) / DIST_SIGMA)
/ Math.sqrt(2 * Math.PI * DIST_SIGMA * DIST_SIGMA));
fid_bearing = curr_point.theta + theta_disp - ImageBlob.computeBearing(fid.x);
fid_bearing = fid_bearing - ((int) (fid_bearing / (2 * Math.PI)));
if (fid_bearing > Math.PI) {
fid_bearing -= 2 * Math.PI;
}
mapbearing = Math.atan2(mapy - curr_point.y - y_disp, mapx - curr_point.x - x_disp);
curr_prob *=
NORMALIZATION
* (Math.exp(
-0.5 * (mapbearing - fid_bearing) * (mapbearing - fid_bearing) / ROT_SIGMA)
/ Math.sqrt(2 * Math.PI * ROT_SIGMA * ROT_SIGMA));
}
probsum += curr_prob;
new_probs.add(curr_prob);
cumulative_probs.add(probsum);
}
if (revFiducials.size() > 0) {
System.out.println("Localization: localizing with fiducials");
particles.clear();
double rand_val;
int index;
int max_index = 0;
for (int i = 0; i < NUM_PARTICLES; i++) {
rand_val = probsum * Math.random();
index = Collections.binarySearch(cumulative_probs, rand_val);
if (index < 0) {
index = -(index + 1);
}
particles.add(new_particles.get(index));
if (new_probs.get(max_index) < new_probs.get(i)) {
max_index = i;
}
}
localPoint = new_particles.get(max_index);
} else {
System.out.println("Localization: no fiducials to localize with");
particles = new_particles;
}
return localPoint;
}
