particlefilters.cc.svn-base

Probabilistic graphical models in matlab.

		time_used = global_timer.elapsed();
	}
	
	cout << endl;
	
	if (time)
	{
		cout << "We were able to generate " << gibbs_step << " complete tree-Gibbs samples." << endl;
	}
	
	// Here the Gibbs loop is done. That's all.
}

template < class Graph, class PositionMap, class WeightMap>
typename graph_traits< Graph >::vertex_descriptor forward_filtering_implementation (Graph & g, const typename graph_traits< Graph >::vertex_descriptor root_node, PositionMap & positions, WeightMap & weights)
{
	// Variable declarations
	
	typedef typename graph_traits< Graph >::vertex_descriptor VertexDescriptor;
	
	ContinuousRandomVariable * current_random_variable;
	
	unsigned int current_variable_index, previous_variable_index ;
	
	VertexDescriptor double_previous_node, previous_node;
	
	VertexDescriptor current_node = VertexDescriptor ();
	
	previous_node = root_node;
	
	double_previous_node = root_node;
	
	typename graph_traits<Graph>::adjacency_iterator u, u_end;
	
	ParticleFilterProposal proposal;
	
	unsigned int number_particles = get (positions, root_node).size();
	
	initialize_particles (g, get (positions, root_node), get (weights, root_node), root_node, proposal);
	
	ContinuousPotential * internal_potential = NULL;
	ContinuousPotential * observation_potential = NULL;
	GaussianPotential * interaction_potential = NULL;
	
	vector <double> cumulative_distribution (number_particles);
	vector <double> stratified_weights (number_particles);
	vector <double> 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)
			{
				
				if ( *u != double_previous_node)
				{
					current_node = *u;
					interaction_potential = static_cast <GaussianPotential *> (g[edge(previous_node, current_node, g).first]);
					
					// WARNING HERE: NOT SURE about the legitimacy of the cast, ConstantPotential should inherit both from Continuous and DiscretePotential
					
					internal_potential = static_cast <ContinuousPotential *>(g[current_node]->get_potential());
				}
			}
		}
		
		for (tie (u, u_end) = adjacent_vertices(current_node, g); u != u_end; ++u)
		{
			if (out_degree(*u,g) == 1)
			{
				observation_potential =  g[edge(current_node, *u, g).first];
				//cout << "Observation node is " << *u << " (" << g[*u]->get_random_variable()->get_index() << ")" << endl;
			}
		}
		
		//cout << "Current node (FF) is " << g[current_node]->get_random_variable()->get_index()  << endl;
		
		previous_variable_index = g[previous_node]->get_random_variable()->get_index();
		
		current_random_variable = static_cast<ContinuousRandomVariable *> (g[current_node]->get_random_variable());
		current_variable_index = current_random_variable->get_index();
		
		// Sampling particle from proposal
		
		vector <double> & current_weights = get (weights, current_node);		
		vector <double> & previous_weights = get (weights, previous_node);
		vector <double> & current_positions = get (positions, current_node);		
		vector <double> & previous_positions = get (positions, previous_node);
		
		vector <double>::iterator previous_particle_position = previous_positions.begin();
		vector <double>::iterator previous_particle_weight = previous_weights.begin();
		vector <double>::iterator current_particle_weight = current_weights.begin();
		
		//cout << "First weight (before update): " << *current_weights.begin() << endl;
		
		proposal.setup_gaussian_proposal(static_cast<ChainedSinglePotential *>(internal_potential), current_variable_index);
		
		for (vector <double>::iterator current_particle_position = current_positions.begin(); current_particle_position != current_positions.end(); ++current_particle_position)
		{
			// Sampling the new particle
			
			// cout << "Particle number " << current_particle_position - current_positions.begin() << endl;
			
			interaction_potential->set_variable_value (previous_variable_index, *previous_particle_position);
			
			//cout << "Var:" << interaction_potential->get_variance() << endl;
			
			proposal.add_gaussian_proposal(*previous_particle_position, interaction_potential->get_variance());
			
			*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); // It is the Bernouilli one, last_sampled_value must be set from the particle. done now (hope)
			
			interaction_potential->set_variable_value (*current_random_variable); // It is a Gaussian potential
			
			// Setting Internal Potential
			
			internal_potential->set_variable_value (*current_random_variable); // don't forget to set the internal potential also !!
			
			*current_particle_weight = ( *previous_particle_weight * observation_potential->get_potential_value() * interaction_potential->get_potential_value() * internal_potential->get_potential_value() )/ proposal.get_proposal_weight(*current_particle_position);
			
			/*
			 if (*current_particle_weight == 0.0)
			 {
				 cout << "New particle, proposal mean, proposal variance: " << *current_particle_position << " , " <<  proposal.debug_get_gaussian_mean() << " , " << proposal.debug_get_gaussian_variance() << endl;
				 
				 cout << "Observation, interaction, internal, proposal: "  << observation_potential->get_potential_value() << " , " << interaction_potential->get_potential_value() << ", " <<internal_potential->get_potential_value() << " , " << proposal.get_proposal_weight(*current_particle_position) << endl;
			 }
			 */
			
			// cout << "Weight of new particle: " << *current_particle_weight << endl;
			
			++previous_particle_position;
			++previous_particle_weight;
			++current_particle_weight;
		}
		
		// Normalize the weights 
		
		/*
		 cout << "Weights: " << endl;
		 
		 for ( vector <double>::iterator it = current_weights.begin(); it != current_weights.end(); ++it )
		 {
			 cout << *it << endl;
		 }		
		 
		 cout << "done" << endl << endl << endl;
		 
		 vector <double> test_weights = get (weights, current_node);
		 
		 for ( vector <double>::iterator it = test_weights.begin(); it != test_weights.end(); ++it )
		 {
			 cout << *it << endl;
		 }	
		 
		 cout << "done" << endl << endl << endl;
		 */
		
		if ( !fast_double_normalize_over (current_weights.begin(), current_weights.end()) )
		{
			cout << "Weights equal 0.0" << endl;
			
			for ( vector <double>::iterator it = current_weights.begin(); it != current_weights.end(); ++it )
			{
				*it = 1.0;
			}	
			
		}
		
		// Resample part
		
		resample ( current_positions, current_weights, cumulative_distribution, stratified_weights, original_particles);
		
		// End of one step of the forward pass
		
		/*
		 cout << "Particles: " << endl;
		 
		 for ( vector <double>::iterator it = current_positions.begin(); it != current_positions.end(); ++it )
		 {
			 cout << *it << endl;
		 }
		 */
		
		if ( out_degree(current_node,g) == 2)
		{
			// We are finished
			
			/*
			 cout << "Particles (at the last step): " << endl;
			 
			 for ( vector <double>::iterator it = current_positions.begin(); it != current_positions.end(); ++it )
			 {
				 cout << *it << endl;
			 }
			 */
			return current_node;
		}
		
		double_previous_node = previous_node;
		previous_node = current_node;
		
	}
	
	//delete proposal;
	
}

// We are going to try to implement a way of getting more samples in the backward pass

template < class Graph, class PositionMap , class WeightMap>
void backward_smoothing_implementation (Graph & g, const typename graph_traits<Graph>::vertex_descriptor tail_node, PositionMap & positions, WeightMap & weights, const unsigned int number_samples)
{
	typedef typename graph_traits<Graph>::vertex_descriptor VertexDescriptor;
	
	VertexDescriptor double_next_node, next_node, current_node;
	typename graph_traits<Graph>::adjacency_iterator u, u_end;
	
	double next_sample_position, current_sample_position;
	
	RandomVariable * next_random_variable;
	RandomVariable * current_random_variable;
	
	vector <double> current_new_weights (get (weights, tail_node).size());
	
	for (unsigned int current_sample = 0; current_sample < number_samples; ++current_sample)
	{
		
		next_node = tail_node;
		double_next_node = tail_node;
		current_node = tail_node;
		
		next_random_variable = g[next_node]->get_random_variable();
		current_random_variable = g[current_node]->get_random_variable();
		
		// We sample the tail node variable, note that we could normalize weights there...
		
		current_sample_position = static_cast < ContinuousRandomVariable * > (current_random_variable)->sample_from_particles(get (positions, tail_node), get (weights, tail_node));
		
		next_sample_position = current_sample_position;
		
		g[current_node]->get_potential()->set_variable_value(* current_random_variable); // set the internal potential
		
		GaussianPotential * interaction_potential = NULL;
		ContinuousPotential * internal_potential = NULL;
		
		while (true)
		{
			for (tie (u, u_end) = adjacent_vertices(next_node, g); u != u_end; ++u)
			{
				if (out_degree(*u,g) != 1)
				{
					// FIX_ME: done
					
					if (*u != double_next_node)
					{
						current_node = *u;
						interaction_potential = static_cast <GaussianPotential *> (g[edge(next_node, current_node, g).first]);
						internal_potential = static_cast <ContinuousPotential *> (g[current_node]->get_potential() );
					}
				}
			}
			
			next_random_variable = g[next_node]->get_random_variable();
			
			current_random_variable = g[current_node]->get_random_variable();
			unsigned int current_variable_index =  current_random_variable->get_index();
			
			//cout << "Current node (BS) is " << current_variable_index << endl;
			
			// Here this is the only time we set up the potentials based on the sampled value
			
			interaction_potential->set_variable_value(* next_random_variable);
			
			vector <double> & current_positions = get (positions, current_node);
			vector <double> & current_weights = get (weights, current_node);
			
			vector <double>::iterator current_particle_weight = current_weights.begin();
			vector <double>::iterator current_particle_new_weight = current_new_weights.begin();
			
			for (vector <double>::iterator current_particle_position = current_positions.begin(); current_particle_position != current_positions.end(); ++current_particle_position)
			{
				interaction_potential->set_variable_value( current_variable_index, *current_particle_position);
				
				//cout << "Updated Weight (previous_weight, interaction_potential): " << *current_particle_weight * interaction_potential->get_potential_value() << " ( " << *current_particle_weight << ", " << interaction_potential->get_potential_value() << " ) " << endl;
				
				*current_particle_new_weight = *current_particle_weight * interaction_potential->get_potential_value();
				
				++current_particle_weight;
				++current_particle_new_weight;
			}
			
			/*
			 cout << "Updated Weights: " << endl;
			 
			 for ( vector <double>::iterator it = current_weights.begin(); it != current_weights.end(); ++it )
			 {
				 cout << *it << endl;
			 }
			 */
			
			// Normalize the weights (CRUCIAL HERE)
			
			fast_double_normalize_over (current_new_weights.begin(), current_new_weights.end());
			
			// We now sample the current RV, with respect to the updated weights
			
			current_sample_position = static_cast <ContinuousRandomVariable *> (current_random_variable)->sample_from_particles(current_positions,  current_new_weights);
			next_sample_position = current_sample_position;
			
			// And we set the internal Potential based on the value obtained
			
			internal_potential->set_variable_value(* current_random_variable);
			
			static_cast <ContinuousRandomVariable * >  (current_random_variable)->set_mean_bw_particles(current_positions,  current_new_weights);
			
			if ( out_degree(current_node,g) == 2)
			{
				// We are finished
				
				break;
			}
			
			double_next_node = next_node;
			next_node = current_node;		
		}
		
	}
	
	//dynamic_cast< ContinuousRandomVariable *> (g[vertex(0,g)]->get_random_variable())->debug_display_state();
}

template < class Graph, class PositionType, class ParticleFilterProposalType >
void initialize_particles (Graph & g, vector <PositionType> & initial_positions, vector <double> & initial_weights, typename graph_traits<Graph>::vertex_descriptor root_node, ParticleFilterProposalType & proposal)
{
	typedef typename graph_traits<Graph>::vertex_descriptor VertexDescriptor;