/home/hauberg/Dokumenter/Capture/humim-tracker-0.1/src/pik_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 PIK_STICK_STATE_H
#define PIK_STICK_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"
#include "cloud_and_stick.h"
#include <OpenTissue/core/math/math_is_number.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

extern double convex_lambda;

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

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

          if (first_leaf)
            {
              first_leaf = false;
            }
          else
            {
              chain_type chain;
              chain.init (this->skeleton.root (), &(*bone));
              this->solver.add_chain (chain);
            }
        }

      // 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 ++;
  
      // Get start end-effectors
      end_effectors.resize (num_end_effectors);
      get_end_effectors (end_effectors);

      // Stick interactions (initialise lambda's)
      for (unsigned int k = 0; k < lambda.size (); k++)
        lambda [k] = NaN;
    };

  double predict (const observation_type &observation, const double variance_scale = 1.0)
    {
      stick_motion (observation);

      double log_r = 0; // Correction factor for importance sampling
  
      #ifdef USE_IMPORTANCE_SAMPLING
      // Compute the Hessian for the Laplace approximation
      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);
    
          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;
            }
        }
      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
      
      return log_r;
    };
  
  pik_stick_state& operator= (pik_stick_state &new_state)
    {
      angle_state<observation_type>::operator= (new_state);
      return *this;
    };
    
  bool load (const std::string filename)
    {
      const bool success = angle_state<observation_type>::load (filename);  
      return success;
    };
    
  bool load (std::ifstream &file)
    {
      const bool success = angle_state<observation_type>::load (file);
  
      return success;
    };

protected:
  size_t num_end_effectors;
  std::vector<double> lambda;
  static const double sigma = 0.0001;
  std::vector<vector3> end_effectors;
  
  void stick_motion (const observation_type &observation)
    {
      const stick_type stick = observation.get_stick ();

      gaussian1D gauss;
    
      // Predict lambdas
      for (unsigned int k = 0; k < lambda.size (); k++)
        {
          if (!is_number (lambda [k]))
            {
              const_chain_iter iter = this->solver.chain_begin ();
              for (unsigned int i = 0; i < k; i++)
                iter ++;
              const vector3 hand = iter->get_end_effector ()->absolute ().T ();
              dist2 (stick, hand, lambda [k]);
            }
          else
            {
              gaussian1D G (lambda [k], sigma);
              while (true)
                {
                  const double l = G.random ();
                  if (0.0 <= l && l <= 1.0)
                    {
                      lambda [k] = l;
                      break;
                    }
                }
            }
        }
      
      for (unsigned int k = 0; k < end_effectors.size (); k++)
        end_effectors [k] = nearest_point_on_stick (stick, lambda [k]);
      set_end_effectors (end_effectors);

      /*
      unsigned int k = 0;
      for (chain_iter iter = this->solver.chain_begin ();
           iter != this->solver.chain_end (); iter++, k++)
        {
          const vector3 nu = nearest_point_on_stick (stick, lambda [k]);
          iter->p_global () = nu;
        }
      */
    }
    
  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 () = 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_STICK_STATE_H