particlefilteroverlay.java

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

package org.placelab.demo.mapview;

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

import org.eclipse.swt.SWT;
import org.placelab.client.PlacelabWithProxy;
import org.placelab.client.tracker.CompoundTracker;
import org.placelab.client.tracker.Estimate;
import org.placelab.client.tracker.Tracker;
import org.placelab.core.Measurement;
import org.placelab.particlefilter.ParticleFilterVisualizer;
import org.placelab.particlefilter.VisualizableParticleFilter;
import org.placelab.util.swt.Glyph;

public class ParticleFilterOverlay extends MapViewOverlay  {
    

    protected ParticleFilterVisualizer pfVisualizers[] = null;
    protected Tracker tracker = null;
    
    public ParticleFilterOverlay(MapView view, PlacelabWithProxy daemon) {
        super(view);
        setTracker(daemon.getTracker());
    }
    
    public ParticleFilterOverlay(MapView view, Tracker _tracker) {
        super(view);
        setTracker(_tracker);
    }

    public void setDaemon(PlacelabWithProxy daemon) {
        setTracker(daemon.getTracker());
    }
    
    public void setTracker(Tracker _tracker) {
        tracker = _tracker;
        createParticleFilterVisualizers();
    }
    
    /* (non-Javadoc)
     * @see org.placelab.demo.mapview.MapViewOverlay#mapChanged(org.placelab.demo.mapview.MapBacking)
     */
    public void mapChanged(MapBacking newMap) {
		if (pfVisualizers != null) {
			for (int i=0; i < pfVisualizers.length; i++) {
				
				holder.getChild().moveAbove(pfVisualizers[i],
							    null);
			}
		}
    }

    /* (non-Javadoc)
     * @see org.placelab.demo.mapview.MapViewOverlay#getTopGlyph()
     */
    public Glyph getTopGlyph() {
        if(pfVisualizers != null)
            return pfVisualizers[pfVisualizers.length - 1];
        else
            return null;
    }

    /* (non-Javadoc)
     * @see org.placelab.demo.mapview.MapViewOverlay#getName()
     */
    public String getName() {
        return "particle filter visualizer";
    }
    
    public void setVisible(boolean flag) {
        if(flag == isVisible) return;
        super.setVisible(flag);
		if (pfVisualizers != null) {
			for (int i=0; i<pfVisualizers.length; i++)
				pfVisualizers[i].setVisible
					(flag);
		}
    }

    /* (non-Javadoc)
     * @see org.placelab.client.tracker.EstimateListener#estimateUpdated(org.placelab.client.tracker.Tracker, org.placelab.client.tracker.Estimate, org.placelab.core.Measurement)
     */
    public void estimateUpdated(Tracker t, Estimate e, Measurement m) {

    }

    public void addParticleFilterVisualizer(Tracker t) {
		ParticleFilterVisualizer v;
		v = new ParticleFilterVisualizer(holder, SWT.NONE);
		v.open(view, t);
		v.setVisible(false);

//		if (t instanceof NonAdaptivePFTracker) {
//			v.setWeightedVisualizationColors(view.getDisplay().getSystemColor(SWT.COLOR_WHITE), view.getDisplay().getSystemColor(SWT.COLOR_GREEN));
//			v.setWeightedVisualization(true);
//		}

		ParticleFilterVisualizer arr[] = pfVisualizers;
		pfVisualizers = new ParticleFilterVisualizer
			[(arr==null ? 0 : arr.length)+1];
		for (int i=0; i < (arr==null ? 0 : arr.length); i++)
			pfVisualizers[i] = arr[i];
		pfVisualizers[(arr==null ? 0 : arr.length)] = v;
	}

	public void createParticleFilterVisualizers() {
		if (pfVisualizers != null) {
			for (int i=0; i < pfVisualizers.length; i++)
				pfVisualizers[i].dispose();
		}

		if (tracker instanceof VisualizableParticleFilter){
			pfVisualizers = new ParticleFilterVisualizer[1];
			pfVisualizers[0] = new ParticleFilterVisualizer
				(holder, SWT.NONE);
			pfVisualizers[0].open(view, tracker);
			pfVisualizers[0].setVisible(false);
		} 
		else if (tracker instanceof CompoundTracker) {
			Vector list = ((CompoundTracker)tracker).getTrackers();
			Enumeration it;
			int size=0;
			for (it = list.elements(); it.hasMoreElements(); ) {
				Tracker t = (Tracker)it.nextElement();
				if (t instanceof VisualizableParticleFilter)
					size++;
			}
			int i=0;
			pfVisualizers = new ParticleFilterVisualizer[size];
			for (it = list.elements(); it.hasMoreElements(); ) {
				Tracker t = (Tracker)it.nextElement();
				if (!(t instanceof VisualizableParticleFilter)) {
					continue;
				}
				
				pfVisualizers[i] = new ParticleFilterVisualizer
					(holder, SWT.NONE);
				pfVisualizers[i].open(view, t);
				pfVisualizers[i].setVisible(false);

//				if (t instanceof NonAdaptivePFTracker) {
//					pfVisualizers[i].setWeightedVisualizationColors(view.getDisplay().getSystemColor(SWT.COLOR_WHITE), view.getDisplay().getSystemColor(SWT.COLOR_GREEN));
//					pfVisualizers[i].setWeightedVisualization(true);
//				}
				i++;
			}
		}
	}

    
}