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

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

template<class observation_type>
class bgsub_state : public skeleton_state<observation_type>
{
public:
  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 ())
    {
      this->compute_pose ();
  
      // Store default chains temporarely
      std::list<vector3> chain_list;
      for (chain_iter iter = this->solver.chain_begin (); iter != this->solver.chain_end (); iter++)
        chain_list.push_back (iter->get_end_effector ()->absolute ().T ());
  
      // 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;
        
          const vector3 absolute = iter->absolute ().T ();
          const vector3 pabsolute = iter->parent ()->absolute ().T ();

          // Make sure the bone isn't already an end-effector
          bool accept = true; //(absolute != pabsolute);
          for (std::list<vector3>::const_iterator ci = chain_list.begin ();
               ci != chain_list.end (); ci ++)
            {
              if (*ci == absolute)
                {
                  accept = false;
                  break;
                }
            }
      
          if (accept)
            {
              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)
    {
      const size_t max_rejects = 10;
      
      this->compute_pose ();

      gaussian1D gauss;
      const project2d &proj2d = observation.get_project2d ();
      const vector3 origin = this->calib.camera_centre ();

      int k = 0;
      for (chain_iter iter = this->solver.chain_begin (); iter != this->solver.chain_end (); iter++)
        {
          vector3 nu, nu_valid;
          CvPoint nu2d;
    
          // Get noise parameters for limb
          const double s = (k >= 7) ? 3.0 * pred_noise :  pred_noise;
          k++;

          // Rejection Sampling of points in embedding space
          bool inside = false;
          for (size_t trial = 0; trial < max_rejects; trial++)
            {
              // Generate a random 3-vector
              for (size_t n = 0; n < 3; n++)
                nu [n] = s * gauss.random ();
              nu = iter->get_end_effector ()->absolute ().T () + nu; // - this->root;
          
              // Ensure that the prediction point is in tune with the observation
              nu2d = this->calib.world_to_pixel (nu, PROJ2D_SCALE);
              if (proj2d.is_inside (nu2d))
                {
                  nu_valid = nu;
                  inside = true;
                  break;
                }
            }
          
          // If the rejection sampling kept going for too long we project the point onto the segment
          if (!inside)
            {   
              const CvPoint goal2d = proj2d.project (nu2d);
              const vector3 goal_line = this->calib.pixel_to_world (goal2d, PROJ2D_SCALE);
              nu_valid = project_point_on_line (nu, origin, goal_line);
            }
          
          // Set end-effector goal
          iter->p_global () = nu_valid;
        }

      this->solver.solve (&solver_type::default_SD_settings ());
      OpenTissue::kinematics::inverse::get_joint_parameters (*(this->solver.skeleton ()),
                                                             this->theta);

      return 0;
    };
  
  bgsub_state& operator= (bgsub_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);
    };
  private:
    const double pred_noise;
};

#endif // BGSUB_STATE_H