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

#include "angle_state.h"
#include "options.h"
#include "simulator.h"
#include "auxil.h"
#include "math_types.h"
#include "cloud_and_stick.h"
#include <OpenTissue/core/math/math_is_number.h>

template <class observation_type>
class stick2d_state : public skeleton_state<observation_type>
{
public:
  stick2d_state (const options &opts, const calibration &_calib, const bool _for_show = true)
    : skeleton_state<observation_type> (opts, _calib, _for_show),
      lambda (2),
      pred_noise (opts.pred_noise ())
    {
      for (unsigned int k = 0; k < lambda.size (); k++)
        lambda [k] = NaN;

      this->compute_pose ();
  
      // Add everything as end-effectors
      int i = 0;
      for (bone_iter iter = this->bone_begin (); iter != this->bone_end (); iter++, i++)
        {
          if (iter->is_root ())
            continue;
          
          chain_type chain;
          chain.init (this->skeleton.root (), &(*iter));
          this->solver.add_chain (chain);
        }
    }

  double predict (const observation_type &observation, const double variance_scale = 1.0)
    {
      this->compute_pose ();
      const stick_type stick = observation.get_stick ();
      const vector3 origin = this->calib.camera_centre ();
      
      const int num_indices = 2;
      const int indices [num_indices+1] = {9, 13, -1};
      
      gaussian1D gauss;
      
      // Predict lambdas
      for (unsigned int i = 0; i < lambda.size (); i++)
        {
          if (!is_number (lambda [i]))
            {
              // Get current hand position
              const_chain_iter iter = this->solver.chain_begin ();
              int k = 0;
              while (k++ != indices [i])
                iter ++;
              const vector3 hand = iter->get_end_effector ()->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.0 <= l && l <= 1.0)
                        {
                          lambda [i] = l;
                          break;
                        }
                    }
                }
            }
        }
      
      // Predict root
      //root (0) += 0.05 * gauss.random ();
      //root (1) += 0.0001 * gauss.random ();
      //root (2) += 0.05 * gauss.random ();
    
      this->compute_pose ();

      vector3 nu;
      int k = 0, i = 0;
      for (chain_iter iter = this->solver.chain_begin ();
           iter != this->solver.chain_end (); iter++, k++)
        {
          const vector3 curr = iter->get_end_effector ()->absolute ().T ();
          if (k == indices [i] && is_number (lambda [i]))
            {
              const vector3 line = nearest_point_on_stick (stick, lambda [i]);
              nu = project_point_on_stick_line (curr, line, origin);
              i++;
            }
          else
            {
              const double s = (k >= 7) ? 3.0 * pred_noise :  pred_noise;
              for (size_t n = 0; n < 3; n++)
                nu [n] = curr [n] + s * gauss.random ();
            }
          iter->p_global () = nu; // - root;
        }

/*      int k = 0, i = 0;
      for (chain_iter iter = this->solver.chain_begin ();
           iter != this->solver.chain_end (); iter++, k++)
        {
          const vector3 curr = iter->get_end_effector ()->absolute ().T ();
          if (k == indices [i])
            {
              nu = project_point_on_stick_plane (curr, v3, origin);
              i++;
            }
          else
            {
              const double s = (k >= 7) ? 3.0 * pred_noise :  pred_noise;
              for (size_t n = 0; n < 3; n++)
                nu [n] = curr [n] +  s * gauss.random ();
            }
          iter->p_global () = nu; // - root;
        }
*/
    
      this->solver.solve (&solver_type::default_SD_settings ());
      OpenTissue::kinematics::inverse::get_joint_parameters (*(this->solver.skeleton ()),
                                                             this->theta);
    
      return 0;
    }

  stick2d_state& operator= (stick2d_state &new_state)
    {
      skeleton_state<observation_type>::operator= (new_state);
      
      return *this;
    }
    
  bool load (const std::string filename)
    {
      return skeleton_state<observation_type>::load (filename);
    }
    
  bool load (std::ifstream &file)
    {
      return skeleton_state<observation_type>::load (file);
    }

protected:
  std::vector<double> lambda;
  const double pred_noise;
  // XXX: Get these parameters from 'opts'
  static const double p_letgo = 0.01, T_E = 0.2, sigma = /*0.01*/ 0.0001;
  
  vector3 project_point_on_stick_line (const vector3 &p, const vector3 &line,
                                       const vector3 &origin) const
    {
      const vector3 lineo = unit (line - origin);
      const vector3 po = p - origin;
      const vector3 proj = lineo * (lineo * po);
      
      return proj + origin;
    }
};

#endif // STICK2D_STATE_H