intersectiontracker.java

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

/*
 * Created on 13-Aug-2004
 *
 */
package org.placelab.client.tracker;

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

import org.placelab.core.BeaconMeasurement;
import org.placelab.core.BeaconReading;
import org.placelab.core.Coordinate;
import org.placelab.core.FixedTwoDCoordinate;
import org.placelab.core.Measurement;
import org.placelab.core.PositionMeasurement;
import org.placelab.core.Types;
import org.placelab.mapper.Beacon;
import org.placelab.mapper.Mapper;
import org.placelab.util.FixedPointLong;
import org.placelab.util.FixedPointLongException;

/**
 * The IntersectionTracker performs basic "fusion" by taking both
 * PositionMeasurements and BeaconMeasurements and creating Estimates
 * that take both types into account.
 * <p>
 * The aims of this tracker are: 
 * <ol>
 * <li> to work on both fixed-point and floating-point platforms,
 * <li> to accept any PositionMeasurement or BeaconMeasurement,
 * <li> to simply but effectively fuse together whatever information is available
 * </ol>
 * <p>
 * It maintains a Vector of "loci" of recently seen BeaconReadings and PositionMeasurements
 * It computes Estimates by taking the intersection of the bounding boxes of the loci
 * These bounding boxes expand as the locus gets older (thus making the position less accurate)   
 * 
 * 
 *
 */
public final class IntersectionTracker extends BeaconTracker {
    // Circumference of the earth: 40075160m equatorial, 40008000m polar
    // Dividing by 360 degrees, we get the following:
	// Taken from FixedTwoDCoordinate.CoordinateModel
	private static final long LAT_TO_METERS = 111320L;
    private static final long LON_TO_METERS = 111133L;
    
    private Estimate curEst;

    long maxLife = 600000; // 10 minutes
    
    private class Locus {
        protected long initialRadiusFlong; 
        protected Coordinate centre;
        protected long timestamp;
        
        protected Coordinate southWest;
        protected Coordinate northEast;
        protected boolean useBoundingBox;
        protected long initialLatRadiusFlong;
        protected long initialLonRadiusFlong;
        
        public Locus(long timestamp,Coordinate centre,int initialRadius) {
            this.centre = centre;
            this.timestamp = timestamp;
            try {
//                System.out.println("new Locus with radius " + initialRadius);
                this.initialRadiusFlong = FixedPointLong.intToFlong(initialRadius);
            } catch(FixedPointLongException fple) {
                this.initialRadiusFlong=0;
                System.err.println("Invalid radius creating locus");
            }
        }
        
        public Locus(long timestamp,Coordinate centre, Coordinate southWest, Coordinate northEast) {
        	this.centre = centre;
            this.timestamp = timestamp;
            this.southWest = southWest;
            this.northEast = northEast;
            
            try {
	            long neLatFlong = FixedPointLong.stringToFlong(northEast.getLatitudeAsString());
	            long neLonFlong = FixedPointLong.stringToFlong(northEast.getLongitudeAsString());
	            long swLatFlong = FixedPointLong.stringToFlong(southWest.getLatitudeAsString());
	            long swLonFlong = FixedPointLong.stringToFlong(southWest.getLongitudeAsString());
	            
	            initialLatRadiusFlong = ((neLatFlong - swLatFlong)*LAT_TO_METERS)/2;
	            initialLonRadiusFlong = ((neLonFlong - swLonFlong)*LON_TO_METERS)/2;

            } catch(FixedPointLongException fple) {
            	fple.printStackTrace();
            }
            useBoundingBox = true;
        }
        
    }
    
    public IntersectionTracker(Mapper m) {
        super(m);
        loci = new Hashtable();
    }

    private Hashtable loci;
    private Object lociSyncObj = new Object();
    public Object lociSyncObject() {
        return lociSyncObj;
    }

    
    /** accepts BeaconMeasurements and PositionMeasurements
     * N.B. Overrides BeaconTracker.acceptableMeasurement
     */
    public boolean acceptableMeasurement(Measurement m) {
        if(m == null) return false;
        if(m instanceof BeaconMeasurement) return true;
        if(m instanceof PositionMeasurement) return true;
        return false;
    }
    
    /**
     * @see org.placelab.client.tracker.Tracker#updateEstimateImpl(org.placelab.core.Measurement)
     */
    protected synchronized void updateEstimateImpl(Measurement m) {
    	if(m == null) return;
        synchronized(lociSyncObject()) {
        	if(m instanceof PositionMeasurement) {
        		PositionMeasurement pm = (PositionMeasurement)m;
        		if(pm.getPosition().isNull()) return;
            	if(!loci.containsKey(pm.getType()) || pm.getTimestamp() > ((Locus)loci.get(pm.getType())).timestamp) {
            		loci.put(pm.getType(),new Locus(pm.getTimestamp(),pm.getPosition(),getInitRadius(pm)));            
            	}
            } else if(m instanceof BeaconMeasurement) {
            	Enumeration i = ((BeaconMeasurement)m).iterator();
            	while(i.hasMoreElements()) {
            		BeaconReading br = (BeaconReading) i.nextElement();
            		Beacon b = findBeacon(br.getId(),(BeaconMeasurement)m,(curEst==null || curEst.getCoord().isNull())?null:curEst.getCoord(), 0);
            		if(b==null || b.getPosition().isNull()) {
            			continue;
            		}

            		if(!loci.containsKey(b) || m.getTimestamp() > ((Locus)loci.get(b)).timestamp) {
            			Hashtable map = b.toHashMap();
            			//grab bounding box information sent to client if available
            			if(map.get("MINLAT") != null && map.get("MAXLAT") != null && 
            			   map.get("MINLON") != null && map.get("MAXLON") != null) {

            				loci.put(b,new Locus(m.getTimestamp(),b.getPosition(),
            						new FixedTwoDCoordinate((String)map.get("MINLAT"),(String)map.get("MINLON")),
									new FixedTwoDCoordinate((String)map.get("MAXLAT"),(String)map.get("MAXLON"))));
            			} else {            				
            				loci.put(b,new Locus(m.getTimestamp(),b.getPosition(),getInitRadius(b)));
            			}
            		}
            	}
            }
        }
    }
        
    private int getInitRadius(PositionMeasurement pm) {
        // should not be doing this here!
        return 50;
    }
    
    private int getInitRadius(Beacon b) {
        return b.getMaximumRange();
    }
    
    /**
     * @see org.placelab.client.tracker.Tracker#getEstimate()
     */
    public Estimate getEstimate() {
        curEst= getEstimateImpl();
        return curEst==null ? Types.newEstimate(System.currentTimeMillis(),Types.newCoordinate(),"0") : curEst;
    }
    
    public void setMaxLife(long maxLife) {
    	this.maxLife = maxLife;
    }
    
    public synchronized Estimate getEstimateImpl() {
    	// this should be in a subclass, with this class being abstract
        // begin sheer hackery
        
        // assumptions 
        final long SPEED_FLONG = FixedPointLong.intToFlongSafe(5); // 5 meters per second
        final long MAXRADIUS_FLONG = FixedPointLong.intToFlongSafe(10000); // 10km
        long north=0,south=0,east=0,west=0;
        long now = System.currentTimeMillis();
        long minRadiusFlong = 0;
        
        while (true) {
			boolean first = true;
			minRadiusFlong = 0;
			long oldestTime = 0;
			Object oldestKey = null;

			synchronized (lociSyncObject()) {
			    Vector toRemove = new Vector();
				Enumeration i = loci.keys();

				while (i.hasMoreElements()) {
					Object key = i.nextElement();
					Locus l = (Locus) loci.get(key);

//					long curRadiusFlong = l.initialRadiusFlong
//							+ ((now - l.timestamp) / 1000L) * SPEED_FLONG;

					long latRadiusFlong = 0;
					long lonRadiusFlong = 0;
					if(l.useBoundingBox) {
							latRadiusFlong = l.initialLatRadiusFlong + ((now - l.timestamp) / 1000L) * SPEED_FLONG;
							lonRadiusFlong = l.initialLonRadiusFlong + ((now - l.timestamp) / 1000L) * SPEED_FLONG;
					} else {
						//set them to be the same
						latRadiusFlong = l.initialRadiusFlong + ((now - l.timestamp) / 1000L) * SPEED_FLONG;
						lonRadiusFlong = latRadiusFlong;
					}
					//System.out.println("initial:
					// "+FixedPointLong.flongToString(l.initialRadiusFlong));
					//System.out.println("cur radius:
					// "+FixedPointLong.flongToString(curRadiusFlong));

					// cull old readings
					if ((latRadiusFlong > MAXRADIUS_FLONG && lonRadiusFlong > MAXRADIUS_FLONG)
							|| now - l.timestamp > maxLife || l.centre.isNull()) {
						toRemove.addElement(key); // remove last key
						continue;
					}
					// make sure its a flong (simplest way to be
					// coordinate-type-and-platform-agnostic)
					FixedTwoDCoordinate centre;
					if (l.centre instanceof FixedTwoDCoordinate) {
						centre = (FixedTwoDCoordinate) l.centre;
					} else {
						centre = new FixedTwoDCoordinate(l.centre
								.getLatitudeAsString(), l.centre
								.getLongitudeAsString());
						//System.out.println("Making new FixedTwoDCoordinate
						// out of " + l.centre + " : " + centre);
					}
					FixedTwoDCoordinate northEast, southWest;
					try {
						northEast = centre.translateFixed(latRadiusFlong,lonRadiusFlong);
						southWest = centre.translateFixed(-latRadiusFlong,-lonRadiusFlong);
					} catch (FixedPointLongException fple) {
						throw new ArithmeticException(
								"IntersectionTracker.getEstimateImpl: FixedPointLongException: "
										+ fple);
					}
					//System.out.println("current diagonal " +
					// northeast.distanceFromAsString(southwest));
					long curNorth = northEast.getLatitudeFlong();
					long curEast = northEast.getLongitudeFlong();
					long curSouth = southWest.getLatitudeFlong();
					long curWest = southWest.getLongitudeFlong();
					if (first) {
						north = curNorth;
						east = curEast;
						south = curSouth;
						west = curWest;
						minRadiusFlong = Math.min(latRadiusFlong,lonRadiusFlong);
						oldestTime = l.timestamp;
						oldestKey = key;
						//System.out.println("First box " +
						// FixedPointLong.flongToString(curNorth) + "," +
						// FixedPointLong.flongToString(curEast) + "," +
						// FixedPointLong.flongToString(curSouth) + "," +
						// FixedPointLong.flongToString(curWest));
						first = false;

					} else {
						//System.out.println("Merging box " +
						// FixedPointLong.flongToString(curNorth) + "," +
						// FixedPointLong.flongToString(curEast) + "," +
						// FixedPointLong.flongToString(curSouth) + "," +
						// FixedPointLong.flongToString(curWest));
						north = Math.min(north, curNorth);
						east = Math.min(east, curEast);
						south = Math.max(south, curSouth);
						west = Math.max(west, curWest);
						minRadiusFlong = Math.min(minRadiusFlong,Math.min(latRadiusFlong,lonRadiusFlong));
						if (oldestTime > l.timestamp) {
							oldestTime = l.timestamp;
							oldestKey = key;
						}
						//System.out.println("Result box " +
						// FixedPointLong.flongToString(north) +
						// ","+FixedPointLong.flongToString(east)+","+FixedPointLong.flongToString(south)+","+FixedPointLong.flongToString(west));
					}
				} //inner while
				
				for(int j = 0; j < toRemove.size(); j++) {
				    loci.remove(toRemove.elementAt(j));
				}
			}//synchronized
			
			if (first) {
				return null;
			}
			if (north >= south && east >= west) {
				// the test needs to handle dateline issues
				// got intersection
				break;
			}

			// NO INTERSECTION

			// First, remove oldest locus if it is older than some
			// threshold.
			// Threshold should be high enough that we do not experience
			// thrashing due to not seeing a visible beacon
			long MAXLIFE_CLASH = 60000; // 1 minute
			//System.out.println("Oldest is " + (now - oldestTime));
			if (now - oldestTime > MAXLIFE_CLASH) {
				//System.out.println("Removed locus due to age: " +
				// oldestKey);
				synchronized (lociSyncObject()) {
					Object x = loci.remove(oldestKey);
					continue;
				}
			}

			// If no locus is that old, then it's likely that we've got bad
			// stumbling data.
			// try to compensate for this error and return
			try {
				FixedTwoDCoordinate northeast = new FixedTwoDCoordinate(north,
						east);
				FixedTwoDCoordinate southwest = new FixedTwoDCoordinate(south,
						west);
				minRadiusFlong += northeast.distanceFromFlong(southwest);
			} catch (FixedPointLongException fple) {
				throw new ArithmeticException(
						"IntersectionTracker.getEstimateImpl: FixedPointLongException: "
								+ fple);
			}
			break;
		} //while
        
        if(System.getProperty("microedition.configuration") == null) {
	        try {
	            Coordinate estCentre = Types.newCoordinate(FixedPointLong.flongToString((north+south)/2),FixedPointLong.flongToString((east+west)/2));
	            Estimate resultEstimate = Types.newEstimate(now, estCentre, FixedPointLong.flongToString(minRadiusFlong));
		        //System.out.println("Final estimate: " + resultEstimate);
	            return resultEstimate;
	        } catch(FixedPointLongException fple) {
	            return null;
	        }
        } else {
	        // this way won't work on the desktop
	        return new FixedTwoDPositionEstimate(now,new FixedTwoDCoordinate((north+south)/2,(east+west)/2),minRadiusFlong);
        }
    }

    /**
     * @see org.placelab.client.tracker.Tracker#updateWithoutMeasurement(long)
     */
    public synchronized void updateWithoutMeasurement(long timeSinceMeasurementMillis) {
        // not modelling velocity, so no-op here
    }

    /**
     * @see org.placelab.client.tracker.Tracker#resetImpl()
     */
    protected void resetImpl() {
    	synchronized(lociSyncObject()) {
    		loci = new Hashtable();
    	}
    }

    //Avoid concurrent modification exceptions when passing the
    //original iterator out
    public Enumeration getLociBeacons() {
    	synchronized(lociSyncObject()) {
    		if(loci == null) {
    			loci = new Hashtable();
    		}
    		return loci.keys();
    	}
    }
    
    /** Artificially age (that is, pay less attention to) currently cached readings */
    public synchronized void advanceTimeMillis(long timeMillis) {
    	synchronized(lociSyncObject()) {
    		Enumeration i = loci.keys();
    		while(i.hasMoreElements()) {
    			Object key = i.nextElement();
    			Locus l = (Locus) loci.get(key);
    			l.timestamp-=timeMillis;
    		}
    	}
    }
    
    public synchronized int lociSize() {
    	synchronized(lociSyncObject()) {
    		if(loci == null) return -1;
    		return loci.size();
    	}
    }
}