Skip to content

Instantly share code, notes, and snippets.

@Pascal66

Pascal66/gist:6634061

Created September 20, 2013 06:39
Show Gist options
  • Save Pascal66/6634061 to your computer and use it in GitHub Desktop.
Save Pascal66/6634061 to your computer and use it in GitHub Desktop.
Download ZIP
Condensation algorithm (Conditional Density Propagation)
Raw
gistfile1.java
import android.util.Log;
public class CondensationAlgo {
// http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/ISARD1/condensation.html
// "model_parameters.h"
/** The following are all the constants controlling the behaviour of the system and output. */
/** How many samples in the distribution? */
int NSamples = 1000;
/** How many iterations to run the filter? */
int NIterations = 100;
/** The simulated object follows a model of the same form as the process */
double SimulatedMean = -0.1;
double SimulatedScaling = 0.4;
double SimulatedSigma = 0.075;
double SimulatedMeasSigma = 0.03;
/** The prior distribution over samples at the first timestep is a Gaussian with the following parameters. */
double PriorMean = 0.0;
double PriorSigma = 0.2;
/** The process model is a first-order Auto-Regressive Process of the following form:
* x_{t+1} - ProcessMean =
* (x_t - ProcessMean) * ProcessScaling + ProcessSigma * w_t
* where w_t is zero-mean unit iid Gaussian noise. */
double ProcessMean = -0.1;
double ProcessScaling = 0.4;
double ProcessSigma = 0.075;
/** The observation density is a mixture of Gaussians, where each observed object has a different sigma as follows. */
double ObsSigma = 0.03;
/** How many columns wide is the ASCII histogram? */
int HistBins = 79;
/** How many lines of text does the ASCII histogram take? */
int HistLines = 25;
/** What is the highest value the density histogram can represent before saturating above HistLines? */
double MaxHistHeight = 0.2;
/** What is the distance from the origin to the edge of the histogram? */
double DisplayWidth = 0.35;
// "data_types.h"
/* The following are model-specific definitions */
/** The state vector for this simple model is one-dimensional. In multiple dimensions, for example, Sample would be an array.
*/
double[] Sample;
/** The process model used in this simple implementation is a one-dimensional first-order auto-regressive process, where
*
* (x_t - mean) = (x_{t-1} - mean) * scaling + sigma w_t
*
* and the w_t are iid unit zero-mean Gaussian noise samples.
*/
class ProcessModel {
double mean;
double scaling;
double sigma;
}
/**
* This structure contains the parameters of the simulation model. The
* simulated data is produced from a model of a single particle
* following a one-dimensional 1st-order ARP whose parameters are
* stored in the structure process. Measurements are simulated by
* adding Gaussian noise with std. deviation sigma to the true
* simulated position of the particle.
*/
class SceneModel {
ProcessModel process = new ProcessModel();
double sigma;
}
/**
* This structure contains the data for the measurements made at each
* timestep. The simulation consists of a single point-measurement.
* This structure stores the true position of the simulated particle,
* and its measured position, which is corrupted by noise.
*/
class MeasData {
double truePosition, observed;
}
/**
* The prior distribution of the state is taken to be Gaussian with
* the parameters stored in this structure.
*/
class PriorModel {
double mean, sigma;
}
/**
* The observation model is of Gaussian noise with std. deviation
* sigma, so
*
* z_t = x_t + sigma v_t
*
* where the v_t are assumed iid unit zero-mean Gaussian noise
* samples. In principle for this simulation, the modelled observation
* noise can be different to the simulated observation noise, hence
* the presence of a separate std. deviation parameter in the
* structure SceneModel.
*/
class ObservationModel {
double sigma;
}
/**
* This structure contains information about how the state
* distribution should be displayed at each timestep. The display in
* this implementation is a one-dimensional histogram, and this
* parameter sets the width of the histogram (so that the interval
* [-histogram_width, histogram_width] is displayed).
*/
class DisplayModel {
double histogram_width;
}
/* End of model-specific structures */
/* The following should work with any model-specific definitions */
/**
* This structure contains all the parameter settings for the models
* which remain constant throughout a run of the algorithm.
*/
class GlobalData {
/** The parameters specifying the model of the prior distribution for the first timestep. */
PriorModel prior = new PriorModel();
/** The parameters specifying the process model. */
ProcessModel process = new ProcessModel();
/** The parameters specifying the simulation model of the scene. This is only used in the case of simulated data. */
SceneModel scene = new SceneModel();
/** The parameters specifying the observation model. */
ObservationModel obs = new ObservationModel();
/** The parameters specifying how to display the state estimate at each timestep. */
DisplayModel disp = new DisplayModel();
/** The number of samples N. */
int nsamples;
/** The number of iterations to run the filter. */
int niterations;
}
/**
* This structure contains all of the information which is specific to
* a given iteration of the algorithm.
*/
class IterationData {
double[] new_positions;
/**
* The following arrays contain the sample positions for the current
* and previous timesteps respectively. At the end of each
* iteration, these pointers are swapped over to avoid copying data
* structures, so their addresses should not be relied on.
*/
double[] old_positions;
/**
* The following arrays give the sample weights and cumulative
* probabilities, as well as the largest cumulative
* probability. There is no stage in the algorithm when the weights
* from both the previous and current timesteps are needed. At the
* beginning of an iteration, sample_weights contains the weights
* from the previous iteration, and by the end it contains the
* weights of the current iteration. The cumulative probabilities
* are not normalised, so largest_cumulative_prob is needed to store
* the largest cumulative probability (for simplicity of the binary
* search algorithm, cumul_prob_array[0] = 0)
*/
double[] sample_weights, cumul_prob_array;
double largest_cumulative_prob;
/**
* The measurements made in a given iteration are stored here. For
* some applications a discrete set of measurements is not
* appropriate, and this could contain, e.g. a pointer to an image
* structure.
*/
MeasData meas = new MeasData();
}
/* End of generic structures */
/* End of global variables */
// #include "condensation.h"
/* All of the global information is packaged into the following two
* structures. `global' contains information which is constant over a
* run of the algorithm and `data' contains all of the current state
* at a given iteration. */
GlobalData global = new GlobalData();
IterationData data = new IterationData();
/* End of global variables */
/* From here on is generic Condensation regardless of the form of
* model or observation used. All of the model-specific routines are
* found in model_specific.c */
/**
* This is binary search using cumulative probabilities to pick a base
* sample. The use of this routine makes Condensation O(NlogN) where N
* is the number of samples. It is probably better to pick base
* samples deterministically, since then the algorithm is O(N) and
* probably marginally more efficient, but this routine is kept here
* for conceptual simplicity and because it maps better to the
* published literature.
*/
int pick_base_sample() {
double choice = uniform_random() * data.largest_cumulative_prob;
int low, middle, high;
low = 0;
high = global.nsamples;
while (high > (low + 1)) {
middle = (high + low) / 2;
if (choice > data.cumul_prob_array[middle])
low = middle;
else
high = middle;
}
return low;
}
/**
* This routine computes all of the new (unweighted) sample
* positions. For each sample, first a base is chosen, then the new
* sample position is computed by sampling from the prediction density
* p(x_t|x_t-1 = base). predict_sample_position is obviously
* model-dependent and is found in model_specific.c, but it can be
* replaced by any process model required.
*/
void predict_new_bases() {
int n, base;
for (n = 0; n < global.nsamples; ++n) {
base = pick_base_sample();
predict_sample_position(n, base);
}
}
/**
* Once all the unweighted sample positions have been computed using
* predict_new_bases, this routine computes the weights by evaluating
* the observation density at each of the positions. Cumulative
* probabilities are also computed at the same time, to permit an
* efficient implementation of pick_base_sample using binary
* search. evaluate_observation_density is obviously model-dependent
* and is found in model_specific.c, but it can be replaced by any
* observation model required.
*/
void calculate_base_weights() {
int n;
double cumul_total;
cumul_total = 0.0;
for (n = 0; n < global.nsamples; ++n) {
data.sample_weights[n] = evaluate_observation_density(n);
data.cumul_prob_array[n] = cumul_total;
cumul_total += data.sample_weights[n];
}
data.largest_cumulative_prob = cumul_total;
}
/**
* Go and output the estimate for this iteration (which is a
* model-dependent routine found in model_specific.c) and then swap
* over the arrays ready for the next iteration.
*/
void update_after_iterating(int iteration) {
double[] /* Sample */temp;
display_data(iteration);
temp = data.new_positions;
data.new_positions = data.old_positions;
data.old_positions = temp;
}
/**
* / obtain_observations is model-dependent and can be found in model_specific.c
*/
void run_filter() {
int i;
for (i = 0; i < global.niterations; ++i) {
obtain_observations(); /* Go make necessary measurements */
predict_new_bases(); /* Push previous state through process model */
calculate_base_weights(); /* Apply Bayesian measurement weighting */
update_after_iterating(i); /* Tidy up, display output, etc. */
}
}
/**
* This routine fills in the data structures with default constant
* values. It could be enhanced by reading informatino from the
* command line to allow e.g. N to be altered without recompiling.
*/
void initialise_defaults() {
global.nsamples = NSamples;
global.niterations = NIterations;
initialise_model_specific_defaults();
}
/**
* Create all the arrays, then fill in the prior distribution for the
* first iteration. The prior is model-dependent, so
* set_up_prior_conditions can be found in model_specific.c
*/
boolean initialise() {
initialise_defaults();
data.new_positions = new double[(/* sizeof(Sample) * */global.nsamples)];
data.old_positions = new double[(/* sizeof(Sample) * */global.nsamples)];
data.sample_weights = new double[(/* sizeof(double) * */global.nsamples)];
data.cumul_prob_array = new double[(/* sizeof(double) * */global.nsamples)];
// if (!data.new_positions || !data.old_positions || !data.sample_weights || !data.cumul_prob_array) {
// fprintf(stderr, "Failed to allocate memory for sample arrays\n");
// return false;
// }
set_up_prior_conditions();
return true;
}
/* Tidy up */
void shut_down() {
// free(data.new_positions);
// free(data.old_positions);
// free(data.sample_weights);
// free(data.cumul_prob_array);
}
boolean main() {
if (!initialise())
return true;
run_filter();
shut_down();
return false;
}
// #include "data_types.h"
// #include "model_parameters.h"
// #include "condensation.h"
/* The following routines are model-specific and should be replaced to
* implement an arbitrary process and observation model. */
void initialise_model_specific_defaults() {
/* Set up the parameters of the simulation model, process and
* observation. */
global.scene.process.mean = SimulatedMean;
global.scene.process.scaling = SimulatedScaling;
global.scene.process.sigma = SimulatedSigma;
global.scene.sigma = SimulatedMeasSigma;
/* Set up the parameters of the prior distribution */
global.prior.mean = PriorMean;
global.prior.sigma = PriorSigma;
/* Set up the parameters of the process model */
global.process.mean = ProcessMean;
global.process.scaling = ProcessScaling;
global.process.sigma = ProcessSigma;
/* Set up the parameters of the observation model */
global.obs.sigma = ObsSigma;
/* Set up the parameters of the display model */
global.disp.histogram_width = DisplayWidth;
}
/**
* Set up the initial sample-set according to the prior model. The
* prior is a simple Gaussian, so each of the positions is filled in
* by sampling that Gaussian and the weights are initialised to be
* uniform. gaussian_random can be found in utility.c
*/
void set_up_prior_conditions() {
int n;
for (n = 0; n < global.nsamples; ++n) {
data.old_positions[n] = global.prior.mean + global.prior.sigma
* gaussian_random();
/* The probabilities are not normalised. */
data.cumul_prob_array[n] = (double) n;
data.sample_weights[n] = 1.0;
}
/**
* The probabilities are not normalised, so store the largest value
* here (for simplicity of the binary search algorithm,
* cumul_prob_array[0] = 0). This can then be used as a
* multiplicative normalisation constant for the sample_weights
* array, as well.
*/
data.largest_cumulative_prob = (double) n;
/* This is the initial positions the simulated object. */
data.meas.truePosition = 0.0;
}
/**
* The process model for a first-order auto-regressive process is:
*
* x_{t+1} - mean = (x_t - mean)*scaling + sigma*w_t
*
* where w_t is unit iid Gaussian noise. gaussian_random can be found
* in utility.c
*/
double iterate_first_order_arp(double truePosition, ProcessModel process) {
return process.mean + ((truePosition - process.mean) * process.scaling)
+ process.sigma * gaussian_random();
}
/**
* In a real implementation, this routine would go and actually make
* measurements and store them in the data.meas structure. This
* simulation consists of an `object' moving around obeying a
* first-order auto-regressive process, and being observed with its
* true positions corrupted by Gaussian measurement noise.
* Accordingly, this routine calculates the new simulated true and
* measured position of the object.
*/
void obtain_observations() {
data.meas.truePosition = iterate_first_order_arp(
data.meas.truePosition, global.scene.process);
data.meas.observed = data.meas.truePosition + global.scene.sigma
* gaussian_random();
}
/**
* This routine samples from the distribution
*
* p(x_t | x_{t-1} = old_positions[old_sample])
*
* and stores the result in new_positions[new_sample]. This is
* straightforward for the simple first-order auto-regressive process
* model used here, but any model could be substituted.
*/
void predict_sample_position(int new_sample, int old_sample) {
data.new_positions[new_sample] = iterate_first_order_arp(
data.old_positions[old_sample], global.process);
}
/**
* This routine evaluates the observation density
*
* p(z_t|x_t = new_positions[new_sample])
*
* The observation model in this implementation is a simple mixture of
* Gaussians, where each simulated object is observed as a 1d position
* and measurement noise is represented as Gaussian. For a
* visual-tracking application, this routine would go and evaluate the
* likelihood that the object is present in the image at the position
* encoded by new_positions[new_sample]. evaluate_gaussian can be
* found in utility.c
*/
double evaluate_observation_density(int new_sample) {
return evaluate_gaussian(data.new_positions[new_sample]
- data.meas.observed, global.obs.sigma);
}
// /**
// * The following two routines provide rudimentary graphical output in
// * the form of an ASCII histogram of the 1d state density.
// */
// int eval_bin(float where) {
// return (HistBins - 1)
// / 2
// + (int) (where * (HistBins - 1) / 2 / global.disp.histogram_width);
// }
// void display_histogram(double estimated_position)
// {
// double bins[HistBins];
//
// int b, n, line, which_bin, meas_bin, est_bin, true_bin;
// double lineheight;
// char outc;
//
// for (b=0; b<HistBins; ++b)
// bins[b] = 0.0;
//
// for (n=0; n<global.nsamples; ++n) {
// which_bin = eval_bin(data.new_positions[n]);
//
// if (which_bin >=0 && which_bin < HistBins)
// bins[which_bin] +=
// data.sample_weights[n] / data.largest_cumulative_prob;
// }
//
// for (b=0; b<HistBins; ++b)
// bins[b] = (bins[b] * (double) HistLines) / MaxHistHeight;
//
// for (line=0; line<HistLines; ++line) {
// lineheight = (double) (HistLines-1-line);
// for (b=0; b<HistBins; ++b) {
// if (line==0 && bins[b] >= lineheight+1.0)
// outc = '*';
// else if (bins[b] >= lineheight+0.5 && bins[b] < lineheight+1.0)
// outc = '-';
// else if (bins[b] >= lineheight && bins[b] < lineheight+0.5)
// outc = '_';
// else
// outc = ' ';
// printf("%c", outc);
// }
// printf("\n");
// }
//
// true_bin = eval_bin(data.meas.true);
// meas_bin = eval_bin(data.meas.observed);
// est_bin = eval_bin(estimated_position);
//
// for (b=0; b<HistBins; ++b) {
// if ((b == meas_bin && b == est_bin) ||
// (b == meas_bin && b == true_bin) ||
// (b == true_bin && b == est_bin))
// outc = '*';
// else if (b == true_bin)
// outc = '.';
// else if (b == meas_bin)
// outc = '+';
// else if (b == est_bin)
// outc = 'x';
// else
// outc = ' ';
// printf("%c", outc);
// }
// printf("\n");
// }
/**
* This routine computes the estimated position of the object by
* estimating the mean of the state-distribution as a weighted mean of
* the sample positions, then displays a histogram of the state
* distribution along with a character denoting the estimated position
* of the object.
*/
void display_data(int iteration) {
int n;
double aggregate;
aggregate = 0.0;
/* Compute the unnormalised weighted mean of the sample
* positions. */
for (n = 0; n < global.nsamples; ++n)
aggregate += data.new_positions[n] * data.sample_weights[n];
aggregate /= data.largest_cumulative_prob;
// display_histogram(aggregate);
Log.i("Condensation", iteration + ": Measured pos. "
+ data.meas.observed + " TruePos " + data.meas.truePosition
+ " Est. position " + aggregate);
// #ifdef ANSI_TERM_SEQUENCES
// /* If possible, run the cursor back up the screen so the histogram
// stays in the same place instead of scrolling down the display. */
// printf("\033[%dA", HistLines+2);
// sleep(1);
// #endif
}
/* End of model-specific routines */
/* The following are some utility routines. */
/**
* This is the worst random number routine in the known universe, but
* I use it for portability. Feel free to replace it.
*/
double uniform_random() {
return Math.random()/* / RAND_MAX */;
}
/**
* This Gaussian routine is stolen from Numerical Recipes and is their
* copyright.
*/
double gaussian_random() {
int next_gaussian = 0;
double saved_gaussian_value = 0;
double fac, rsq, v1, v2;
if (next_gaussian == 0) {
do {
v1 = 2.0 * uniform_random() - 1.0;
v2 = 2.0 * uniform_random() - 1.0;
rsq = v1 * v1 + v2 * v2;
} while (rsq >= 1.0 || rsq == 0.0);
fac = Math.sqrt(-2.0 * Math.log(rsq) / rsq);
saved_gaussian_value = v1 * fac;
next_gaussian = 1;
return v2 * fac;
} else {
next_gaussian = 0;
return saved_gaussian_value;
}
}
double evaluate_gaussian(double val, double sigma) {
/** This private definition is used for portability */
double Condense_PI = 3.14159265358979323846;
return 1.0 / (Math.sqrt(2.0 * Condense_PI) * sigma)
* Math.exp(-0.5 * (val * val / (sigma * sigma)));
}
/* End of utility routines */
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment