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

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

#include <OpenTissue/core/math/big/big_svd.h>
#include <OpenTissue/core/math/math_is_number.h>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <iostream>
template <class observation_type>
class brownian_stick_state : public angle_state<observation_type>
{
public:
  typedef boost::numeric::ublas::diagonal_matrix<double> diagonal_matrix_type;

  brownian_stick_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 ()),
      lambda (2)
    {
      for (unsigned int k = 0; k < lambda.size (); k++)
        lambda [k] = NaN;

      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);
            }
        }
      
      // Count number of end-effectors
      dim = 0;
      for (chain_iter iter = this->solver.chain_begin (); iter != this->solver.chain_end (); iter++)
        dim += 3;

    }

private:
  void jacobian (matrix_type &J)
    {
      for (chain_iter iter = this->solver.chain_begin (); iter != this->solver.chain_end (); iter++)
        iter->p_global () = iter->get_end_effector ()->absolute ().T ();

      compute_jacobian (this->solver.chain_begin (), this->solver.chain_end (),
                        this->bone_begin (), this->bone_end (), J);
    }
  
  void current_pos (vector_type &x)
    {
      int idx = 0;
      for (chain_iter iter = this->solver.chain_begin (); iter != this->solver.chain_end (); iter++)
        {
          vector3 local_x = iter->get_end_effector ()->absolute ().T ();
          for (int d = 0; d < 3; d++)
            x (idx++) = local_x (d);
        }
    }

  void project (const vector_type &x)
    {
      int idx = 0;
      for (chain_iter iter = this->solver.chain_begin (); iter != this->solver.chain_end (); iter++)
        {
          vector3 local_x;
          for (int d = 0; d < 3; d++)
            local_x (d) = x (idx++);
                
          iter->p_global () = local_x;
        }

      solver_type::Output output;
      this->solver.solve (&solver_type::default_SD_settings (), &output);
      OpenTissue::kinematics::inverse::get_joint_parameters (*(this->solver.skeleton ()),
                                                             this->theta);
      //std::cout << output.value () << " " << output.gradient_norm () << std::endl;
    }

public:
  // Implementation of the Euler-Heun scheme (works with Stratonovich models)
  double predict (const observation_type &observation, const double variance_scale = 1.0)
    {
      const int num_time_steps = 1;
      const double normalisation = sqrt ((double)num_time_steps);
    
      const stick_type stick = observation.get_stick ();

      const int num_indices = 2;
      const int indices [num_indices+1] = {9, 13, -1};

      matrix_type J; // 30x41
      ublas::matrix<double> U, V, UUT;
      vector_type S, W1 (dim), W2 (dim), incr1 (dim), incr2 (dim), x (dim), x_tmp (dim);
      diagonal_matrix_type Sd;
      
      gaussian1D g;
      // 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 (sqrt (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;
                        }
                    }
                }
            }
        }
      
      for (int time_step = 0; time_step < num_time_steps; time_step++)
        {
          // Simulate Euclidean Brownian motion
          for (int k = 0; k < dim; )
            {
              const double s = ((k >= 21) ? 3.0 * pred_noise :  pred_noise) / normalisation; 
              for (size_t n = 0; n < 3; n++,  k++)
                {
                  W1 (k) = s * g.random ();
                  W2 (k) = s * g.random ();
                }
            }

          // Get current position          
          this->compute_pose ();
          current_pos (x);

          // Compute P = U U^T at current position
          jacobian (J);
          OpenTissue::math::big::svd (J, U, S, V);
          if (Sd.size1 () == 0)
            Sd.resize (S.size (), S.size ());
          Sd.clear ();
          for (size_t i = 0; i < S.size (); i++)
            Sd (i, i) = (S (i) > 1e-9) ? 1.0 : 0.0;
          U = prod (U, Sd);
          UUT = prod (U, trans (U));

          // Compute the ordinary Euler step
          incr1 = prod (UUT, W1);

          // Perform inner Euler step
          incr2 = prod (UUT, W2);
          x_tmp = x + incr2;
          project (x_tmp);
          jacobian (J);
          OpenTissue::math::big::svd (J, U, S, V);

          //Sd.resize (S.size (), S.size ());
          Sd.clear ();
          for (size_t i = 0; i < S.size (); i++)
            Sd (i, i) = (S (i) > 1e-9) ? 1.0 : 0.0;

          U = prod (U, Sd);
          UUT = prod (U, trans (U));
          incr2 = prod (UUT, W1);
          
          // Euler-Heun Step
          x = x + 0.5 * (incr1 + incr2);

          // Enforce stick interaction
          size_t i = 0;
          for (int k = 0; k < dim; k += 3)
            {
              if (k == 3*indices [i] && is_number (lambda [i]))
                {
                  const vector3 nu = nearest_point_on_stick (stick, lambda [i]);
                  i++;
                  for (size_t n = 0; n < 3; n++)
                    x (k+n) = nu (n);
                }
            }

          project (x);
        }
        
      return 0;
    }

  brownian_stick_state& operator= (brownian_stick_state &new_state)
    {
      angle_state<observation_type>::operator= (new_state);
      dim = new_state.dim;

      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;
    int dim;
    std::vector<double> lambda;
    static const double p_letgo = 0.05, T_E = 1.0, sigma = /*0.01*/ 0.0001;
};

#endif // BROWNIAN_STICK_STATE_H