particlefilters.cc.svn-base

Probabilistic graphical models in matlab.

#include <ParticleFilters.h>
#include <Potential.h>
#include <Node.h>
#include <RandomVariable.h>
#include <GraphTreePartition.h>
#include <OutputGraph.h>
#include <Experiment.h>

#include <ctime>
#include <boost/graph/subgraph.hpp>
#include <boost/graph/copy.hpp>

using namespace std;
using namespace boost;

// Explicit Template Instantiations //

typedef property< edge_index_t, unsigned int, ContinuousPotential *> NPEdgeProperty;
typedef adjacency_list<vecS, vecS, undirectedS, GraphicalModelNode *, NPEdgeProperty > NPGraph;

typedef property< edge_index_t, unsigned int, DiscretePotential *> DiscreteEdgeProperty;
typedef adjacency_list<vecS, vecS, undirectedS, MessageNode<DiscreteRandomVariable, boost::adjacency_list_traits<vecS, vecS, undirectedS>::vertex_descriptor> *, DiscreteEdgeProperty > DiscreteGraph;

template void metropolis < NPGraph> (NPGraph &, const unsigned int, const unsigned int, bool);

template void non_parametric_tree_gibbs_sampler < NPGraph> (NPGraph &, const unsigned int, const unsigned int, const unsigned int, bool);

// End of Explicit Template instantiations //

//extern Experiment < DiscreteGraph> * current_discrete_experiment;

//******************* ParticleFilterProposal Implementation ****************//

ParticleFilterProposal::ParticleFilterProposal(): number_potentials(0), gaussian_proposal (fibonnacci_number_generator , normal_distribution <double> ())
{
}

double ParticleFilterProposal::get_proposal_weight(const double position) const
{
	//return (1/ sqrt( 2 * M_PI * sqrt(gaussian_proposal.distribution().sigma()) ) ) * exp( - ((position - gaussian_proposal.distribution().mean())*(position - gaussian_proposal.distribution().mean())) / (2*(gaussian_proposal.distribution().sigma())*(gaussian_proposal.distribution().sigma())  ) ); // 1/sqrt(2*pi*sigma) * exp(- (x-mean)2 / (2*sigma2) )
	
	// We can probably use the following formula ( proportional constant taken out)
	
	// cout << "dist mean " <<gaussian_proposal.distribution().mean();
	// cout <<  " position " << position;
	// cout << 
	
	return exp( - ( (position - gaussian_proposal.distribution().mean()) * (position - gaussian_proposal.distribution().mean() ) ) / (2*(gaussian_proposal.distribution().sigma())*(gaussian_proposal.distribution().sigma())  ) );
}


double ParticleFilterProposal::get_proposal_position(ContinuousRandomVariable & rv)
{
	//cout << "Gaussian sigma: " << gaussian_proposal.distribution().sigma() << endl;
	
	if (! rv.is_conditionned() )
	{
		rv.last_sampled_value = gaussian_proposal();
	}
	
	return rv.last_sampled_value;
}

void ParticleFilterProposal::setup_gaussian_proposal( ChainedSinglePotential * const ptr, const unsigned int variable_index)
{
	list < GaussianPotential * > new_lst;
	
	for (list <Potential * >::const_iterator it = ptr->potential_lst.begin(); it != ptr->potential_lst.end(); ++it)
	{
		GaussianPotential *  a = dynamic_cast<GaussianPotential*> (*it);
		
		if (a )
		{
			//cout << "Successfully downcasted" << endl;
			new_lst.push_back(a);
		}
		/*
		 else 
		 {
			 cout << " Not downcasted" << endl;	
		 }
		 */
	}
	
	setup_gaussian_proposal(new_lst, variable_index);
}


void ParticleFilterProposal::setup_gaussian_proposal(const list < GaussianPotential * >  & potential_lst, const unsigned int variable_index)
{
	// Assuming all of our Potentials are Gaussian
	
	// Must compute means
	
	number_potentials = potential_lst.size();
	means_sum = 0.0;
	
	if (number_potentials == 0)
	{
		return;
	}
	
	list <GaussianPotential * >::const_iterator it = potential_lst.begin();	
	constant_variance = (*it)->get_variance();
	
	for (list <GaussianPotential * >::const_iterator it = potential_lst.begin(); it != potential_lst.end(); ++it) 
	{
		means_sum += (*it)->get_variable_mean(variable_index);
		assert ( (*it)->get_variance() == constant_variance );
	}
	
	/*
	 if (variable_index == 2)
	 {
		 cout << " Number_potentials, means_sum, constant_variance/number_potentials: " << number_potentials << ", " << means_sum / number_potentials << ", " << constant_variance/number_potentials << endl;
	 }
	 */
	
	// Here we put directly the new proposal into effect
	
	gaussian_proposal.distribution() = normal_distribution < double > ( means_sum / number_potentials, sqrt (constant_variance/number_potentials) );
	
	//cout << "Gaussian mean: " << gaussian_proposal.distribution().mean() << endl;
}

void ParticleFilterProposal::add_gaussian_proposal(const double new_mean, const double new_variance)
{
	
	if (number_potentials != 0)
	{
		assert ( new_variance == constant_variance );
	}
	
	//cout << "New variance" << new_variance << endl;
	
	// Here we put directly the new proposal into effect
	
	// cout << "Proposal will be (number_potentials, new_mean, means_sum, constant_variance, means_sum/number_potentials): " <<  number_potentials+1 << ", " << new_mean << ", " << means_sum << ", " <<  constant_variance << ", " << (means_sum+new_mean) / (number_potentials+1) << endl;
	
	gaussian_proposal.distribution() = normal_distribution < double > ( (means_sum+new_mean) / (number_potentials+1), sqrt (new_variance/(number_potentials+1) ) );
}

void ParticleFilterProposal::set_gaussian_variance(const double new_variance)
{
	gaussian_proposal.distribution() = normal_distribution < double > ( gaussian_proposal.distribution().mean(), sqrt (new_variance) );
}

// Deprecated methods

/*
 void ParticleFilterProposal::setup_gaussian (const double a, const double b) // a is mean, b is variance
 {
	 //gaussian_proposal = variate_generator < lagged_fibonacci607 &, normal_distribution <double> >(fibonnacci_number_generator , normal_distribution < > ( a, b ));
	 
	 //normal_distribution <double> under_dist = gaussian_proposal.distribution() = normal_distribution < double > ( a, b );
	 
	 gaussian_proposal.distribution() = normal_distribution < double > ( a, sqrt (b) );
	 //mean = a;
	 //variance = b;
 }
 
 */

/*
 double ParticleFilterProposal::get_proposal_position()
 {	
	 cout << "Gaussian sigma: " << gaussian_proposal.distribution().sigma() << endl;
	 
	 return gaussian_proposal();
 }
 */

//******************* End of ParticleFilterProposal Implementation ****************//


//*************** DiscreteParticleFilterProposal Implementation ******************//

DiscreteParticleFilterProposal::DiscreteParticleFilterProposal()
{	
}

void DiscreteParticleFilterProposal::setup_discrete_proposal( const ChainedSingleDiscretePotential & csdp, DiscreteRandomVariable  & drv)
{
	general_sampling_probabilities.clear();
	general_sampling_probabilities.resize( drv.get_number_values() , 1.0);
	current_sampling_probabilities.clear();
	current_sampling_probabilities.resize( drv.get_number_values() , 1.0);
	
	//unsigned int variable_index = drv.get_index();
	
	for (unsigned int sp = 0 ; sp < general_sampling_probabilities.size(); ++sp)
	{
		for (list <DiscretePotential * >::const_iterator it = csdp.potential_lst.begin(); it != csdp.potential_lst.end(); ++it)
		{
			drv.set_value(sp);
			
			(*it)->set_variable_value (drv);
			
			general_sampling_probabilities[sp] = general_sampling_probabilities[sp] * (*it)->get_potential_value ();
		}
	}	
}

void DiscreteParticleFilterProposal::add_discrete_proposal(DiscretePotential & dp, DiscreteRandomVariable  & drv)
{
	//unsigned int variable_index = drv.get_index();
	
	for (unsigned int sp = 0 ; sp < current_sampling_probabilities.size(); ++sp)
	{
		drv.set_value(sp);
		dp.set_variable_value (drv);
		
		current_sampling_probabilities[sp] = general_sampling_probabilities[sp] * dp.get_potential_value ( );
	}	
}

unsigned int DiscreteParticleFilterProposal::get_proposal_position(DiscreteRandomVariable & rv)
{	
	if (!fast_double_normalize_over (current_sampling_probabilities.begin(), current_sampling_probabilities.end()))
	{
		cout << "Warning: all probabilities are 0" << endl;
	}
	
	return rv.sample_without_recording(current_sampling_probabilities);
}

inline double DiscreteParticleFilterProposal::get_proposal_weight(const unsigned int a) const
{
	return current_sampling_probabilities[a];	
}

//*************** End of DiscreteParticleFilterProposal Implementation ******************//

template <>
void initialize_particles <subgraph<DiscreteGraph>, unsigned int, DiscreteParticleFilterProposal> (subgraph<DiscreteGraph> & g, vector <unsigned int> & initial_positions, vector <double> & initial_weights, const graph_traits<DiscreteGraph>::vertex_descriptor root_node, DiscreteParticleFilterProposal & proposal)
{
	typedef graph_traits<DiscreteGraph>::vertex_descriptor VertexDescriptor;
	
	graph_traits<DiscreteGraph>::adjacency_iterator u, u_end;
	
	unsigned int number_particles = initial_weights.size();
	
	vector <double> cumulative_distribution (number_particles);
	vector <double> stratified_weights (number_particles);
	vector <unsigned int> original_particles (number_particles);
	
	Potential * internal_potential;
	DiscretePotential * observation_potential = NULL;
	
	internal_potential = static_cast < DiscretePotential *> ( g[root_node]->get_potential() );
	
	for (tie (u, u_end) = adjacent_vertices(root_node, g); u != u_end; ++u)
	{
		if (out_degree(*u,g) == 1)
		{
			observation_potential =  g[edge(root_node, *u, g).first];
		}
	}
	
	DiscreteRandomVariable * current_random_variable = g[root_node]->get_random_variable();
	
	// The proposal will be based on the values of the others nodes...
	
	proposal.setup_discrete_proposal(static_cast<ChainedSingleDiscretePotential &>( *internal_potential), *current_random_variable);
	
	vector <double>::iterator current_particle_weight = initial_weights.begin();
	
	for (vector <unsigned int>::iterator current_particle_position = initial_positions.begin(); current_particle_position != initial_positions.end(); ++current_particle_position)
	{
		*current_particle_position = proposal.get_proposal_position(*current_random_variable); // this also sets the current_random_variable.last_sampled_value
		
		// Compute the new weight
		
		observation_potential->set_variable_value (*current_random_variable);
		
		internal_potential->set_variable_value (*current_random_variable); // don't forget to set the internal potential also !!
		
		*current_particle_weight = ( observation_potential->get_potential_value() * internal_potential->get_potential_value() )/ proposal.get_proposal_weight(*current_particle_position);
		
		// cout << "Particle, weight of obs, internal, prop: "  << *current_particle_position << ", " << observation_potential->get_potential_value() << ", " <<internal_potential->get_potential_value() << ", " << proposal->get_proposal_weight(*current_particle_position) << endl;
		
		++current_particle_weight;
	}
	
	fast_double_normalize_over (initial_weights.begin(), initial_weights.end());
	
	resample (initial_positions, initial_weights, cumulative_distribution, stratified_weights, original_particles);
	
	//cout << "SIZE: " << initial_positions.size() << endl;
	
	//current_random_variable->set_mean_particles(initial_positions);
	
	//cout << "SIZE: " << initial_positions.size() << endl;
}

typedef iterator_property_map < vector< vector < unsigned int > >::iterator, property_map < subgraph<DiscreteGraph>, vertex_index_t>::type > DiscretePositionMap;
typedef iterator_property_map < vector< vector < double > >::iterator, property_map < subgraph<DiscreteGraph>, vertex_index_t>::type > DiscreteWeightMap;

template <>
graph_traits< subgraph<DiscreteGraph> >::vertex_descriptor forward_filtering_implementation < subgraph<DiscreteGraph>, DiscretePositionMap, DiscreteWeightMap > (subgraph<DiscreteGraph> & g, const graph_traits< subgraph<DiscreteGraph> >::vertex_descriptor root_node, DiscretePositionMap & positions, DiscreteWeightMap & weights)
{
	// Variable declarations
	
	typedef graph_traits<DiscreteGraph>::vertex_descriptor VertexDescriptor;
	
	DiscreteRandomVariable * current_random_variable, * previous_random_variable;
	
	unsigned int current_variable_index;
	
	VertexDescriptor double_previous_node, previous_node;
	
	VertexDescriptor current_node = VertexDescriptor();
	
	previous_node = root_node;
	
	double_previous_node = root_node;
	
	graph_traits<DiscreteGraph>::adjacency_iterator u, u_end;
	
	DiscreteParticleFilterProposal proposal;
	
	unsigned int number_particles = get (positions, root_node).size();
	
	initialize_particles (g, get (positions, root_node), get (weights, root_node), root_node, proposal);
	
	Potential * internal_potential = NULL;
	DiscretePotential * observation_potential = NULL;
	DiscretePotential * interaction_potential = NULL;
	
	vector <double> cumulative_distribution (number_particles);
	vector <double> stratified_weights (number_particles);
	vector <unsigned int> original_particles (number_particles);
	
	// We got our initial distribution covered, resample it ?? Currently, NO resampling is performed.
	
	// Initialization done.
	// Beginning of main loop.
	
	while (true)
	{
		
		// * We must obtain:
		
		// * potential coming from the edges linking the node to the other nodes not in the chain, call that internal_potential (because we arranged it as an internal potential)
		// * potential between node and observation, call that observation_potential;
		// * potential between the two nodes, call that interaction potential;
		
		for (tie (u, u_end) = adjacent_vertices(previous_node, g); u != u_end; ++u)
		{
			if (out_degree(*u,g) != 1)
			{