particlefilters.cc.svn-base

Probabilistic graphical models in matlab.

				if ( *u != double_previous_node)
				{
					current_node = *u;
					interaction_potential = g[ edge(previous_node, current_node, g).first];
					
					internal_potential = static_cast < DiscretePotential *> (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 << "Current node is " << current_node << " (" << g[current_node]->get_random_variable()->get_index() << ")" << ", previous was " << previous_node << " (" << g[previous_node]->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 = g[current_node]->get_random_variable();
		previous_random_variable = g[previous_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 <unsigned int> & current_positions = get (positions, current_node);		
		vector <unsigned int> & previous_positions = get (positions, previous_node);
		
		vector <unsigned int>::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_discrete_proposal(static_cast<ChainedSingleDiscretePotential &> (*internal_potential), *current_random_variable);
		
		for (vector <unsigned int>::iterator current_particle_position = current_positions.begin(); current_particle_position != current_positions.end(); ++current_particle_position)
		{
			// Sampling the new particle
			
			previous_random_variable->set_value (*previous_particle_position);
			
			interaction_potential->set_variable_value( *previous_random_variable);
			
			proposal.add_discrete_proposal(*interaction_potential, *current_random_variable);
			
			*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); // All of these are discrete potentials	
			
			interaction_potential->set_variable_value (*current_random_variable);
			
			// Setting Internal Potential
			
			internal_potential->set_variable_value (*current_random_variable);
			
			// cout << "New particle, weight of old particle, obs, int, internal, prop: " << *current_particle_position << ", " << *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) << endl;
			
			*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);
			
			// cout << "Weight of new particle: " << *current_particle_weight << endl;
			
			++previous_particle_position;
			++previous_particle_weight;
			++current_particle_weight;
		}
		
		// Normalize the weights 
		
		if (!fast_double_normalize_over (current_weights.begin(), current_weights.end()))
		{
			cout << "Weights all to 0" << endl;
			
		}
		
		// Resample part
		
		resample (current_positions, current_weights, cumulative_distribution, stratified_weights, original_particles);
		
		//g[current_node]->get_random_variable()->set_mean_particles(current_positions);
		
		if (current_node == 54)
		{
			for ( vector <unsigned int>:: iterator it = current_positions.begin(); it != current_positions.end(); ++it)
			{
				//cout << *it << endl;
			}
		}
		
		if ( out_degree(current_node,g) == 2)
		{
			// We are finished
			
			return current_node;
		}
		
		double_previous_node = previous_node;
		previous_node = current_node;
		
	}
}


template < >
void backward_smoothing_implementation < subgraph<DiscreteGraph>, DiscretePositionMap, DiscreteWeightMap > (subgraph<DiscreteGraph> & g, const graph_traits< subgraph<DiscreteGraph> >::vertex_descriptor tail_node, DiscretePositionMap & positions, DiscreteWeightMap & weights, const unsigned int number_samples)
{
	
	//cout << "there" << endl;
	
	typedef graph_traits< subgraph<DiscreteGraph> >::vertex_descriptor VertexDescriptor;
	
	VertexDescriptor double_next_node, next_node, current_node;
	
	graph_traits<subgraph<DiscreteGraph> >::adjacency_iterator u, u_end;
	
	unsigned int 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)
	{
		//cout << "current sample" << current_sample << endl;
		
		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();
		
		current_sample_position = static_cast < DiscreteRandomVariable * > (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
		
		DiscretePotential * interaction_potential = NULL;
		DiscretePotential * 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 = g[edge(next_node, current_node, g).first];
						internal_potential = static_cast < DiscretePotential *> (g[current_node]->get_potential() );
					}
				}
			}
			
			
			//unsigned int current_variable_index =  g[current_node]->get_random_variable()->get_index();
			
			next_random_variable = g[next_node]->get_random_variable();
			current_random_variable = g[current_node]->get_random_variable();
			
			//cout << "Current node (BS) is " << current_variable_index << endl;
			
			// Here this is the only time we set up the interaction potential based on the sampled value
			
			interaction_potential->set_variable_value(* next_random_variable);
			
			vector <unsigned int> & 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 <unsigned int>::iterator current_particle_position = current_positions.begin(); current_particle_position != current_positions.end(); ++current_particle_position)
			{
				static_cast <DiscreteRandomVariable *> (current_random_variable)->set_value(*current_particle_position);
				
				interaction_potential->set_variable_value( * current_random_variable);
				
				//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());
			
			// CHECK_ME: not sure about the node... Checked, OK
			
			current_sample_position = static_cast <DiscreteRandomVariable * > (current_random_variable)->sample_from_particles(current_positions,  current_new_weights);
			next_sample_position = current_sample_position;
			
			vector <double>:: iterator jt = current_new_weights.begin();
			
			if ( current_random_variable->get_index() == 55 )
			{
				for ( vector <unsigned int>:: iterator it = current_positions.begin(); it != current_positions.end(); ++it)
				{
					
					//cout << *it << " " <<  *jt << endl;
					++jt;
				}
				
						//	cout << "*******************" << endl;
			}
			

			
			static_cast <DiscreteRandomVariable * >  (current_random_variable)->set_mean_bw_particles(current_positions,  current_new_weights);
			
			internal_potential->set_variable_value(*current_random_variable);
			
			if ( out_degree(current_node,g) == 2)
			{
				// We are finished
				
				break;
			}
			
			double_next_node = next_node;
			next_node = current_node;		
		}
	}
}

template <>
void non_parametric_tree_gibbs_sampler_implementation < DiscreteGraph> (DiscreteGraph & g, const unsigned int max_steps, const unsigned int number_particles, const unsigned int backward_samples, bool time)
{
	typedef graph_traits<DiscreteGraph>::vertex_iterator  VertexIterator;
	typedef graph_traits<DiscreteGraph>::adjacency_iterator  AdjacencyIterator;
	typedef graph_traits<DiscreteGraph>::vertex_descriptor  VertexDescriptor;
	typedef graph_traits<DiscreteGraph>::edge_descriptor EdgeDescriptor;
	
	typedef graph_traits< subgraph<DiscreteGraph> >::vertex_descriptor SubGraphVertexDescriptor;
	
	VertexIterator u, u_end;
	AdjacencyIterator v,v_end;
	VertexDescriptor w;
	
	subgraph<DiscreteGraph>::children_iterator current_tree, current_tree_end;
	
	subgraph <DiscreteGraph> subgraph_g;
	
	copy_graph(g, subgraph_g);
	
	// Use the following function if you are dealing with a MRF with same and pair number of rows / columns 
	
	partition_mrf_graph_into_chains(subgraph_g);
	
	// Else use the following if you are dealing with a single chain
	
	//partition_simple_chain(subgraph_g);
	
	//display_subgraph(subgraph_g);
	
	// Following block set up the correct internal potential for a variable given the edges linking it to the other variables (conditionned)
	
	
	list < vector < vector < double > > * > trees_weights_map;
	list < vector < vector < unsigned int > > * > trees_positions_map;
	
	for ( tie(current_tree, current_tree_end) = subgraph_g.children(); current_tree != current_tree_end; ++current_tree)
	{
		vector < double > particles_weights (number_particles);
		
		vector < vector < double > > * weights_vector = new  vector < vector < double > > ( num_vertices (*current_tree), particles_weights);
		
		trees_weights_map.push_back(weights_vector);
		
		vector < unsigned int > particles_positions (number_particles);
		
		vector < vector < unsigned int > > * positions_vector = new  vector < vector < unsigned int > > ( num_vertices (*current_tree), particles_positions);
		
		trees_positions_map.push_back(positions_vector);
		
		for (tie(u,u_end) = vertices (*current_tree); u!= u_end; ++u)
		{
			w = current_tree->local_to_global(*u);
			
			for (tie(v, v_end)= adjacent_vertices ( w, g ); v != v_end; ++v)
			{
				if (!current_tree->find_vertex(*v).second)
				{
					// Add that edge potential to the ChainedSinglePotential
					
					// BUG HERE, it is the potential of the EDGE !!!!!  ... Fixed
					
					static_cast < ChainedSingleDiscretePotential * > (g[w]->get_potential())->add_potential( g[edge(w,*v,g).first] );
				}
			}
			
		}
		
	}
	
	// End
	
	list < vector < vector < unsigned int > > * >::iterator current_tree_positions_map = trees_positions_map.begin();
	list < vector < vector < double > > * >::iterator current_tree_weights_map = trees_weights_map.begin();
	
	vector < iterator_property_map < vector< vector < unsigned int > >::iterator, property_map < subgraph<DiscreteGraph>, vertex_index_t>::type > > positions_property_maps_vec;
	vector < iterator_property_map < vector< vector < double > >::iterator, property_map < subgraph<DiscreteGraph>, vertex_index_t>::type > > weights_property_maps_vec;
	
	for ( tie(current_tree, current_tree_end) = subgraph_g.children(); current_tree != current_tree_end; ++current_tree)
	{		
		positions_property_maps_vec.push_back(make_iterator_property_map( (*current_tree_positions_map)->begin(), get(vertex_index, *current_tree)));
		
		weights_property_maps_vec.push_back(make_iterator_property_map( (*current_tree_weights_map)->begin(), get(vertex_index, *current_tree)));
		
		++current_tree_weights_map;
		++current_tree_positions_map;
	}
	
	cout << "Running NP Chain Gibbs (Discrete) with " << max_steps << (time ? " seconds and " : " steps and ") << number_particles << " particles." << endl;
	
	fibonnacci_number_generator.seed(std::time(NULL));
	
	unsigned int gibbs_step(0);
	
	double time_used(0.0);
	
	double last_progress_dot_time = double (max_steps)/100.0;
	
	if (time)
	{
		global_timer.restart();