particlefilters.cc.svn-base

Probabilistic graphical models in matlab.

	typename graph_traits<Graph>::adjacency_iterator u, u_end;
	
	vector <double>::iterator current_particle_weight;
	
	ContinuousPotential * internal_potential = NULL;
	ContinuousPotential * observation_potential = NULL;
	
	internal_potential = static_cast < ContinuousPotential * > (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];
		}
	}
	
	ContinuousRandomVariable * current_random_variable = static_cast<ContinuousRandomVariable *> (g[root_node]->get_random_variable());
	unsigned int current_variable_index (current_random_variable->get_index());
	
	// The proposal will be based on the values of the others nodes...
	
	proposal.setup_gaussian_proposal(static_cast<ChainedSinglePotential *>(internal_potential), current_variable_index);
	
	while ( true )
	{
		current_particle_weight = initial_weights.begin();
		
		for (vector <double>::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); // this is the Bernouilli
			
			internal_potential->set_variable_value (*current_random_variable); // don't forget to set the internal potential also !!
			
			//cout << "Current particle, Observation, Internal: " << *current_particle_position << ", " << observation_potential->get_potential_value() << ", " << internal_potential->get_potential_value() << endl;
			
			//cout << "Current particle weight: " << *current_particle_weight << endl;
			
			//cout << "Prop: " << proposal.get_proposal_weight(*current_particle_position) << endl;
			
			*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;
		}
		
		
		if ( accumulate(initial_weights.begin(), initial_weights.end(), 0.0) != 0.0 )
		{
			return;
		}
		
		cout << "The sum of weights equal 0, we increase variance." << endl;
		
		proposal.set_gaussian_variance(10.0*proposal.debug_get_gaussian_variance());
		
	}
	
}

template <class PositionType >
void resample( vector < PositionType > & current_positions, vector < double> & current_weights, vector < double> & cumulative_distribution, vector < double> & stratified_weights, vector < PositionType > & original_particles)
{	
	// Compute cumulative distribution
	
	double sum = 0.0;
	
	vector <double>::iterator current_particle_weight = current_weights.begin();
	
	unsigned int number_particles = current_weights.size();
	
	for ( vector <double>::iterator it = cumulative_distribution.begin(); it != cumulative_distribution.end(); ++it )
	{
		sum += *current_particle_weight;
		*it = sum;
		++current_particle_weight;
	}
	
	sum = uniform_distribution_0_1()/number_particles;
	
	current_particle_weight = current_weights.begin();
	
	for ( vector <double>::iterator it = stratified_weights.begin(); it != stratified_weights.end(); ++it )
	{
		*it = sum;
		sum += 1.0/number_particles;
	}
	
	copy(current_positions.begin(), current_positions.end(), original_particles.begin());
	
	vector <double>::iterator cumulative_distribution_it;
	typename vector < PositionType >::iterator selected_particle;
	vector <double>::iterator stratified_weights_it = stratified_weights.begin();
	
	selected_particle = original_particles.begin();
	cumulative_distribution_it = cumulative_distribution.begin();
	
	for (typename vector < PositionType >::iterator current_particle_position = current_positions.begin(); current_particle_position != current_positions.end(); ++current_particle_position)
	{
		while ( *stratified_weights_it > *cumulative_distribution_it)
		{
			++selected_particle;
			++cumulative_distribution_it;
		}
		
		*current_particle_position = *selected_particle;
		
		++stratified_weights_it;
	}
	
	// I think this is all for the resampling, set the weights to 1/number_particles
	
	for ( vector <double>::iterator it = current_weights.begin(); it != current_weights.end(); ++it )
	{
		*it = 1.0/number_particles;
	}
	
}

//*********** Other NP algorithms **************//

// This in fact is just a gibbs sampler with embedded Metropolis-Hastings steps.

template <class Graph>
void metropolis (Graph & g, const unsigned int max_steps, const unsigned int burnoff_steps = 0, bool time = false)
{
	vector < pair < double, double > > samples_vector ( num_vertices (g), pair < double, double > (0.0, 0.0) );
	
	metropolis_implementation(g, make_iterator_property_map(samples_vector.begin(), get(vertex_index,g)), max_steps, burnoff_steps, time);
}

template <class Graph, class SampleMap>
void metropolis_implementation (Graph & g, SampleMap previous_samples, const unsigned int max_steps, const unsigned int burnoff_steps, bool time)
{
	typename graph_traits<Graph>::vertex_iterator u, u_end;
	typename graph_traits<Graph>::adjacency_iterator v, v_end;
	
	ContinuousRandomVariable * current_random_variable;
	unsigned int current_variable_index;
	
	double new_observation_weight, new_sample;
	
	ContinuousPotential * observation_potential = NULL;
	
	ParticleFilterProposal proposal;
	
	// Seeding the RNG with a truely random value, should be replaced by clock() or something
	
	//fibonnacci_number_generator.seed(66);
	
	bool record = false;
	
	assert ( burnoff_steps < max_steps);
	
	fibonnacci_number_generator.seed(std::time(NULL));
	
	cout << "Running Metropolis with " << max_steps << (time ? " seconds."  :  " steps.") << endl;
	
	// Main Metropolis step
	
	unsigned int mh_step(0);
	
	double used_time(0.0);
	
	double last_progress_dot_time = double (max_steps)/100.0;
	
	if (time)
	{
		global_timer.restart();
	}
	
	while (time ? used_time < max_steps : mh_step < max_steps )
	{
		++mh_step;
		
		if (!time)
		{
			if ( (mh_step % (max_steps/100) == 0) || (max_steps < 100) )
			{ 
				cout << "." << flush; 
			}
			
			if ( mh_step > burnoff_steps )
			{ 
				record = true; 
			}
		}
		
		else
		{
			if (used_time > last_progress_dot_time)
			{ 
				cout << "." << flush;
				last_progress_dot_time += double (max_steps)/100.0;
			}
			
			if ( used_time > burnoff_steps )
			{ 
				record = true; 
			}
			
			current_experiment->callback_monitoring(used_time);
		}
		
		for (tie (u,u_end) = vertices(g); u != u_end; ++u)
		{
			if ( out_degree (*u,g) == 1)
			{
				// We are in an observation node 
				continue;
			}
			
			//cout << "RV index " << g[*u]->get_random_variable()->get_index();
			
			current_random_variable = static_cast < ContinuousRandomVariable * > (g[*u]->get_random_variable());
			current_variable_index = current_random_variable->get_index();
			
			list <GaussianPotential *> edge_gaussian_lst;
			
			for (tie (v,v_end) = adjacent_vertices(*u,g); v != v_end; ++v)
			{
				if ( out_degree (*v,g) == 1)
				{
					// We are in an observation node, get the observation potential
					
					observation_potential = g[edge(*u,*v,g).first];
				}
				
				else 
				{
					edge_gaussian_lst.push_back( static_cast<GaussianPotential *> (g[edge(*u,*v,g).first]) );
				}
			}
			
			proposal.setup_gaussian_proposal (edge_gaussian_lst, current_variable_index);
			
			new_sample = proposal.get_proposal_position(*current_random_variable);
			
			pair <double, double> & previous_sample = get (previous_samples, *u);
			
			observation_potential->set_variable_value(*current_random_variable);
			
			new_observation_weight = observation_potential->get_potential_value();
			
			/*
			 if (new_observation_weight == 0.0)
			 {
				 cout << "Here too we reach 0 in OW" << endl;
			 }
			 */
			
			// previous_observation_weight = previous_sample.second;
			
			//previous_observation_weight = observation_potential->get_potential_value();
			
			if (*u == 19)
			{
				if (previous_sample.second == 0.0)
				{
					//cout << "arg " << mh_step << endl;
					
				}	
				else 
				{
					//cout << previous_sample.second<< " " << mh_step << endl;
				}
				
				
			}	
			
			double ratio = new_observation_weight / previous_sample.second;
			
			double z = uniform_distribution_0_1();
			
			if (z < ratio)
			{
				// We accept the new sample
				
				// Record it only if we are beyond the burnoff period
				
				if (record)
				{
					current_random_variable->record_sample(new_sample);
				}
				
				previous_sample.first = new_sample;
				previous_sample.second = new_observation_weight;
				
				// We need to update the potentials
				
				for (tie (v,v_end) = adjacent_vertices(*u,g); v != v_end; ++v)
				{
					if ( out_degree (*v,g) == 1)
					{
						// We are in an observation node 
						continue;
					}
					
					g[edge(*u,*v,g).first]->set_variable_value(*current_random_variable);
					
				}
				
			}
			
			else 
			{
				// Sample is rejected, we use the old one
				if (record)
				{
					current_random_variable->record_sample( previous_sample.first);
				}
				
			}
			
			
		}
		
		if (time)
		{
			used_time = global_timer.elapsed();
		}
		
	}
	
	for (tie (u,u_end) = vertices(g); u != u_end; ++u)
	{
		if ( out_degree (*u,g) == 1)
		{
			// We are in an observation node 
			continue;
		}
		
		g[*u]->get_random_variable()->obtain_inference_from_prior_samples();
	}
	
	cout << endl;
	
	if (time)
	{
		cout << "We used " << mh_step << " Metropolis-Hastings steps." << endl;
	}
}