condensation.cpp

来自美国著名大学的研究,实现动目标的自动跟踪算法

*/
/*********************************************************************************
  Function    : calculate_base_weights
  Description : 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.
  Parameter   : 
  Return      : 
  Call	      :
*********************************************************************************/

void
IterationData::calculate_base_weights(double sigma)
{
	double cumul_total;
	
	cumul_total = 0.0;
	for (int n = 0; n < m_nSamples; n++)
	{
		sample_weights[n] = evaluate_observation_density(n, sigma);
		cumul_prob_array[n] = cumul_total;
		cumul_total += sample_weights[n];
	}
	largest_cumulative_prob = cumul_total;
}


/*********************************************************************************
  Function    : update_after_iterating
  Description : 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.
  Parameter   : iteration (Input) -
  Return      : 
  Call	      :
*********************************************************************************/

void
IterationData::update_after_iterating(int iteration)
{
	Sample *temp;

//	display_data(iteration);

	temp = new_positions;
	new_positions = old_positions;
	old_positions = temp;
}


/*********************************************************************************
  Function    : initialise_defaults
  Description : 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.
  Parameter   : 
  Return      : 
  Call	      :
*********************************************************************************/

GlobalData::GlobalData( )
{
	m_nSamples = NSamples;
	m_nIterations = NIterations;
}

/*********************************************************************************
  Function    : Init
  Description : Create all the arrays, then fill in the prior distribution for the
		first iteration. The prior is model-dependent
  Parameter   : 
  Return      : 
  Call	      :
*********************************************************************************/

IterationData::IterationData()
{
	m_nSamples = NSamples;

	new_positions = new Sample[m_nSamples];
	old_positions = new Sample[m_nSamples];

	sample_weights   = new double[m_nSamples];
	cumul_prob_array = new double[m_nSamples];
	
	if (!new_positions  || !old_positions || !sample_weights || !cumul_prob_array)
	{
		//TRACE("Failed to allocate memory for sample arrays.\n");
		return;
	}

	//set_up_prior_conditions();
	
	return;
}


/*********************************************************************************
  Function    : pick_base_sample
  Description : 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.
  Parameter   : 
  Return      : 
  Call	      :
*********************************************************************************/

int
IterationData::pick_base_sample( )
{
	double choice = uniform_random() * largest_cumulative_prob;
	int low, middle, high;

	low  = 0;
	high = m_nSamples;
	
	while (high > (low + 1) )
	{
		middle = (high + low) / 2;
		if (choice > cumul_prob_array[middle])
		{
			low = middle;
		}
		else
		{
			high = middle;
		}
	}
	
	return low;
}

/*********************************************************************************
  Function    : predict_sample_position
  Description : 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.
  Parameter   : new_sample (Input) -
		old_sample (Input) -
  Return      : 
  Call	      :
*********************************************************************************/

void
IterationData::predict_sample_position(int new_sample, int old_sample, GlobalData *globa)
{
	new_positions[new_sample] = globa->process.first_order_arp(old_positions[old_sample]);
}

/*********************************************************************************
  Function    : predict_new_bases
  Description : 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). 
  Parameter   : 
  Return      : 
  Call	      :
*********************************************************************************/

void
IterationData::predict_new_bases(GlobalData *globa)
{
	int base;
	
	for (int n = 0; n < m_nSamples; n++)
	{
		base = pick_base_sample();
		predict_sample_position(n, base, globa);
	}
}


/*********************************************************************************
  Function    : set_up_prior_conditions
  Description : 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.
  Parameter   : 
  Return      : 
  Call	      :
*********************************************************************************/

void
IterationData::set_up_prior_conditions(double mean, double sigma)
{
	int n;

	for ( n=0; n < m_nSamples; ++n)
	{
		old_positions[n] = mean + sigma * gaussian_random();
		/* The probabilities are not normalised. */
		cumul_prob_array[n] = (double) n;
		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. */
	largest_cumulative_prob = (double) n;

	/* This is the initial positions the simulated object. */
	meas.SetTrueValue(0.0);
}

/*********************************************************************************
  Function    : shut_down
  Description : Tidy up
  Parameter   : 
  Return      : 
  Call	      :
*********************************************************************************/
IterationData::~IterationData( )
{
	delete []new_positions;
	delete []old_positions;
	delete []sample_weights;
	delete []cumul_prob_array;
}

Condensation::Condensation()
{

}

Condensation::~Condensation()
{

}


/*********************************************************************************
  Function    : Run
  Description : run filter
  Parameter   : 
  Return      : 
  Call	      :
*********************************************************************************/
void
Condensation::Run(int times)
{
	data.set_up_prior_conditions(global.prior.mean(), global.prior.sigma() );

	for (int i = 0; i < times; i++)
	{
		data.meas.obtain_observations(&global);	/* Go make necessary measurements */
		data.predict_new_bases(&global);	/* Push previous state through process model */
		data.calculate_base_weights(global.obs.sigma());/* Apply Bayesian measurement weighting */
		data.update_after_iterating(i);		/* Tidy up, display output, etc. */
	}
}


/*********************************************************************************
  Function    : main
  Description : 
  Parameter   : 
  Return      : 
  Call	      :
*********************************************************************************/
int main(int argc, char *argv[])
{
	Condensation cdst;

	cdst.Run(100);

	return 0;
}