/home/hauberg/Dokumenter/Capture/humim-tracker-0.1/src/OpenTissue/OpenTissue/kinematics/inverse/inverse_nonlinear_solver.h

Go to the documentation of this file.

#ifndef OPENTISSUE_KINEMATICS_INVERSE_INVERSE_NONLINEAR_SOLVER_H
#define OPENTISSUE_KINEMATICS_INVERSE_INVERSE_NONLINEAR_SOLVER_H
//
// OpenTissue Template Library
// - A generic toolbox for physics-based modeling and simulation.
// Copyright (C) 2008 Department of Computer Science, University of Copenhagen.
//
// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/kinematics/inverse/inverse_chain.h> // The basic data structure

// Utility functions for extracting data
#include <OpenTissue/kinematics/inverse/inverse_compute_jacobian.h>
#include <OpenTissue/kinematics/inverse/inverse_set_joint_parameters.h>
#include <OpenTissue/kinematics/inverse/inverse_get_joint_parameters.h>
#include <OpenTissue/kinematics/inverse/inverse_compute_joint_limits_projection.h>
#include <OpenTissue/kinematics/inverse/inverse_compute_weighted_difference.h>

// Numerical methods
#include <OpenTissue/core/math/optimization/optimization_projected_bfgs.h>
#include <OpenTissue/core/math/optimization/optimization_projected_steepest_descent.h>

#include <OpenTissue/core/math/big/big_prod_trans.h> // Needed for prod_trans

#include <OpenTissue/utility/utility_timer.h>

#include <OpenTissue/core/geometry/geometry_compute_closest_points_line_line.h>

#include <list>
#include <cassert>
#include <algorithm>

namespace OpenTissue
{
  namespace kinematics
  {
    namespace inverse
    {
      namespace detail
      {

        template< typename solver_type >
        class FunctionCalculator
        {
        public:

          typedef typename solver_type::vector_type             vector_type;
          typedef typename vector_type::value_type              real_type;
          typedef typename solver_type::skeleton_type           skeleton_type;
          typedef typename skeleton_type::bone_iterator         bone_iterator;
          typedef typename skeleton_type::const_bone_iterator   const_bone_iterator;

        protected:

          solver_type * m_S;       
          vector_type   m_delta;   

        public:

          FunctionCalculator()
            : m_S(0)
          {}
          
          /*
          inline
          real_type dist (const vector3 &a, const vector3 &b) const
          {
            const vector3 delta = a - b;
            return sqrt (dot (delta, delta));
          }

          bool bone_intersects_bone (const const_bone_iterator &bone1, const const_bone_iterator &bone2) const
          {
            vector3 a1 = bone1->absolute ().T ();
            vector3 b1 = bone1->parent ()->absolute ().T ();
            vector3 a2 = bone2->absolute ().T ();
            vector3 b2 = bone2->parent ()->absolute ().T ();

            // Test if the bones are connected
            if (dist (a1, b2) < 0.0001)
              return false;
            
            const double length1 = bone1->get_length ();
            const double radius1 = std::min (bone1->get_width (), length1 / 2.0);
            const vector3 u1 = unit (a1 - b1) / length1;
            a1 -= radius1 * u1;
            b1 += radius1 * u1;

            const double length2 = bone2->get_length ();
            const double radius2 = std::min (bone2->get_width (), length2 / 2.0);
            const vector3 u2 = unit (a2 - b2) / length2;
            a2 -= radius2 * u2;
            b2 += radius2 * u2;

            real_type alpha1, alpha2;
            geometry::compute_closest_points_line_line (b1, u1, b2, u2, alpha1, alpha2);
            alpha1 = math::clamp (alpha1, 0.0, length1 - radius1);
            alpha2 = math::clamp (alpha2, 0.0, length2 - radius2);

            const vector3 p1 = b1 + alpha1 * u1;
            const vector3 p2 = b2 + alpha2 * u2;
            const real_type capsule_dist = dist (p1, p2);
            
            if (capsule_dist < (radius1 + radius2))
            {
            std::cerr
            //   << capsule_dist << " " << bone1->get_width () << " " << bone2->get_width ()
            //   << " " << bone1->get_length () << " " << bone2->get_length ()
               << " " << bone1->get_number () << " " << bone2->get_number ()
            //   << " " << radius1 << " " << radius2 << " " << radius1+radius2
            //   << " " << p1 << " " << p2
            //   << " " << a1 << " " << b1 << " " << a2 << " " << b2
            //   << " " << alpha1 << " " << alpha2 << " " << length1 - radius1 << " " << length2 - radius2
               << std::endl;
              return true;
            }
            else
              return false;
          }
          
          int number_of_self_intersections (skeleton_type *skeleton) const
          {
            const_bone_iterator bone_begin = skeleton->begin ();
            bone_begin++; // skip 'root'
            const const_bone_iterator bone_end   = skeleton->end ();
            
            int ns = 0;
            for (const_bone_iterator iter1 = bone_begin ; iter1 != bone_end; iter1++)
            {
              if (iter1->get_intersectable ())
                continue;
            
              const_bone_iterator iter2 = iter1;
              iter2++; // skip iter1 to avoid comparing with yourself
              iter2++; // skip to avoid comparing neighbours
              //if (iter2->get_number() == iter1->parent ()->get_number())
              //  { iter2++; std:: cerr << " ######## " << std::endl; }
              for (; iter2 != bone_end; iter2++)
                if (iter2->get_intersectable () && iter2->parent ()->get_intersectable () && this->bone_intersects_bone (iter1, iter2))
                  ns ++;
            }
            
            return ns;
          }
          */
          
          void init(solver_type* S) {  m_S = S; }

          real_type operator ()(vector_type const & theta) const
          {
            using ublas::inner_prod;
            real_type retval;
            
            OpenTissue::kinematics::inverse::set_joint_parameters( *(this->m_S->skeleton()), theta );
            vector_type & delta = const_cast<vector_type&>( this->m_delta );
            OpenTissue::kinematics::inverse::compute_weighted_difference( this->m_S->chain_begin(), this->m_S->chain_end(), delta );
            retval = inner_prod(delta,delta);

            //const real_type ns = this->number_of_self_intersections( this->m_S->skeleton() );
            //retval *= (1.0 + ns);
            //if (ns > 0)
            //  std::cerr << ns << " " << retval << std::endl;
            
            return retval;
          }
        };

        template<typename solver_type >
        class GradientCalculator
        {
        public:

          typedef typename solver_type::math_types    math_types; 
          typedef typename solver_type::vector_type   vector_type; 
          typedef typename solver_type::matrix_type   matrix_type; 
          typedef typename math_types::value_traits   value_traits; 

        protected:

          solver_type * m_S;       
          vector_type   m_delta;   
          matrix_type   m_J;       
          vector_type   m_tmp;     

        public:

          GradientCalculator()
            : m_S(0)
          {}

          void init(solver_type* S ) { m_S = S; }

          vector_type operator ()(vector_type const& theta) const
          {
            typedef typename solver_type::chain_iterator                 chain_iterator;
            typedef typename chain_iterator::value_type                  chain_type;
            typedef typename chain_type::bone_traits                     bone_traits;

            using ublas::prod;
            using ublas::trans;

            OpenTissue::kinematics::inverse::set_joint_parameters( *(this->m_S->skeleton()), theta );

            vector_type & delta = const_cast<vector_type&>( this->m_delta );
            OpenTissue::kinematics::inverse::compute_weighted_difference( this->m_S->chain_begin(), this->m_S->chain_end(), delta);

            /*
            matrix_type & J = const_cast<matrix_type&>( this->m_J );
            OpenTissue::kinematics::inverse::compute_jacobian( this->m_S->chain_begin(), this->m_S->chain_end(), this->m_S->skeleton()->begin(), this->m_S->skeleton()->end(), J  );

            vector_type & tmp = const_cast<vector_type&>( this->m_tmp );
            if(tmp.size() != J.size2())
              tmp.resize( J.size2() );
            OpenTissue::math::big::prod_trans( J, delta, tmp ); 
            tmp *= -value_traits::two();
            */
            
            // BEGIN: playtime
            const chain_iterator chain_begin = this->m_S->chain_begin();
            const chain_iterator chain_end = this->m_S->chain_end();

            // Determine how many chains we have
            const size_t max_chains = std::distance( this->m_S->chain_begin(), this->m_S->chain_end() );
            const size_t max_bones  = std::distance( this->m_S->skeleton()->begin(), this->m_S->skeleton()->end() );

            // Determine how many rows and columns we have in the Jacobian
            std::vector<size_t> row_index(max_chains);      
            std::vector<size_t> column_index(max_bones);    

            size_t rows    = 0;              
            size_t columns = 0;              

            std::vector<size_t> dof_array(max_bones);   
            {
              size_t index = 0;
              for(chain_iterator chain = chain_begin;chain!=chain_end;++chain)
              {
                row_index[index++] = rows;
                rows += chain->get_goal_dimension();
              }

              typename solver_type::skeleton_type::bone_iterator bone     = this->m_S->skeleton()->begin();
              typename solver_type::skeleton_type::bone_iterator bone_end = this->m_S->skeleton()->end();
              for(;bone!=bone_end;++bone)
              {
                dof_array[ bone->get_number()] = bone->active_dofs();
              }

              for (size_t i=0;i<max_bones;++i)
              {
                column_index[i] = columns;
                columns += dof_array[i];
              }
            }

            vector_type tmp2 (columns, 0.0);
            matrix_type J1 (3, 1);
            matrix_type J3 (3, 3);
            size_t chain_index = 0;
            for(chain_iterator chain = chain_begin;chain!=chain_end;++chain)
            {
              const typename chain_type::bone_iterator bend = chain->bone_end();
              typename chain_type::bone_iterator bone = chain->bone_begin();

              //size_t theta_idx = 0;
              for(; bone != bend ; ++bone)
              {
                const size_t bone_index = bone->get_number();
                const size_t begin_row      = row_index[chain_index];
                const size_t begin_column   = column_index[bone_index];

                const size_t dofs = dof_array[bone_index];
                if (dofs == 1)
                {
                  bone_traits::compute_jacobian( (*bone) , (*chain), J1);
                  tmp2 (begin_column) += delta (begin_row)   * J1 (0, 0)
                                       + delta (begin_row+1) * J1 (1, 0)
                                       + delta (begin_row+2) * J1 (2, 0);
                }
                else if (dofs == 3)
                {
                  bone_traits::compute_jacobian( (*bone) , (*chain), J3);
                  for (size_t t = 0; t < dofs; t++)
                  {
                    tmp2 (begin_column+t) += delta (begin_row)   * J3 (0, t)
                                           + delta (begin_row+1) * J3 (1, t)
                                           + delta (begin_row+2) * J3 (2, t);
                  }
                }
                else
                {
                  std::cerr << "For helvede da!" << std::endl;
                }
              }
              chain_index ++;
            }
            tmp2 *= -value_traits::two();
            //const double diff = inner_prod (tmp-tmp2, tmp-tmp2);
            //std::cerr << diff << std::endl;
            // END: playtime
            
            return tmp2;
          }
        };

        template<typename solver_type >
        class ProjectionCalculator
        {
        public:

          typedef typename solver_type::math_types    math_types; 
          typedef typename solver_type::vector_type   vector_type; 
          typedef typename math_types::real_type      real_type; 
          typedef typename math_types::value_traits   value_traits; 

        protected:

          solver_type * m_S; 

        public:

          ProjectionCalculator()
            : m_S(0)
          {}

          void init(solver_type* S ){ this->m_S = S; }


          vector_type operator ()(vector_type const & theta) const
          {
            // 2008-07-06 kenny: Hmm is this efficient? Would this not create a un-needed temporary?
            vector_type ret = theta; 
            OpenTissue::kinematics::inverse::compute_joint_limits_projection(*(this->m_S->skeleton()), ret );
            return ret;
          }
        };

      } // namespace detail

      template<typename skeleton_type_ >
      class NonlinearSolver
      {
      public:

        typedef          skeleton_type_              skeleton_type;
        typedef typename skeleton_type::math_types   math_types;
        typedef typename skeleton_type::bone_traits  bone_traits;
        typedef          Chain<skeleton_type>            chain_type;

      protected:

        typedef typename math_types::real_type       real_type;
        typedef typename math_types::vector3_type    vector3_type;
        typedef typename math_types::value_traits    value_traits;
        typedef std::list<chain_type>                                chain_container;

      public:

        typedef typename chain_container::iterator                 chain_iterator;
        typedef typename chain_container::const_iterator             const_chain_iterator;
        typedef typename ublas::vector<real_type>                    vector_type;  
        typedef typename ublas::vector_range< vector_type >        vector_range;
        typedef typename ublas::vector_range< const vector_type >  const_vector_range;
        typedef typename ublas::compressed_matrix<real_type>         matrix_type;
        typedef typename ublas::identity_matrix<real_type>             identity_matrix_type;

      protected:

        typedef          NonlinearSolver<skeleton_type>              solver_type;
        typedef typename detail::GradientCalculator<solver_type>     gradient_type;
        typedef typename detail::FunctionCalculator<solver_type >    function_type;
        typedef typename detail::ProjectionCalculator<solver_type >  projection_type;

      protected:

        gradient_type     m_gradient;   
        function_type     m_function;   
        projection_type   m_projection; 
        chain_container   m_chains;     
        skeleton_type*    m_skeleton;   
        vector_type       m_theta;      
        matrix_type       m_H;          

      public:

        skeleton_type       * skeleton()       { return this->m_skeleton; }
        skeleton_type const * skeleton() const { return this->m_skeleton; }

        chain_iterator        chain_begin()       { return m_chains.begin(); }
        const_chain_iterator  chain_begin() const { return m_chains.begin(); }

        chain_iterator        chain_end()       { return m_chains.end(); }
        const_chain_iterator  chain_end() const { return m_chains.end(); }

        size_t size() const { return m_chains.size(); }

        /*
        * Add Chain.
        * This method adds a chain and returns an iterator for the newly created iterator.
        *
        * @param chain.   The chain that should be added to the solver.
        * @return         An iterator to the newly created chain.
        */
        chain_iterator add_chain(chain_type const & chain)
        {
          return m_chains.insert(m_chains.end(), chain);
        }

        chain_iterator remove_chain(chain_iterator & chain)
        {
          chain_iterator iter = this->m_chains.erase(chain);

          if( iter != this->m_chains.end() )
            return iter;
          return this->m_chains.begin();
        }

      public:

        NonlinearSolver()
          : m_gradient()
          , m_function()
          , m_projection()
          , m_skeleton(0)
        {
          this->m_gradient.init(this);
          this->m_function.init(this);
          this->m_projection.init(this);
        }

        NonlinearSolver(solver_type const & solver)  
          : m_gradient()
          , m_function()
          , m_projection()
        {
          *this = solver;
        }

        NonlinearSolver(skeleton_type & S)
        {
          this->init(S);
        }

        ~NonlinearSolver(){ }

        solver_type& operator=(solver_type const &S)
        {
          if(this!= &S)
          {
            this->m_skeleton   = S.m_skeleton;
            this->m_chains     = S.m_chains;
            this->m_gradient   = S.m_gradient;
            this->m_function   = S.m_function;
            this->m_projection = S.m_projection;
            this->m_theta      = S.m_theta;
            this->m_H          = S.m_H;

            this->m_gradient.init(this);
            this->m_function.init(this);
            this->m_projection.init(this);
          }
          return *this;
        }

      public:

        class Output
        {
        protected:

          real_type   m_value;           
          real_type   m_wall_time;       
          size_t      m_status;          
          std::string m_error_message;   
          size_t      m_iterations;      
          real_type   m_gradient_norm;   
          vector_type m_profiling;       

        public:

          real_type   const & value () const { return m_value;}
          real_type         & value ()       { return m_value;}

          real_type   const & wall_time () const { return m_wall_time;}
          real_type         & wall_time ()       { return m_wall_time;}

          size_t      const & status () const { return m_status;}
          size_t            & status ()       { return m_status;}

          std::string const & error_message () const { return m_error_message;}
          std::string       & error_message ()       { return m_error_message;}

          size_t      const & iterations () const { return m_iterations;}
          size_t            & iterations ()       { return m_iterations;}

          real_type   const & gradient_norm () const { return m_gradient_norm;}
          real_type         & gradient_norm ()       { return m_gradient_norm;}

          vector_type const * profiling () const { return &m_profiling;}
          vector_type       * profiling ()       { return &m_profiling;}

        public:

          Output()
            : m_value(value_traits::zero())
            , m_wall_time(value_traits::zero())
            , m_status( OpenTissue::math::optimization::OK )
            , m_error_message( "" )
            , m_iterations(0u)
            , m_gradient_norm( value_traits::infinity() )
          {}

          Output( Output const & o)
          {
            *this = o;
          }

          Output & operator=( Output const & o)
          {
            if( this != &o)
            {
              m_value         = o.m_value;
              m_wall_time     = o.m_wall_time;
              m_status        = o.m_status;
              m_error_message = o.m_error_message;
              m_iterations    = o.m_iterations;
              m_gradient_norm = o.m_gradient_norm;
              m_profiling     = o.m_profiling;
            }
            return *this;
          }

        };

      public:

        class Settings
        {
        protected:

          size_t    m_choice;                
          bool      m_restart;               
          size_t    m_max_iterations;        
          real_type m_absolute_tolerance;    
          real_type m_relative_tolerance;    
          real_type m_stagnation_tolerance;  
          real_type m_alpha;                 
          real_type m_beta;                  
          real_type m_tau;                   

        public:

          size_t    const & choice () const { return m_choice; }
          size_t          & choice ()       { return m_choice; }

          bool      const & restart () const { return m_restart; }
          bool            & restart ()       { return m_restart; }

          size_t    const & max_iterations () const { return m_max_iterations; }
          size_t          & max_iterations ()       { return m_max_iterations; }

          real_type const & absolute_tolerance () const { return m_absolute_tolerance; }
          real_type       & absolute_tolerance ()       { return m_absolute_tolerance; }

          real_type const & relative_tolerance () const { return m_relative_tolerance; }
          real_type       & relative_tolerance ()       { return m_relative_tolerance; }

          real_type const & stagnation_tolerance () const { return m_stagnation_tolerance; }
          real_type       & stagnation_tolerance ()       { return m_stagnation_tolerance; }

          real_type const & alpha () const { return m_alpha; }
          real_type       & alpha ()       { return m_alpha; }

          real_type const & beta () const { return m_beta; }
          real_type       & beta ()       { return m_beta; }

          real_type const & tau () const { return m_tau; }
          real_type       & tau ()       { return m_tau; }

        public:

          Settings()
            : m_choice(0u)
            , m_restart(true)
            , m_max_iterations(0u)
            , m_absolute_tolerance( value_traits::zero() )
            , m_relative_tolerance( value_traits::zero() )
            , m_stagnation_tolerance( value_traits::zero() )
            , m_alpha( value_traits::zero() )
            , m_beta( value_traits::zero() )
            , m_tau( value_traits::zero() )
          {}

          Settings(
            size_t    const & choice
            , bool      const & restart
            , size_t    const & max_iterations
            , real_type const & absolute_tolerance
            , real_type const & relative_tolerance
            , real_type const & stagnation_tolerance
            , real_type const & alpha
            , real_type const & beta
            , real_type const & tau
            )
            : m_choice(choice)
            , m_restart(restart)
            , m_max_iterations(max_iterations)
            , m_absolute_tolerance( absolute_tolerance )
            , m_relative_tolerance( relative_tolerance )
            , m_stagnation_tolerance( stagnation_tolerance )
            , m_alpha( alpha )
            , m_beta( beta )
            , m_tau( tau )
          {}

          Settings(Settings const & s){  *this = s; }

          Settings & operator=(Settings const & s)
          {
            if(this != &s)
            {
              m_choice               = s.choice();
              m_restart              = s.restart();
              m_max_iterations       = s.max_iterations();
              m_absolute_tolerance   = s.absolute_tolerance();
              m_relative_tolerance   = s.relative_tolerance();
              m_stagnation_tolerance = s.stagnation_tolerance();
              m_alpha                = s.alpha();
              m_beta                 = s.beta();
              m_tau                  = s.tau();
            }
            return *this;
          }

        };


        static Settings const & default_BFGS_settings()
        {
          static Settings settings( 
            0u
            , true
            , 5u
            , boost::numeric_cast<real_type>(1e-3)
            , boost::numeric_cast<real_type>(0.00001)
            , boost::numeric_cast<real_type>(0.00001)
            , boost::numeric_cast<real_type>(0.0001)
            , boost::numeric_cast<real_type>(0.5)
            , value_traits::zero()
            );
          return settings;
        }

        static Settings const & default_SD_settings()
        {
          static Settings settings( 
            1u
            , false
            , 10u
            , boost::numeric_cast<real_type>(1e-3)
            , boost::numeric_cast<real_type>(0.00001)
            , boost::numeric_cast<real_type>(0.00001)
            , boost::numeric_cast<real_type>(0.0001)
            , boost::numeric_cast<real_type>(0.5)
            , value_traits::zero()
            );
          return settings;
        }

        static Settings const & default_SDF_settings()
        {
          static Settings settings( 
            2u
            , false
            , 100u
            , boost::numeric_cast<real_type>(1e-3)
            , boost::numeric_cast<real_type>(0.00001)
            , boost::numeric_cast<real_type>(0.00001)
            , value_traits::zero()
            , value_traits::zero()
            , boost::numeric_cast<real_type>(0.001)
            );
          return settings;
        }



      public:

        void init(skeleton_type const & S)
        {
          this->m_theta.resize(S.size()*6u);
          this->m_theta.clear();

          this->m_H.clear();
          this->m_H.assign(identity_matrix_type(this->m_H.size1(),this->m_H.size2()));
          this->m_chains.clear();
          this->m_skeleton = const_cast<skeleton_type*>(&S);

          this->m_gradient.init(this);
          this->m_function.init(this);
          this->m_projection.init(this);
        }

        void solve(
          Settings const * settings_ = 0
          , Output * output = 0
          )
        {   
          Settings const * settings = settings_ ? settings_ : &this->default_SD_settings();

          size_t        status        = 0u;
          size_t        iteration     = 0u;
          real_type     gradient_norm = value_traits::zero();
          vector_type * profiling     = output ? output->profiling() : 0;

          OpenTissue::utility::Timer<real_type> watch;

          if(output)
            watch.start();

          OpenTissue::kinematics::inverse::get_joint_parameters( *(this->m_skeleton), this->m_theta );

          switch( settings->choice() )
          {
          case 0: /*use BFGS */
            {
              //Before invoking solver we must determine the initial estimate of the Hessian matrix
              size_t const N = this->m_theta.size();
              if(  this->m_H.size1()!=N || this->m_H.size2()!=N )
              {
                this->m_H.resize( N, N, false);
                this->m_H.assign( identity_matrix_type(this->m_H.size1(),this->m_H.size2()) );
              }

              if(settings->restart())
              {
                this->m_H.assign(identity_matrix_type(this->m_H.size1(),this->m_H.size2()));
              }

              OpenTissue::math::optimization::projected_bfgs(
                this->m_function
                , this->m_gradient
                , this->m_H
                , this->m_theta
                , this->m_projection
                , settings->max_iterations()
                , settings->absolute_tolerance()
                , settings->relative_tolerance()
                , settings->stagnation_tolerance()
                , status
                , iteration
                , gradient_norm
                , settings->alpha()
                , settings->beta()
                , profiling
                );
            }
            break;
          case 1: /*use Steepest Descent with Line-Search */
            {
              OpenTissue::math::optimization::projected_steepest_descent(
                this->m_function
                , this->m_gradient
                , this->m_theta
                , this->m_projection
                , settings->max_iterations()
                , settings->absolute_tolerance()
                , settings->relative_tolerance()
                , settings->stagnation_tolerance()
                , status
                , iteration
                , gradient_norm
                , settings->alpha()
                , settings->beta()
                , profiling
                );
            }
            break;
          case 2: /*use Steepest Descent with Fixed Step-Length */
            {
              OpenTissue::math::optimization::projected_steepest_descent(
                this->m_function
                , this->m_gradient
                , this->m_theta
                , this->m_projection
                , settings->max_iterations()
                , settings->absolute_tolerance()
                , settings->relative_tolerance()
                , settings->stagnation_tolerance()
                , status
                , iteration
                , gradient_norm
                , settings->tau()
                , profiling
                );
            }
            break;
          default:
            assert(false || !"solve(): Could not determine the numerical method to be used");
            break;
          };

          OpenTissue::kinematics::inverse::set_joint_parameters( *(this->m_skeleton) , this->m_theta );


          if(output)
          {
            watch.stop();
            output->status()        = status;
            output->error_message() = OpenTissue::math::optimization::get_error_message(status);
            output->gradient_norm() = gradient_norm;
            output->iterations()    = iteration;
            output->value()         = this->m_function( this->m_theta );
            output->wall_time()     = watch();
 
          }

        }

      };

    } // namespace inverse
  } // namespace kinematics
} // namespace OpenTissue

//OPENTISSUE_KINEMATICS_INVERSE_INVERSE_NONLINEAR_SOLVER_H
#endif