cdf.java

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

/*
 * Created on Jan 13, 2004
 *
 * Intel research seattle.
 */
package org.placelab.particlefilter;

import java.util.Vector;

import org.placelab.util.NumUtil;

/**
 * 
 *
 * 
 */
public class CDF {
	private double[] cumulativeWeight;
	private int[] ownerIndex;
	private double scaleFactor;
	public static final double RANDOM=-1.0;

	public void dumpCompressed() {
		int curr=ParticleFilter.INVALID_INDEX;
		int i=0;
		int total=0;
		
		for (i=0; i<cumulativeWeight.length;++i) {
			if (curr!=ownerIndex[i]) {
				++total;
				curr=ownerIndex[i];
			}
		}
		System.out.println("CDF: "+ total+ " owners "+ cumulativeWeight[cumulativeWeight.length-1]);
	}
	
	public CDF(double[] _cumulativeWeight, int[] _ownerIndex, double _scaleFactor) {
		if (_cumulativeWeight.length != _ownerIndex.length) {
			throw new IllegalArgumentException("You gave CDF different sized arrays!");
		}

		cumulativeWeight=_cumulativeWeight;
		ownerIndex=_ownerIndex;
		scaleFactor=_scaleFactor;
	}
	
	public double getCombTineSize(int desiredNumberOfSamples) {
		double numberOfEntries=(double)desiredNumberOfSamples;
		double tineSizeNaive=1.0/numberOfEntries;
		return tineSizeNaive*scaleFactor;
	}
	public int getFirstNonEmptyIndex() {
		//find first non-zero value
		for (int i=0; i<ownerIndex.length;++i) {
			if (ownerIndex[i] != ParticleFilter.INVALID_INDEX) {
				return ownerIndex[i];
			}
		}
		throw new IllegalArgumentException("Our CDF is empty. This "+
			"usually means that the likelihood function produced no likely "+
			"particles at all.");
	}

	
	public Vector getClonesOfHeavilyWeightedEntries(double startingOffset_NEGATIVE_IS_RANDOM, 
			Vector currentParticles, int numClones) {
		
		double tineSize = getCombTineSize(numClones), start;
		if (startingOffset_NEGATIVE_IS_RANDOM>=0.0) {
			start = startingOffset_NEGATIVE_IS_RANDOM;
		} else {
			start = NumUtil.rand.nextDouble()*tineSize;
		}
		int curr=0;
		double target=start;
//		System.out.println("XXX " + target + " " + currentParticles.size());
		Vector newParticles = new Vector();
		for (int count=0; count < numClones; ++count) {
			while ((curr < ownerIndex.length-1) && (cumulativeWeight[curr]<target)) {
				++curr;
			}
			int indexToCopy=ownerIndex[curr];
			if (indexToCopy==ParticleFilter.INVALID_INDEX) {
				indexToCopy=getFirstNonEmptyIndex();
			}
			Particle p = ((Particle)currentParticles.elementAt(indexToCopy)).createClone();
			
			newParticles.addElement(p);
			target+=tineSize;
		}
		
		return newParticles;
	}
	public class TEST_PROBE {
		private static final int INDEX_NOT_FOUND=-1231;
		//THIS IS REALLY SLOW (don't use this in real code)
		public int getIndexOfSampleContributingThisBitOfWeight(double d) {
			double actualDesiredWeight=d*scaleFactor;
			int result=INDEX_NOT_FOUND;
			for (int i=0; i<cumulativeWeight.length;++i) {
				if (cumulativeWeight[i]>=actualDesiredWeight) {
					result=ownerIndex[i];
					break;
				}
			}
			if (result==INDEX_NOT_FOUND) {
				throw new IllegalArgumentException("We can't find anything "+
					"that makes sense for that weight (probably not btw 0.0 and" +
					" 1.0:"+d);
				
			}
			if (result==ParticleFilter.INVALID_INDEX) {
				return CDF.this.getFirstNonEmptyIndex();
			}
			return result;
		}
		public double getCombTineSizeForTesting(int desiredSize) {
			return getCombTineSize(desiredSize);
		}
		public int size() {
			return cumulativeWeight.length;
		}
	}

}