/home/hauberg/Dokumenter/Capture/humim-tracker-0.1/src/pf_skeleton_tracker.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 PF_SKELETON_TRACKER_H
#define PF_SKELETON_TRACKER_H

#include <ctime>

#include "particle_filter.h"
#include "angle_state.h"
#include "pik_state.h"
#include "tangent_state.h"
#include "projection_state.h"
#include "KBK_stick_state.h"
#include "stick_state.h"
#include "stick2d_state.h"
#include "brownian_stick_state.h"
#include "pik_stick_state.h"
#include "brownian_state.h"
#include "bgsub_state.h"
#include "angle_bgsub_state.h"
#include "hull_state.h"
#include "point_cloud.h"
#include "kinect_cloud.h"
#include "triclops_cloud.h"
#include "cloud_and_stick.h"
#include "skeleton_cloud_measure.h"
#include "skeleton_pose_estimator.h"
#include "options.h"
#include "mocap_data.h"
#include "calibration.h"

#define TRACKER(state_type, observation_type) \
        pf_skeleton_tracker< state_type<observation_type>, observation_type > 

//#define FILTER_TYPE annealed_particle_filter
#define FILTER_TYPE bootstrap

template <class _state_type, class observation_type>
class pf_skeleton_tracker : public FILTER_TYPE<_state_type, observation_type>
{
public:
  typedef _state_type state_type;

  pf_skeleton_tracker (const options &opts, calibration &_calib, const bool _playback = false)
    : FILTER_TYPE<state_type, observation_type> (opts, _calib, _playback),
      calib (_calib),
      observation (opts, _calib),
      output_directory (opts.output_directory ()),
      mean_file (fullfile (output_directory, "mean_points.txt")),
      validation_file (fullfile (output_directory, "validation.txt")),
      playback (_playback),
      m (opts, _calib),
      meas (opts, _calib),
      mocap (opts, _calib)
    {
      mean_fid = fopen (mean_file.c_str (), "w");
      validation_fid = fopen (validation_file.c_str (), "w");
      if (!playback)
        state_log.open (fullfile (output_directory, "state_log.txt").c_str ());
        
      srand (time (NULL));
      
      observation.refresh (false);
    };
  
  ~pf_skeleton_tracker ()
    {
      if (mean_fid != NULL)
        fclose (mean_fid);
      
      if (validation_fid != NULL)
        fclose (validation_fid);
      
      if (!playback)
        state_log.close ();
    }
  
  bool refresh ()
    {
      bool valid = observation.refresh ();
      mocap.refresh ();
      if (valid)
        {
          if (playback)
            {
              valid = FILTER_TYPE<state_type, observation_type>::load_states ();
            }
          else
            {
              FILTER_TYPE<state_type, observation_type>::refresh (observation);
              
              for (size_t k = 0; k < this->num_particles; k++)
                this->states [k]->write_to_log (state_log);
            }
          compute_mode ();
          
          // Perform optional validation
          if (validation_fid != NULL)
            {
              const double score = mocap.average_pose_dist (m);
              fprintf (validation_fid, "%lf\n", score);
            }
        }
        
      return valid;
    };
  
  calibration& get_calibration ()
    {
      return calib;
    };
  
  void prepare_mesh ()
    {
      m.prepare_mesh ();
    }
  
  void show () const
    {
      glPushMatrix ();
      glScalef (0.5, 0.5, 0.5);
      glTranslatef (0, -4, 0);
      
//      const double max_weight = find_max (this->weights);
      for (size_t k = 0; k < this->num_particles; k++)
        this->states [k]->show (); //this->weights [k]/max_weight);
//      m.show ();
      
      //for (size_t k = 0; k < this->num_particles; k++)
      //  OpenTissue::gl::DrawPoint (this->states [k]->get_root (), 0.15);
      
      /*
      for (size_t k = 0; k < this->num_particles; k++)
        {
          int l = 0;
          for (const_bone_iter iter = this->states [k]->bone_begin (); iter != this->states [k]->bone_end (); iter++)
            {
              const vector3 colour = jet ((double)l/14.0);
              OpenTissue::gl::ColorPicker (colour [0], colour [1], colour [2]);
              OpenTissue::gl::DrawPoint (iter->absolute ().T (), 0.15);
              l++;
            }
        }
      */

      observation.show ();
      mocap.show ();

      glPopMatrix ();
    };
  
  const state_type& get_pose_estimate ()
    {
      return m;
    };
  
  const observation_type& get_observation () const
    {
      return observation;
    }
    
protected:
  double measure_hypothesis (state_type &state, const observation_type &observation)
    {
      return meas.log_measure (state, observation);
    };

  void compute_mode ()
    {
      // Compute mode
      //mode<state_type> (this->weights, this->states, 0.5, m);
      mean< state_type > (this->weights, this->states, m);
      //projected_mean< state_type > (this->weights, this->states, m);
      //max<state_type> (this->weights, this->states, m);
      
      m.compute_pose ();

      if (mean_fid != NULL)
        {
          for (const_bone_iter iter = m.bone_begin (); iter != m.bone_end (); iter++)
            {
              const vector3 p = iter->absolute ().T ();
              fprintf (mean_fid, "%f %f %f ", p (0), p (1), p (2));
            }
          fprintf (mean_fid, "\n");
        }
    };

  calibration &calib;
  observation_type observation; // XXX: const???
  const std::string output_directory, mean_file, validation_file;
  const bool playback;
  state_type m;
  measure<state_type, observation_type> meas;
  FILE *mean_fid;
  mocap_data<observation_type> mocap;
  FILE *validation_fid;
  std::ofstream state_log;
};

#endif // PF_SKELETON_TRACKER_H