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

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

template <class observation_type>
class angle_bgsub_state : public skeleton_state<observation_type>
{
public:
  angle_bgsub_state (const options &opts, const calibration &_calib, const bool _for_show = true)
    : skeleton_state<observation_type> (opts, _calib, _for_show),
      pred_noise (opts.pred_noise ()),
      noise (0, pred_noise),
      extrapolate (opts.extrapolate ())
    {
      OpenTissue::kinematics::inverse::get_joint_parameters (this->skeleton, prev_theta);
      for (size_t k = 0; k < this->theta.size (); k++)
        this->prev_theta [k] = this->theta [k];
    };

  double predict (const observation_type &observation, const double variance_scale = 1.0)
    {
      const size_t max_rejections = 10000;
      const project2d &proj2d = observation.get_project2d ();    
      const size_t theta_size = this->theta.size ();

      num_rejections = 0;
    
      for (size_t attempt = 0; attempt < max_rejections; attempt++)
        {
          for (size_t idx = 0; idx < theta_size; idx++)
            {
              const double v = (idx == 27 || idx == 37)  ? 250 : 500;
              von_mises hack_noise (0, v);
    
              const double min_limit = this->get_min_limit (idx);
              const double max_limit = this->get_max_limit (idx);
              
              // Linear extrapolation
              double new_det_val;
              if (extrapolate)
                {
                  new_det_val = 2.0 * this->theta [idx] - this->prev_theta [idx];
                  new_det_val = force_in_range (new_det_val, min_limit, max_limit);
                }
              else
                {
                  new_det_val = this->theta [idx];
                }
              
              // Rejection sampling from a Von Mises distribution constrained to the
              // joint limits (XXX: should we have a max-limit on the number of iterations?)
              double new_rand_val;
              do
                {
                  new_rand_val = new_det_val + hack_noise.random ();
                }
              while (new_rand_val > max_limit || new_rand_val < min_limit);
        
              // Save stuff
              this->prev_theta [idx] = this->theta [idx];
              this->theta [idx] = new_rand_val;
            }
  
          // Predict root
          /*
          gaussian1D root_noise (0, 0.1);
          for (size_t idx = 0; idx < 3; idx++)
            root (idx) += root_noise.random ();
          */

          // Determine if the bone end-points are within the silhoute
          this->compute_pose ();
          bool accept = true;
          for (bone_iter iter = this->bone_begin (); iter != this->bone_end (); iter++)
            {
              const vector3 bone_pos = iter->absolute ().T ();
              const CvPoint bone_pos2d = this->calib.world_to_pixel (bone_pos, PROJ2D_SCALE);
              if (!proj2d.is_inside (bone_pos2d))
                {
                  accept = false;
                  break;
                }
            }
         
          if (accept)
            break;
          
          num_rejections ++;
        } // end: rejection sampling

      return 0;
    };
        
  angle_bgsub_state& operator= (angle_bgsub_state &new_state)
    {
      skeleton_state<observation_type>::operator= (new_state);

      for (size_t k = 0; k < this->prev_theta.size (); k++)
        this->prev_theta [k] = new_state.prev_theta [k];

      return *this;
    };

  bool load (const std::string filename)
    {
      const bool success = skeleton_state<observation_type>::load (filename);
  
      for (size_t k = 0; k < this->theta.size (); k++)
        this->prev_theta [k] = this->theta [k];
  
      return success;
    };
    
  bool load (std::ifstream &file)
    {
      const bool success = skeleton_state<observation_type>::load (file);
  
      for (size_t k = 0; k < this->theta.size (); k++)
        this->prev_theta [k] = this->theta [k];
  
      return success;
    };

  void write_to_log (std::ofstream &file) const
    {
      file << num_rejections << std::endl;
    }

protected:
  solver_type::vector_type prev_theta;
  const double pred_noise;
  const von_mises noise;
  const bool extrapolate;
  size_t num_rejections;
};

#endif // ANGLE_BGSUB_STATE_H