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

#define USE_IMPORTANCE_SAMPLING
#define NDEBUG // Make uBLAS run less slow

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

#ifdef USE_IMPORTANCE_SAMPLING
#include <OpenTissue/core/math/big/big_cholesky.h>
#include <iostream>
#endif // USE_IMPORTANCE_SAMPLING

// Simple indices for end-effector names
#define HEAD       0
#define LEFT_HAND  1
#define RIGHT_HAND 2
#define LEFT_FOOT  3
#define RIGHT_FOOT 4

//#define PELVIS
#define LINEAR_MOTION

extern double convex_lambda;

typedef boost::numeric::ublas::triangular_matrix<double, boost::numeric::ublas::lower> triangular_matrix_type;

inline
vector3 fix (vector3 &curr)
{
  return curr;
}

inline
vector3 linear_extrapolate (vector3 &curr, vector3 &prev)
{
  return 2.0 * curr - prev;
}

inline
void add_noise (vector3 &v, gaussian1D &noise)
{
  for (int k = 0; k < 3; k++)
    v [k] += noise.random ();
}

template <class observation_type>
class pik_state : public angle_state<observation_type>
{
public:
  pik_state (const options &opts, const calibration &_calib, const bool _for_show = true)
    : angle_state<observation_type> (opts, _calib, _for_show)
    {
      // Set convex lambda
      convex_lambda = opts.convex_lambda ();
      
      // Add leafs as end-effectors
      for (const_bone_iter bone = this->skeleton.begin (); bone != this->skeleton.end (); bone++)
        {
          if (!bone->is_leaf ())
            continue;

          chain_type chain;
          chain.init (this->skeleton.root (), &(*bone));
          this->solver.add_chain (chain);

          #ifdef PELVIS
          chain_type chainp;
          chainp.init (this->skeleton.root (), &(*(bone->parent ())));
          this->solver.add_chain (chainp);
          #endif // PELVIS
        }

      // Add shoulders for pelvic exercise
      #ifdef PELVIS
      {
        chain_type chain1;
        chain1.init (this->skeleton.root (), this->skeleton.get_bone (7));
        this->solver.add_chain (chain1);

        chain_type chain2;
        chain2.init (this->skeleton.root (), this->skeleton.get_bone (8));
        this->solver.add_chain (chain2);

        chain_type chain3;
        chain3.init (this->skeleton.root (), this->skeleton.get_bone (12));
        this->solver.add_chain (chain3);
      }
      #endif // PELVIS

      // Compute number of end-effectors
      num_end_effectors = 0;
      for (const_chain_iter iter = this->solver.chain_begin (); iter != this->solver.chain_end (); iter++)
        num_end_effectors ++;
  
      prev_end_effectors.resize (num_end_effectors);
      curr_end_effectors.resize (num_end_effectors);
  
      // Get start end-effectors
      get_end_effectors (prev_end_effectors);
      get_end_effectors (curr_end_effectors);
    };

  double predict (const observation_type &observation, const double variance_scale = 1.0)
    {
      #ifdef PELVIS
      pelvic_lift ();
      #endif // PELVIS
  
      #ifdef LINEAR_MOTION
      linear_motion ();
      #endif // LINEAR_MOTION

      double log_r = 0; // Correction factor for importance sampling
  
      #ifdef USE_IMPORTANCE_SAMPLING
      // Compute the Hessian for the Laplace approximation
      //const double w = 1600.0; // Precision: W = w*I
      const double w = 10000.0; // Precision: W = w*I
      matrix_type J; // dim: 9x47 (for upper body model)
      compute_jacobian (this->solver.chain_begin (), this->solver.chain_end (),
                        this->bone_begin (), this->bone_end (), J);
  
      matrix_type H = w * (1.0 - convex_lambda) * prod (trans (J), J);
      const size_t N = H.size1 ();
      for (size_t k = 0; k < N; k++)
        H (k, k) += w * convex_lambda;
    
      // Perform Cholesky for Gaussian sampling
      triangular_matrix_type Lh (N, N);
      const size_t s = OpenTissue::math::big::cholesky_decompose (H, Lh);
      if (s == 0) // Cholesky: success
        {
          // Generate a random sample from a standard normal distribution
          gaussian1D g;
          solver_type::vector_type x (N);
    
          /*
          // Sampling without respecting joint limits
          solver_type::vector_type y (N);
          double norm_y = 0;
          for (size_t k = 0; k < N; k++)
            {
              y (k) = g.random ();
              norm_y += square (y (k));
            }
    
          // Transform normal data
          x = solve (trans (Lh), y, ublas::upper_tag ());
    
          // Get new prediction
          for (size_t k = 0; k < N; k++)
            {
              // New angle value (stored in 'x' temporarely)
              y (k) = this->theta (k) + x (k);
            }
      
          // Copy new sample back to 'theta)
          for (size_t k = 0; k < N; k++)
            this->theta (k) = y (k);
          */    
    
          const int max_rejects = 3;
          double norm_y = 0;
          for (int k = N-1; k >= 0; k--)
            {
              bool reject = false;
              int reject_count = 0;
              double yk, new_theta_k;
              do
                {
                  if (reject_count < max_rejects)
                    yk = g.random ();
                  else
                    yk = 0; // XXX: Do something more clever!
              
                  // Solve k'th part of the linear sample system
                  double tmp_y = yk;
                  for (size_t l = k+1; l < N; l++)
                    tmp_y -= Lh (l, k) * x (l);
                  x (k) = tmp_y / Lh (k, k);
                  
                  // Test if we should reject the sample
                  new_theta_k = this->theta (k) + x (k);
                  if (new_theta_k < this->min_limits (k) || new_theta_k > this->max_limits (k))
                    {
                    reject = true;
                    reject_count ++;
                    //std::cerr << k << " " << new_theta_k << " " << min_limits (k)
                    //          << " " << max_limits (k) << " " << yk << " " << x (k)
                    //          << std::endl;
                    }
                  else
                    reject = false;
                }
              while (reject && reject_count <= max_rejects);
              
              norm_y += square (yk);
              this->theta (k) = new_theta_k;
            }
    
          // Compute correction factor
          //get_end_effectors (prev_end_effectors);
          /*
          double det_sigma = 0;
          for (size_t k = 0; k < N; k++)
            det_sigma += Lh (k, k);
          det_sigma *= 2.0;
          
          double d_w = 0;
          for (size_t k = 0; k < prev_end_effectors.size (); k++)
            for (size_t l = 0; l < 3; l++)
              d_w += square (curr_end_effectors [k][l] - prev_end_effectors [k][l]);
          d_w *= w;
    
          log_r = -0.5 * (log (det_sigma) + d_w + norm_y);
          */
        }
      else // Cholesky: failure
        {
          // This sitution really shouldn't occur
          std::cerr << "pik_state::predict: could not perform Cholesky transform" << std::endl;
        }
      #endif // USE_IMPORTANCE_SAMPLING
      
      // *
      // Save the current positions for next prediction
      for (size_t k = 0; k < num_end_effectors; k++)
        prev_end_effectors [k] = curr_end_effectors [k];
      // * / 
    /*
      #ifndef USE_IMPORTANCE_SAMPLING
      get_end_effectors (prev_end_effectors);
      #endif
      for (size_t k = 0; k < curr_end_effectors.size (); k++)
        curr_end_effectors [k] = prev_end_effectors [k];
    */
      return log_r;
    };
  
  void pelvic_lift ()
    {
      // Predict root
      const matrix3 R = OpenTissue::math::Rx (-3.14159 / 8.0) * OpenTissue::math::Rz (0.25);
      gaussian1D root_noise (0, 0.1);
      vector3 tmp;
      do
        {
          tmp (0) = 0.1 * root_noise.random ();
          tmp (1) = 1.0 * root_noise.random ();
          tmp (2) = 7.5 * root_noise.random ();
          tmp = R * tmp;
        }
      while (tmp (2) + this->root (2) < 0.0);
      this->root += tmp;
  
      set_end_effectors (prev_end_effectors); // IK mode-seeking
    };
    
  void linear_motion ()
    {
      get_end_effectors (curr_end_effectors);

      // Extrapolate end-effectors, i.e. sample from 'p (g_t | g_{1:t-1})'
      // Note: we use 'prev_end_effectors' for storing the new positions
      //gaussian1D gauss (0, 0.35);
      gaussian1D gauss (0, this->pred_noise);
      for (size_t k = 0; k < num_end_effectors; k++)
        {
          if (k == 0 || k > 2) // head and feet
            prev_end_effectors [k] = curr_end_effectors [k];
          else // hands
            prev_end_effectors [k] = 2.0 * curr_end_effectors [k] - prev_end_effectors [k];
          
          if (k <= 2) // head and hands
            for (size_t n = 0; n < prev_end_effectors [k].size (); n++)
              prev_end_effectors [k] [n] += gauss.random ();
        }
    
      set_end_effectors (prev_end_effectors); // IK mode-seeking
    };
  
  pik_state& operator= (pik_state &new_state)
    {
      angle_state<observation_type>::operator= (new_state);

      // Copy previous end-effector positions
      for (size_t k = 0; k < prev_end_effectors.size (); k++)
        prev_end_effectors [k] = new_state.prev_end_effectors [k];

      return *this;
    };
    
  bool load (const std::string filename)
    {
      const bool success = angle_state<observation_type>::load (filename);
  
      get_end_effectors (prev_end_effectors);
  
      return success;
    };
    
  bool load (std::ifstream &file)
    {
      const bool success = angle_state<observation_type>::load (file);
  
      get_end_effectors (prev_end_effectors);
  
      return success;
    };

protected:
  size_t num_end_effectors;
  std::vector<vector3> prev_end_effectors, curr_end_effectors;
  
  void set_end_effectors (const std::vector<vector3> &data)
    {
      this->compute_pose ();
  
      int i = 0;
      for (chain_iter iter = this->solver.chain_begin (); iter != this->solver.chain_end (); iter++)
        iter->p_global () = data [i++] - this->root;
      this->solver.solve (&solver_type::default_SD_settings ());
    
      //solver.skeleton ()->compute_pose (); // XXX: Is this needed?
      OpenTissue::kinematics::inverse::get_joint_parameters (*(this->solver.skeleton ()), this->theta);
      
      // In case we didn't reach the goal we store what we actually reached
      i = 0;
      for (chain_iter iter = this->solver.chain_begin (); iter != this->solver.chain_end (); iter++)
        //iter->p_global () = curr_end_effectors [i++];
        iter->p_global () = iter->get_end_effector ()->absolute ().T ();
    };
    
  void get_end_effectors (std::vector<vector3> &data)
    {
      this->compute_pose (); // XXX: is this needed?
  
      int i = 0;
      for (const_chain_iter iter = this->solver.chain_begin (); iter != this->solver.chain_end (); iter++)
        data [i++] = iter->get_end_effector ()->absolute ().T () + this->root;
        //data [i++] = iter->p_global (); // XXX: WHY THE HELL DOESN'T THIS WORK??
    };
};

#endif // PIK_STATE_H