...

HistogramFilter/bin/.gitignore

@@ -1,2 +1,9 @@
 /config/
 /robots/
+/ProbabilityMatriz$1.class
+/MainProgram$2.class
+/MainProgram$1.class
+/ProbabilityMatriz$MatrizIterator.class
+/ProbabilityMatriz$MatrixIterator.class
+/MatrizInterator.class
+/ProbabilityMatrizData.class

HistogramFilter/src/MainProgram.java

@@ -4,6 +4,7 @@ import java.awt.event.KeyListener;
 import java.awt.event.WindowEvent;
 import java.awt.event.WindowListener;
 import java.util.ArrayList;
+import java.util.Iterator;
 
 import javax.swing.JFrame;
 import javax.swing.JOptionPane;
@@ -60,15 +61,15 @@ public class MainProgram extends JPanel implements KeyListener, WindowListener,
 		splitPane.setDividerLocation((int) (frame.getHeight() / 2));
 		frame.add(splitPane);
 
-		matriz = new ProbabilityMatriz(map.getBoundingRect(), 5, 10);
-//		
-//		for (DiscretePoint p: matriz) {
-////			System.out.println(n(p.midAng()));
-//			if (p.z == 2 && p.x == 5 && p.y == 5)
-//				p.set(1);
-//			else
-//				p.set(0);
-//		}
+		matriz = new ProbabilityMatriz(map.getBoundingRect(), 10, 7);
+		
+		for (DiscretePoint p: matriz) {
+//			System.out.println(n(p.midAng()));
+			if (p.z == 0 && p.x == 5 && p.y == 5)
+				p.set(1);
+			else
+				p.set(0);
+		}
 		
 		imap.showMatriz(matriz);
 //		showHelp();
@@ -204,26 +205,8 @@ public class MainProgram extends JPanel implements KeyListener, WindowListener,
 			String rs = JOptionPane.showInputDialog("Enter rotation (degress-clockwise):");
 			double r = Double.parseDouble(rs);
 			robot.rotate(r);
-			
-			/* PROBABILIDADES DEPOIS MOVIMENTO */
-			ProbabilityMatriz newmatriz = new ProbabilityMatriz(matriz.bounds, matriz.ssize, matriz.alphad);
-			System.out.print("Iniciando...");
-			for (DiscretePoint newp: newmatriz) {
-				double sum = 0;
-				for (DiscretePoint p: matriz) {
-					Pose pa = p.pose();
-					Pose pb = newp.pose();
-					sum += model.rotateProbability(pa, pb, r)*p.get();
-				}
-				newp.set(sum);
-			}
-			System.out.println("Finalizado");
-			matriz = newmatriz;
-			imap.setMatriz(matriz);
-			matriz.normalize();
-
-			scanner.clean();
-			imap.repaint();
+		
+			rotateAndMoveRobot(0, r);
 		} catch (Exception e) {
 		}
 	}
@@ -234,29 +217,69 @@ public class MainProgram extends JPanel implements KeyListener, WindowListener,
 			double r = Double.parseDouble(rs);
 			robot.move(r);
 			
-			/* PROBABILIDADES DEPOIS MOVIMENTO */
-			ProbabilityMatriz newmatriz = new ProbabilityMatriz(matriz.bounds, matriz.ssize, matriz.alphad);
-			System.out.print("Iniciando...");
-			for (DiscretePoint newp: newmatriz) {
-				double sum = 0;
-				for (DiscretePoint p: matriz) {
-					Pose pa = p.pose();
-					Pose pb = newp.pose();
-					sum += model.moveProbability(pa, pb, r)*p.get();
+			rotateAndMoveRobot(r, 0);
+		} catch (Exception e) {
+		}
+	}
+	
+	private void rotateAndMoveRobot(double move, double ang) {
+		/* PROBABILIDADES DEPOIS MOVIMENTO */
+		ProbabilityMatriz newmatriz = new ProbabilityMatriz(matriz.bounds, matriz.ssize, matriz.alphad);
+		System.out.print("Iniciando...");
+		
+		// Verifica o numero de cores
+		int cores = Runtime.getRuntime().availableProcessors();
+		
+		// tudo isso
+		ArrayList<Thread> threads = new ArrayList<Thread>();
+		for (int i = 0; i < cores; i++) {
+			Iterator<DiscretePoint> iteretor = newmatriz.iteratorSet((newmatriz.totalsize*i)/cores, newmatriz.totalsize*(i+1)/cores);
+			Thread thread = new Thread () {
+				public void run() {
+					while(iteretor.hasNext()) {
+						DiscretePoint newp = iteretor.next();
+						double sum = 0;
+						for (DiscretePoint p: matriz) {
+							Pose pa = p.pose();
+							Pose pb = newp.pose();
+							sum += model.motionModelOdometry(pa, pb, move, ang)*p.get();
+						}
+						newp.set(sum);
+					}
 				}
-				newp.set(sum);
+			};
+			thread.start();
+			threads.add(thread);
+		}
+		
+		for (Thread thread: threads) {
+			try {
+				thread.join();
+			} catch (InterruptedException e) {
 			}
-			System.out.println("Finalizado");
-			matriz = newmatriz;
-			matriz.normalize();
-			imap.setMatriz(matriz);
-	
-			scanner.clean();
-			imap.repaint();
-		} catch (Exception e) {
 		}
+		
+		// OU ISSO
+//		for (DiscretePoint newp: newmatriz) {
+//			double sum = 0;
+//			for (DiscretePoint p: matriz) {
+//				Pose pa = p.pose();
+//				Pose pb = newp.pose();
+//				sum += model.motionModelOdometry(pa, pb, move, ang)*p.get();
+//			}
+//			newp.set(sum);
+//		}
+//		
+		System.out.println("Finalizado");
+		matriz = newmatriz;
+		imap.setMatriz(matriz);
+		matriz.normalize();
+
+		scanner.clean();
+		imap.repaint();
 	}
 
+
 	@Override
 	public void keyReleased(KeyEvent arg0) {
 		if (robot == null)

HistogramFilter/src/ProbabilityMatriz.java

@@ -3,6 +3,39 @@ import java.util.Iterator;
 import lejos.geom.Rectangle;
 
 public class ProbabilityMatriz implements Iterable<DiscretePoint> {
+	public class MatrizIterator implements Iterator<DiscretePoint> {
+		private final ProbabilityMatriz m;
+		public long count = 0;
+		public long end = totalsize;
+
+		public MatrizIterator(ProbabilityMatriz m) {
+			this.m = m;
+		}
+
+		public void set (long start, long end) {
+			this.end = end;
+			this.count = start;
+		}
+
+		@Override
+		public boolean hasNext() {
+			return count < end;
+		}
+
+		@Override
+		public DiscretePoint next() {
+			int b = matriz[0].length;
+			int c = matriz[0][0].length;
+			
+			int x = (int) (count/(b*c));
+			int y = (int) (count%(c*b)/c);
+			int z = (int) (count%c);
+			DiscretePoint tmp = new DiscretePoint(m, x, y, z);
+			count++;
+			return tmp;
+		}
+	}
+
 	/*
 	 * Recebe as bordas dos triangulos da matriz (bounds)
 	 * ssize - squere size, tamanho do quadrado utilizado na discretizacao
@@ -66,28 +99,7 @@ public class ProbabilityMatriz implements Iterable<DiscretePoint> {
 	@Override
 	public Iterator<DiscretePoint> iterator() {
 		ProbabilityMatriz m = this;
-		return new Iterator<DiscretePoint>() {
-			long count = 0;
-			
-			@Override
-			public boolean hasNext() {
-				return count != totalsize;
-			}
-
-			@Override
-			public DiscretePoint next() {
-				int b = matriz[0].length;
-				int c = matriz[0][0].length;
-				
-				int x = (int) (count/(b*c));
-				int y = (int) (count%(c*b)/c);
-				int z = (int) (count%c);
-				DiscretePoint tmp = new DiscretePoint(m, x, y, z);
-				count++;
-				return tmp;
-			}
-			
-		};
+		return new MatrizIterator(m);
 	}
 
 	public void set(double v, int x, int y, int z) {
@@ -97,4 +109,12 @@ public class ProbabilityMatriz implements Iterable<DiscretePoint> {
 	public double get(int x, int y, int z) {
 		return matriz[x][y][z];
 	}
+
+
+	public Iterator<DiscretePoint> iteratorSet(long start, long end) {
+		ProbabilityMatriz m = this;
+		MatrizIterator it = new MatrizIterator(m);
+		it.set(start, end);
+		return new MatrizIterator(m);
+	}
 }

HistogramFilter/src/config/Models.java

@@ -8,20 +8,39 @@ import lejos.robotics.navigation.Pose;
 import robots.DataRead;
 
 public class Models {
+	
+	private static boolean USE_TRIANGULAR = false;
+	private static int SONAR_CONE = 10;
+	private static double SONAR_SIGMA = 10;
+	
+	private static double ALPHA1 = 0.1;		// desvio padrao do erro no gito
+	private static double ALPHA2 = 0.00015;	// desvio padrao do erro qunado gira proporconal ao deslocalmento
+	private static double ALPHA3 = 0.1;		// desvio padrao no erro do deslocamento
+	private static double ALPHA4 = 0.01;    // desvio padrao no erro do deslocamento baseado em quanto rodou (nao importa para o nosso robo)
+	
 	LineMap map;
 	public Models(LineMap map) {
 		this.map = map;
 	}
 	
-    // return pdf(x) = standard Gaussian pdf
-    private static double pdf(double x) {
-        return Math.exp(-x*x / 2) / Math.sqrt(2 * Math.PI);
-    }
-
     // return pdf(x, mu, signma) = Gaussian pdf with mean mu and stddev sigma
-    private static double pdf(double x, double mu, double sigma) {
-        return pdf((x - mu) / sigma) / sigma;
+    private static double pdf(double x, double sigma) {
+    		if (USE_TRIANGULAR) {
+    			double r = Math.sqrt(sigma*6);
+    			double abs = Math.abs(x);
+    			if (abs > r) return 0;
+    			return (r-abs)/(6*sigma);
+    		}
+        return Math.exp(-x*x / 2 / sigma) / Math.sqrt(2 * Math.PI * sigma);
     }
+
+	private double probTriangularDistribution (double a, double b) {
+		double r = Math.sqrt(b*6);
+		double abs = Math.abs(a);
+		if (abs > r) return 0;
+		return (r-abs)/(6*b);
+	}
+	
 	
 	/* retorna a probabilidade do robo estar em (ap) sendo que ele saiu de (bp) 
 	 * com um movimento de rotacao de ang graus.*/
@@ -35,17 +54,12 @@ public class Models {
 		return motionModelOdometry(pa, pb, move, 0);
 	}
 	
-	private double probTriangularDistribution (double a, double b) {
-		double r = Math.sqrt(b*6);
-		double abs = Math.abs(a);
-		if (abs > r) return 0;
-		return (r-abs)/(6*b);
-	}
-	
+	/* retorna a probabilidade do robo estar em (pa) sendo que ele saiu de (pb) 
+	 * sendo que ele rodou ang graus e foi move centimetros para frente.*/
 	public double motionModelOdometry (Pose pa, Pose pb, double move, double ang) {
 		double drot1 = n(Math.toRadians(ang));
-		double pah = Math.toRadians(pa.getHeading() + Math.toRadians(90));
-		double pbh = Math.toRadians(pb.getHeading() + Math.toRadians(90));
+		double pah = Math.toRadians(pa.getHeading());
+		double pbh = Math.toRadians(pb.getHeading());
 		
 		double dtrans = move;
 		
@@ -53,14 +67,14 @@ public class Models {
 		double dx = pa.getX()-pb.getX();
 		double dy = pa.getY()-pb.getY();
 		
-		double dcrot1 = n(Math.atan2(dy, dx) - pbh - Math.toRadians(90));
+		double dcrot1 = n(Math.atan2(dy, dx) - Math.toRadians(90));
 		double dctrans = Math.sqrt(dx*dx+dy*dy);
-		double dcrot2 = n(pah - pbh - dcrot1);
+		double dcrot2 = n(pah - dcrot1);
 		
-		double p1 = probTriangularDistribution(n(pah-pbh-drot1), 0.001*dtrans+0.1*drot1);
-		double p2 = probTriangularDistribution(dtrans-dctrans, dtrans+0.001*drot1);
-		double p3 = probTriangularDistribution(dcrot2, 0.001*dtrans);
-		if (dtrans == 0) p3 = 1.0;
+		double p1 = probTriangularDistribution(n(pah-pbh-drot1), ALPHA2*dtrans+ALPHA1*drot1);
+		double p2 = probTriangularDistribution(dtrans-dctrans, ALPHA3*dtrans+ALPHA4*drot1);
+		double p3 = probTriangularDistribution(dcrot2, ALPHA2*dtrans);
+		if (dtrans == 0) p3 = 1;
 		return p1*p2*p3;
 	}
 	
@@ -78,7 +92,7 @@ public class Models {
 			Pose pose = new Pose(p.getX(), p.getY(), newang);
 			
 			float mindist = Float.POSITIVE_INFINITY;
-			int cone = 1; // CONE CONE
+			int cone = SONAR_CONE; // CONE CONE
 			for (int angulo = -cone / 2; angulo <= cone / 2; angulo++) {
 				pose.setHeading((float) (newang - angulo));
 				float dist = map.range(pose);
@@ -94,7 +108,7 @@ public class Models {
 				prob = 0;
 				break;
 			}
-			prob *= pdf(dp.getDistance(), teoricaldistance, 20); // desvio padrao
+			prob *= pdf(dp.getDistance()-teoricaldistance, SONAR_SIGMA); // desvio padrao
 		}
 		return prob;
 	}