particlefiltervisualizer.java

一个基于PlaceLab的室内和室外的智能导航系统

package org.placelab.particlefilter;

import java.util.Enumeration;
import java.util.Vector;

import org.eclipse.swt.events.PaintEvent;
import org.eclipse.swt.graphics.Color;
import org.eclipse.swt.graphics.Point;
import org.eclipse.swt.graphics.Rectangle;
import org.placelab.client.tracker.Estimate;
import org.placelab.client.tracker.EstimateListener;
import org.placelab.client.tracker.FilteredEstimateListener;
import org.placelab.client.tracker.Tracker;
import org.placelab.core.Measurement;
import org.placelab.core.TwoDCoordinate;
import org.placelab.demo.mapview.MapView;
import org.placelab.demo.mapview.MapViewOverlay;
import org.placelab.particlefilter.beacon.HybridMotionModel;
import org.placelab.particlefilter.beacon.PositionParticle;
import org.placelab.particlefilter.beacon.PositionWithMotionParticle;
import org.placelab.util.swt.Glyph;
import org.placelab.util.swt.GlyphComposite;
import org.placelab.util.swt.GlyphGC;
import org.placelab.util.swt.GlyphHolder;

public class ParticleFilterVisualizer extends Glyph implements EstimateListener {

	protected MapView view=null;
	protected Tracker tracker=null;
	protected Color color=null, color2=null;
	protected boolean visible=true;
	protected boolean weightedVisualization=false;
	protected Rectangle particleBounds=null;
	private int PARTICLE_DRAW_SIZE = 3;

	public ParticleFilterVisualizer(GlyphComposite parent, int style) {
		super(parent, style);
	}

	public ParticleFilterVisualizer(GlyphHolder holder, int style) {
		super(holder, style);
	}
	
	public void open(MapView view, Tracker t) {
		this.view = view;
		if (!(t instanceof VisualizableParticleFilter)) {
			throw new IllegalArgumentException("Tracker must implement VisualizableParticleFilter");
		}
		this.tracker = t;
		t.addEstimateListener(new FilteredEstimateListener(this, MapViewOverlay.FILTER_TIME, FilteredEstimateListener.FILTER_BY_TIME));
		try {
			PARTICLE_DRAW_SIZE = Integer.parseInt(
					  System.getProperty("placelab.particlefiltervis.particlesize"));
		} catch (Exception ex) {;}	

		/*bounds = new Rectangle(0, 0, backing.getImageResource().width, 
		  backing.getImageResource().height);*/
		particleBounds = new Rectangle(0, 0, 0, 0);
		//setZoom(getParent().getZoom());
	}
	public void setForeground(Color c) {
		color = c;
		if (isOpened()) redraw(null);
	}
	public void setWeightedVisualizationColors(Color zero, Color one) {
		color = one;
		color2= zero;
		if (isOpened()) redraw(null);
	}
	public void setWeightedVisualization(boolean flag) {
		if (weightedVisualization != flag) {
			weightedVisualization = flag;
			if (isOpened()) redraw(null);
		}
	}
	public boolean getWeightedVisualization() {
		return weightedVisualization;
	}
	public boolean isOpened() {
		return (tracker!=null || view!=null);
	}
	
	protected Vector getParticleList() {
		if ((tracker == null) || (!(tracker instanceof VisualizableParticleFilter)))
		    return null;
		return ((VisualizableParticleFilter)tracker).getParticleList();
	}


	public Point getParticlePixels(PositionParticle particle) {
		TwoDCoordinate coord = particle.getPosition();

		int x = 0, y = 0;
		if(view != null) {
			x= view.longitudeToPixels(coord.getLongitude());
			y= view.latitudeToPixels (coord.getLatitude ());
		} else {
		    // doesn't make sense outside a mapview anymore
			x = -1;
			y = -1;
		}
		x = (int)(x * getZoom() + 0.5);
		y = (int)(y * getZoom() + 0.5);
		return new Point(x, y);
	}
	public void estimateUpdated(Tracker t, Estimate e, Measurement m) {
		if (!isOpened() || !visible) return;

	 Vector particles = getParticleList();
		Rectangle newBounds = null;
		if (particles != null) {
			for (Enumeration it=particles.elements(); it.hasMoreElements(); ) {
				Particle p = (Particle) it.nextElement();
				if (! (p instanceof PositionParticle)) continue;
				Point point = getParticlePixels((PositionParticle)p);
				if (newBounds == null) 
					newBounds = new Rectangle(point.x, point.y, 
								  2, 2);
				else
					newBounds = newBounds.union
						(new Rectangle(point.x, point.y,2,2));
			}
		}
		if (newBounds==null) newBounds = new Rectangle(0,0,0,0);
		Rectangle oldBounds = particleBounds;
		particleBounds = newBounds;
		redraw(oldBounds);
	}
	public Rectangle getBoundsImpl() {
		if (!isOpened() || !visible)
			return new Rectangle(0, 0, 0, 0);
		else
			return new Rectangle(particleBounds.x, 
					     particleBounds.y,
					     particleBounds.width, 
					     particleBounds.height);
		/*0, 0, (int)(bounds.width*getZoom()+0.5),
		  (int)(bounds.height*getZoom()+0.5));*/
	}
	public boolean pointInsideImpl(int x, int y) {
		return false;
	}
	public void paintImpl(PaintEvent e, GlyphGC gc) {
		if (!visible || !isOpened()) return;
					
		Color fg=null, bg=null;
		if (color != null) {
			fg = gc.getForeground();
			bg = gc.getBackground();
		}
		Color magenta = new Color(null, 255, 0, 255);
		
	 Vector particles = getParticleList();
		int sumx=0, sumy=0;
		int index = 0;
		if (particles != null) {
			for (Enumeration it=particles.elements(); it.hasMoreElements(); ) {
				index++;
				Particle p = (Particle) it.nextElement();
				if (!(p instanceof PositionParticle)) continue;
				PositionParticle particle = (PositionParticle) p;
				Point topLeftofStdev = null;
				
				if (particle instanceof RBPFParticle) {
					RBPFParticle rbp = (RBPFParticle)particle;
					PositionParticle clone = new PositionParticle(particle);
					clone.getPosition().moveBy(-rbp.getStdev()*2.0, rbp.getStdev()*2.0);
					topLeftofStdev = getParticlePixels(clone);
				}
				
				
				Point point = getParticlePixels(particle);
				sumx += point.x; sumy += point.y;
				int wd = PARTICLE_DRAW_SIZE;
				
				Color cc = magenta;
				if (PARTICLE_DRAW_SIZE != 3) { // show particles of different state differently
					PositionWithMotionParticle pp = (PositionWithMotionParticle) p;
					if (pp.state == HybridMotionModel.USE_CENTROID) {
						cc = new Color(null, 0, 255, 0);
					}
				}
				Rectangle pr = new Rectangle(point.x-wd, point.y-wd, wd*2, wd*2);				
				gc.setForeground(cc);
				gc.setBackground(cc);
				
				gc.fillRectangle(pr.x, pr.y, pr.width, pr.height);
				if (topLeftofStdev != null) { // draw the stdev ring
					int width = 2*(point.x-topLeftofStdev.x);
					int height = 2*(point.y-topLeftofStdev.y);
					gc.drawOval(topLeftofStdev.x, topLeftofStdev.y, width, height);
				}
				
			}
		} else {
			System.out.println("Error: particles Vector is null");
		}
		if (color != null) {
			gc.setForeground(fg);
			gc.setBackground(bg);
		}
	}
	public void setVisible(boolean v) { 
		visible = v; 
		redraw(null);
	}
	public boolean getVisible() { return visible; }
}