/home/hauberg/Dokumenter/Capture/humim-tracker-0.1/src/angle_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_STATE_H
#define ANGLE_STATE_H

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

template <class observation_type>
class angle_state : public skeleton_state<observation_type>
{
public:
  angle_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)
    {
      #define HANDS2_NORMAL

      const size_t theta_size = this->theta.size ();

      #ifdef HANDS2_NORMAL
      const double std [] = {0.118043, 0.106630, 0.187879, 0.110447, 0.115539, 0.154490,
                             0.207707, 0.247319, 0.131030, 0.162429, 0.278345, 0.232262,
                             0.129147, 0.201286, 0.169725, 0.257265, 0.350806, 0.163058,
                             0.204395, 0.222967, 0.194014, 0.121656, 0.155806, 0.199396,
                             0.537728, 0.354365, 0.560878, 0.381538, 0.234784, 0.239393,
                             0.179768, 0.161243, 0.181075, 0.184029, 0.904360, 0.797576,
                             0.525455, 0.590451, 0.201901, 0.143306, 0.169167};

      gaussian1D gauss;
      for (size_t idx = 0; idx < theta_size; idx++)
        {
          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
          double new_rand_val;
          const size_t max_iter = 20;
          size_t iter = 1;
          do
            {
              new_rand_val = new_det_val + 0.5 * std [idx] * gauss.random ();
              //std::cerr << idx << " " << std [idx] << " " << new_det_val << " " << new_rand_val << " " << max_limit << " " << min_limit << std::endl;
              
              if (iter++ >= max_iter)
                {
                  new_rand_val = new_det_val;
                  break;
                }
            }
          while (new_rand_val > max_limit || new_rand_val < min_limit);
    
          // Save stuff
          this->theta [idx] = new_rand_val;
        }
    
      #endif // HANDS2_NORMAL
      
      #ifndef HANDS2_NORMAL
      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;
        }
      #endif // !HANDS2_NORMAL
  
      // Predict root
      /*
      gaussian1D root_noise (0, 0.1);
      for (size_t idx = 0; idx < 3; idx++)
        root (idx) += root_noise.random ();
      */
      
      //skeleton_state<observation_type>::compute_pose (); // XXX: Do we need this?

      return 0;
    };
        
  angle_state& operator= (angle_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;
    };

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

#endif // ANGLE_STATE_H