private Fiducial findFiducial(Color bottom, Color top) {
for (Fiducial fid : fiducialPairs) {
if (fid.getBottomColor() == bottom && fid.getTopColor() == top) {
return fid;
}
}
return null;
}
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;
}