/home/hauberg/Dokumenter/Capture/humim-tracker-0.1/src/particle_filter.h

Go to the documentation of this file.

// Copyright (C) 2011 Soren Hauberg
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, see <http://www.gnu.org/licenses/>.
 
#ifndef PARTICLE_FILTER_H
#define PARTICLE_FILTER_H

#include <vector>
#include <iostream> // XXX: kill me
#include <fstream>
#include <omp.h>

#include "simulator.h"
#include "options.h"
#include "auxil.h"
#include "skeleton_state.h"
#include "calibration.h"

// Helper function for normalising particle weights
inline
void normalise_weights (std::vector<double> &weights, double weights_sum = -1)
{
  const size_t num_particles = weights.size ();
  
  // Check if we need to compute the sum
  if (weights_sum < 0)
    {
      weights_sum = weights [0];
      for (size_t k = 1; k < num_particles; k++)
        weights_sum += weights [k];
    }
  
  // Divide with the sum (if the sum is zero, reset the weights)
  if (weights_sum > 0)
    for (size_t k = 0; k < num_particles; k++)
      weights [k] = weights [k] / weights_sum;
  else
    {
      const double w = 1.0 / (double)num_particles;
      for (size_t k = 0; k < num_particles; k++)
        weights [k] = w;
    }
}

inline
void normalise_log_weights (std::vector<double> &weights)
{
  const size_t num_particles = weights.size ();

  // Subtract maximum weight from other weights and re-express the weights
  // on an ordinary scale
  double max_weight = weights [0];
  for (size_t k = 1; k < num_particles; k++)
    max_weight = std::max (weights [k], max_weight);
      
  for (size_t k = 0; k < num_particles; k++)
    weights [k] = exp (weights [k] - max_weight);
  
  // Compute the sum
  double weights_sum = weights [0];
  for (size_t k = 1; k < num_particles; k++)
    weights_sum += weights [k];
  
  // Divide with the sum (if the sum is zero, reset the weights)
  if (weights_sum > 0)
    for (size_t k = 0; k < num_particles; k++)
      weights [k] = weights [k] / weights_sum;
  else
    {
      const double w = 1.0 / (double)num_particles;
      for (size_t k = 0; k < num_particles; k++)
        weights [k] = w;
    }

/*
  std::cerr << "[";
  for (size_t k = 0; k < num_particles; k++)
    std::cerr << weights [k] << " ";
  std::cerr << "]" << std::endl;
*/
}

template<typename state_type, typename observation_type>
class particle_filter
{
protected:
  const size_t num_particles; // number of particles
  std::vector<state_type*> states; // store the states for each particle
  std::vector<double> weights; // store the weights
  const std::string output_directory;
  int frame;

public:
  particle_filter (const options &opts, const calibration &calib, const bool playback = false)
    : num_particles (opts.num_particles ()),
      states (num_particles),
      weights (num_particles, 1.0/(double)num_particles),
      output_directory (opts.output_directory ()),
      frame (opts.data_start_idx ())
    {
      // Add noise to particles
      if (!playback)
        {
          for (size_t k = 0; k < num_particles; k++)
            {
              states [k] = new state_type (opts, calib, false);
              //states [k]->predict ();
            }
        }
    }
    
  virtual ~particle_filter ()
    {
      for (size_t k = 0; k < num_particles; k++)
        delete states [k];
    }

  size_t get_num_particles () const
    {
      return num_particles;
    }
    
  std::vector<state_type*>& get_states ()
    {
      return states;
    }

  void save_weights (const char *filename)
    {
      std::ofstream file (filename);
      if (!file.is_open ())
        std::cerr << "particle_filter:save_weights: couldn't open '" << filename
                  << "' for writing" << std::endl;
      else
        {
          for (size_t k = 0; k < this->num_particles; k++)
            file << weights [k] << " ";
          file.close ();
        }
    }
  
  bool load_weights (const char *filename)
    {
      std::ifstream file (filename);
      if (!file.is_open ())
        {
          std::cerr << "particle_filter:load_weights: couldn't open '" << filename
                    << "' for reading" << std::endl;
          return false;
        }
      else
        {
          for (size_t k = 0; k < this->num_particles; k++)
            file >> weights [k];
          file.close ();
        }
      return true;
    }
  
  void save_states ()
    {
      const std::string pose_template = fullfile (output_directory, "states_%d.poses");
      char filename [200];
      sprintf (filename, pose_template.c_str (), frame);
      
      std::ofstream file (filename);
      if (!file.is_open ())
        std::cerr << "particle_filter::save_states: could not open '" << filename
                  << "' for writing" << std::endl;
      else
        {
          for (size_t k = 0; k < this->num_particles; k++)
            states [k]->save (file);
        }
      file.close ();
    
      const std::string weight_template = fullfile (output_directory, "weights_%d.txt");
      sprintf (filename, weight_template.c_str (), frame);
      save_weights (filename);

      const std::string spatial_template = fullfile (output_directory, "spatial_%d.poses");
      sprintf (filename, spatial_template.c_str (), frame);
      std::ofstream file2 (filename);
      if (!file2.is_open ())
        std::cerr << "particle_filter::save_states: could not open '" << filename
                  << "' for writing" << std::endl;
      else
        {
          for (size_t k = 0; k < this->num_particles; k++)
            {
               for (const_bone_iter iter = states [k]->bone_begin ();
                    iter != states [k]->bone_end (); iter++)
                 {
                   const vector3 p = iter->absolute ().T ();
                   file2 << p (0) << " " << p (1) << " " << p (2) << " ";
                 }
               file2 << std::endl;
            }
        }
      file2.close ();
    
      frame ++;
    }

  bool load_states ()
    {
      const std::string pose_template = fullfile (output_directory, "states_%d.poses");
      char filename [200];
      sprintf (filename, pose_template.c_str (), frame);
      
      bool success = false;
      std::ifstream file (filename);
      if (!file.is_open ())
        std::cerr << "particle_filter::load_states: could not open '" << filename
                  << "' for reading" << std::endl;
      else
        {
          success = true;
          for (size_t k = 0; k < this->num_particles; k++)
            states [k]->load (file);
        }
      file.close ();

      if (success)
        {
          const std::string weight_template = fullfile (output_directory, "weights_%d.txt");
          sprintf (filename, weight_template.c_str (), frame);
          success = load_weights (filename);
      
          frame ++;
        }
      
      return success;
    }

  //virtual void update_weights (const observation_type &observation) = 0;
  //virtual void update_particles (const observation_type &observation) = 0;
  virtual void refresh (const observation_type &observation) = 0;
};

template<typename state_type, typename observation_type>
class bootstrap : public particle_filter<state_type, observation_type>
{ 
public:
  bootstrap (const options &opts, const calibration &calib, const bool playback = false)
    : particle_filter<state_type, observation_type> (opts, calib, false),
      tmp_states (this->num_particles)
    {
      if (!playback)
        {
          for (size_t k = 0; k < this->num_particles; k++)
            this->tmp_states [k] = new state_type (opts, calib, false);
        }
      else
        {
          for (size_t k = 0; k < this->num_particles; k++)
            this->tmp_states [k] = NULL;
        }
    }
  
  virtual ~bootstrap ()
    {
      for (size_t k = 0; k < this->num_particles; k++)
        if (this->tmp_states [k] != NULL)
          delete this->tmp_states [k];
    }
    
  // Iterative functions
  void update_weights (const observation_type &observation)
    {
      #pragma omp parallel for
      for (size_t k = 0; k < this->num_particles; k++)
        this->weights [k] += measure_hypothesis (*this->states [k], observation);

      // Normalise
      normalise_log_weights (this->weights);
    }
    
  void update_particles (const observation_type &observation)
    {
      discrete dis (this->weights);
      for (size_t k = 0; k < this->num_particles; k++)
        *(this->tmp_states [k]) = *(this->states [dis.random ()]);

      for (size_t k = 0; k < this->num_particles; k++)
        *(this->states [k]) = *(this->tmp_states [k]);

      #pragma omp parallel for
      for (size_t k = 0; k < this->num_particles; k++)
        this->weights [k] = this->states [k]->predict (observation);
    }
  
  virtual
  void refresh (const observation_type &observation)
    {
      update_particles (observation);
      update_weights (observation);
    }
    
  virtual double measure_hypothesis (state_type &state, const observation_type &observation) = 0;

private:
  std::vector<state_type*> tmp_states;
};

template<typename state_type, typename observation_type>
class annealed_particle_filter : public particle_filter<state_type, observation_type>
{ 
public:
  annealed_particle_filter (const options &opts, const calibration &calib, const bool playback = false)
    : particle_filter<state_type, observation_type> (opts, calib, false),
      tmp_states (this->num_particles),
      num_annealing_steps (10), // XXX: get from 'opts'
      beta0 (1.0), // XXX: get from 'opts'?
      beta_increment (1.1) // XXX: get from 'opts'
    {
      if (!playback)
        {
          for (size_t k = 0; k < this->num_particles; k++)
            this->tmp_states [k] = new state_type (opts, calib, false);
        }
      else
        {
          for (size_t k = 0; k < this->num_particles; k++)
            this->tmp_states [k] = NULL;
        }
    }
  
  virtual ~annealed_particle_filter ()
    {
      for (size_t k = 0; k < this->num_particles; k++)
        if (this->tmp_states [k] != NULL)
          delete this->tmp_states [k];
    }
    
  // Iterative functions
  void update_weights (const observation_type &observation, const double beta)
    {
      #pragma omp parallel for
      for (size_t k = 0; k < this->num_particles; k++)
        {
          const double tmp = measure_hypothesis (*this->states [k], observation);
          this->weights [k] += -pow (-tmp, beta);
        }

      // Normalise
      normalise_log_weights (this->weights);
    }
    
  void update_particles (const observation_type &observation, const double beta)
    {
      discrete dis (this->weights);
      for (size_t k = 0; k < this->num_particles; k++)
        *(this->tmp_states [k]) = *(this->states [dis.random ()]);

      for (size_t k = 0; k < this->num_particles; k++)
        *(this->states [k]) = *(this->tmp_states [k]);

      #pragma omp parallel for
      for (size_t k = 0; k < this->num_particles; k++)
        this->weights [k] = this->states [k]->predict (observation, beta);
    }
    
  virtual
  void refresh (const observation_type &observation)
    {
      double beta = beta0;
      for (size_t k = 0; k < num_annealing_steps; k++)
        {
          update_particles (observation, beta);
          update_weights (observation, beta);
          beta *= beta_increment;
        }
    }
    
  virtual double measure_hypothesis (state_type &state, const observation_type &observation) = 0;

private:
  std::vector<state_type*> tmp_states;
  const size_t num_annealing_steps;
  const double beta0, beta_increment;
};
#endif // PARTICLE_FILTER_H