/home/hauberg/Dokumenter/Capture/humim-tracker-0.1/src/KBK_stick_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 KBK_STICK_STATE_H
#define KBK_STICK_STATE_H

#include "angle_state.h"
#include "options.h"
#include "simulator.h"
#include "cloud_and_stick.h"

#include <vector>
#include <limits>
#include <OpenTissue/core/math/math_is_number.h>

#define NaN std::numeric_limits<double>::quiet_NaN ()

template <class observation_type>
class KBK_stick_state : public angle_state<observation_type>
{
public:
  KBK_stick_state (const options &opts, const calibration &_calib, const bool _for_show = true)
    : angle_state<observation_type> (opts, _calib, _for_show),
      lambda (2),
      p_letgo (0.01), T_E (1), sigma (0.05) // XXX: Get these parameters from 'opts'
    {
      for (unsigned int k = 0; k < lambda.size (); k++)
        lambda [k] = NaN;
      
      //const std::string filename = fullfile (opts.output_directory (), "num_rejections.txt");
      //num_rejections_file = fopen (filename.c_str (), "w");
    };
  
  ~KBK_stick_state ()
    {
      //fclose (num_rejections_file);
      //num_rejections_file.close ();
    };

  double predict (const observation_type &observation, const double variance_scale = 1.0)
    {
      this->compute_pose ();
      const stick_type stick = observation.get_stick ();

      const int num_indices = 2;
      const int indices [num_indices+1] = {10, 14, -1};
      
      // Predict lambdas
      for (unsigned int i = 0; i < lambda.size (); i++)
        {
          if (!is_number (lambda [i]))
            {
              // Get current hand position
              const_bone_iter iter = this->skeleton.begin ();
              int k = 0;
              while (k++ != indices [i])
                iter ++;
              const vector3 hand = iter->absolute ().T ();

              // Compute distance between stick and hand
              double alpha;
              const double hand_object_dist2 = dist2 (stick, hand, alpha);
              if (hand_object_dist2 < T_E)
                lambda [i] = alpha;
              else
                lambda [i] = NaN;
            }
          else
            {
              /*
              uniform U;
              if (i > 0 && U.random () < p_letgo) // i > 0 means we only allow left hand to let go
                {
                  lambda [i] = NaN;
                }
              else
              */
                {
                  gaussian1D G (lambda [i], sigma);
                  while (true)
                    {
                      const double l = G.random ();
                      if (0 <= l && l <= 1)
                        {
                          lambda [i] = l;
                          break;
                        }
                    }
                }
            }
        }
      
      // Predict thetas
      num_rejections = 0;
      // This rejection-sampling loop is inherently slow. To make things worse
      // it causes thread-divergence, which is the main cause of the ridiculous
      // slowness of the approach. Basically, the slowest particle sets the
      // speed limit...
      //while (true)
      const vector_type init_theta (this->get_theta ());
      for (int iter = 0; iter < 5000; iter++)
        {
          // Ordinary prediction
          this->set_theta (init_theta);
          angle_state<observation_type>::predict (observation);
          
          // Rejection sampling
          this->compute_pose ();
          int k = 0, i = 0;
          bool accept = true;
          for (const_bone_iter iter = this->skeleton.begin ();
               iter != this->skeleton.end (); iter++, k++)
            {
              if (k == indices [i] && is_number (lambda [i]))
                {
                  const vector3 attained = iter->absolute ().T ();
                  const vector3 guess = nearest_point_on_stick (stick, lambda [i]);
                  const vector3 delta = attained - guess;
                  const double dist2 = delta * delta;
                  if (dist2 > T_E)
                    {
                      accept = false;
                      break;
                    }
                  i++;
                }
            }

          if (accept)
            {
              break;
            }
      
          num_rejections ++;
        }

      return 0;
    };

  KBK_stick_state& operator= (KBK_stick_state &new_state)
    {
      skeleton_state<observation_type>::operator= (new_state);
      this->lambda = new_state.lambda;
      
      return *this;
    };
  
  void write_to_log (std::ofstream &file) const
    {
      file << num_rejections << std::endl;
    }
        
protected:
  int num_rejections;
  std::vector<double> lambda;
  const double p_letgo, T_E, sigma;
};

#endif // KBK_STICK_STATE_H