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

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

#include <OpenTissue/core/math/big/big_moore_penrose_pseudoinverse.h>
#include <iostream>

template <class observation_type>
class tangent_state : public angle_state<observation_type>
{
public:
  tangent_state (const options &opts, const calibration &_calib, const bool _for_show = true)
    : angle_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 ()) // || i == 3 || i == 4 || i == 7 || i == 11)
            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 double std [] = {0.019905, 0.003157, 0.008050, 0.180706, 0.101031, 0.137931,
                             0.275119, 0.182599, 0.286416, 0.275119, 0.182599, 0.286416,
                             0.390496, 0.301062, 0.483356, 0.508283, 0.808703, 0.806479,
                             0.275119, 0.182599, 0.286416, 0.381652, 0.255074, 0.218130,
                             0.498532, 0.249264, 0.615678, 0.938598, 0.593349, 1.098928,
                             0.275119, 0.182599, 0.286416, 0.509799, 0.464794, 0.370578,
                             0.988121, 0.796506, 0.450776, 1.155469, 1.320700, 0.709060};
      
      this->compute_pose ();
      for (chain_iter iter = this->solver.chain_begin (); iter != this->solver.chain_end (); iter++)
        iter->p_global () = iter->get_end_effector ()->absolute ().T ();

      matrix_type J; // 30x41
      compute_jacobian (this->solver.chain_begin (), this->solver.chain_end (),
                        this->bone_begin (), this->bone_end (), J);
  
      matrix_type Jpinv;
      OpenTissue::math::big::svd_moore_penrose_pseudoinverse (J, Jpinv);

      gaussian1D g; // (0, 0.05);
      matrix_type x (Jpinv.size2 (), 1);

      for (size_t k = 0; k < J.size1 (); k++)
        x (k, 0) = 0.25 * std [k] * g.random ();
  
      matrix_type T = prod (Jpinv, x);
      for (size_t k = 0; k < T.size1 (); k++)
        this->theta (k) += T (k, 0);
  
      skeleton_state<observation_type>::project_limits ();

      return 0;
    };
  
  tangent_state& operator= (tangent_state &new_state)
    {
      angle_state<observation_type>::operator= (new_state);

      return *this;
    };
    
  bool load (const std::string filename)
    {
      return angle_state<observation_type>::load (filename);
    };

  bool load (std::ifstream &file)
    {
      return angle_state<observation_type>::load (file);
    };

  private:
    const double pred_noise;
};

#endif // TANGENT_STATE_H