JKatoomba.hh

Go to the documentation of this file.

#ifndef __JACOUSTICS__JKATOOMBA__
#define __JACOUSTICS__JKATOOMBA__

#include <vector>

#include "JFit/JEstimator.hh"
#include "JFit/JRegressor.hh"
#include "JFit/JSimplex.hh"
#include "JFit/JMEstimator.hh"
#include "JMath/JMatrixNS.hh"
#include "JLang/JSharedPointer.hh"

#include "JMath/JMatrixNS.hh"
#include "Jeep/JMessage.hh"

#include "JAcoustics/JSoundVelocity.hh"
#include "JAcoustics/JEKey.hh"
#include "JAcoustics/JGeometry.hh"
#include "JAcoustics/JModel.hh"
#include "JAcoustics/JHit.hh"


/**
 * \file
 *
 * Fit functions of acoustic model.
 * \author mdejong
 */
namespace JACOUSTICS {}
namespace JPP { using namespace JACOUSTICS; }

namespace JACOUSTICS {

  using JMATH::JMath;
  using JFIT::JEstimator;
  using JFIT::JAbstractMinimiser;
  using JFIT::JSimplex;
  using JFIT::JMEstimator;


  /**
   * Enumeration for fit algorithms.
   */
  enum JFit_t {
    linear_t      = 0,            //!< Linear  fit
    simplex_t,                    //!< Simplex fit
    gandalf_t                     //!< Gandalf fit
  };


  /**
   * Auxiliary base class for fit function of acoustic model.
   */
  struct JKatoomba_t : 
    public JGeometry
  {
    /**
     * Constructor
     *
     * \param  detector           detector
     * \param  velocity           sound velocity
     */
    JKatoomba_t(const JDetector&      detector,
                const JSoundVelocity& velocity) :
      detector(detector),
      velocity(velocity)
    {};


    /**
     * Get estimated time-of-arrival for given hit.
     *
     * \param  model              model
     * \param  hit                hit
     * \return                    time-of-arrival
     */
    template<class JPDF_t>
    double getToA(const JModel& model, const JHit<JPDF_t>& hit) const
    {
      const JGEOMETRY::JString& string     = detector[hit.getString()];
      const JMODEL   ::JString& parameters = model.string[hit.getString()];
      const JPosition3D         position   = string.getPosition(parameters, hit.getFloor());

      const double D     = hit.getDistance(position);
      const double Vi    = velocity.getInverseVelocity(D, hit.getZ(), position.getZ());

      return model.emitter[hit.getEKey()].t1  +  D * Vi;
    }


    const JDetector& detector;
    JSoundVelocity   velocity;

    JLANG::JSharedPointer<JMEstimator> estimator;  //!< M-Estimator function


    /**
     * Get fit option.
     *
     * \return                    option
     */
    static bool getOption()
    {
      return get_option();
    }


    /**
     * Set fit option.
     *
     * \param  option             option
     */
    static void setOption(const bool option)
    {
      get_option() = option;
    }

  protected:
    /**
     * Get reference to fit option.
     *
     * \return                    option
     */
    static bool& get_option()
    {
      static bool option = true;

      return option;
    }


    /**
     * H-equation as per hit.
     */
    struct H_t :
      public JMODEL::JEmitter,
      public JMODEL::JString,
      public JMath<H_t>
    {
      /**
       * Default constructor.
       */
      H_t() :
        JMODEL::JEmitter(),
        JMODEL::JString ()
      {}


      /**
       * Constructor.
       *
       * \param  emitter       emitter
       * \param  string        string
       */
      H_t(const JMODEL::JEmitter& emitter,
          const JMODEL::JString&  string) :
        JMODEL::JEmitter(emitter),
        JMODEL::JString (string)
      {}


      /**
       * Scale H-equation.
       *
       * \param  factor        multiplication factor
       * \return               this H-equation
       */
      H_t& mul(const double factor)
      {
        static_cast<JMODEL::JEmitter&>(*this).mul(factor);
        static_cast<JMODEL::JString&> (*this).mul(factor);

        return *this;
      }
    };

    /**
     * Indices of H-equation in global model.
     */
    struct I_t 
    {
      /**
       * Default constructor.
       */
      I_t() :
        t1(0),
        tx(0),
        ty(0)
      {}


      /**
       * Constructor.
       *
       * \param  t1            index t1
       * \param  tx            index tx
       * \param  ty            index ty
       */
      I_t(const int t1,
          const int tx,
          const int ty) :
        t1(t1),
        tx(tx),
        ty(ty)
      {}

      int t1;
      int tx;
      int ty;
    };
  };


  /**
   * Template definition of fit function of acoustic model.
   */
  template<template<class T> class JMinimiser_t>
  struct JKatoomba; 


  /**
   * Template specialisation of fit function of acoustic model based on JAbstractMinimiser minimiser.\n
   * This class can be used to evaluate the chi2.
   */
  template<>
  struct JKatoomba<JAbstractMinimiser> : 
    public JAbstractMinimiser<JModel>,
    public JKatoomba_t
  {
    typedef double                                        result_type;


    /**
     * Constructor
     *
     * \param  detector           detector
     * \param  velocity           sound velocity
     */
    JKatoomba(const JDetector&      detector,
              const JSoundVelocity& velocity) :
      JKatoomba_t(detector, velocity)
    {};


    /**
     * Fit function.\n
     * This method is used to determine the chi2 of given hit with respect to actual model.
     *
     * \param  model              model
     * \param  hit                hit
     * \return                    chi2 and gradient
     */
    template<class JPDF_t>
    result_type operator()(const JModel& model, const JHit<JPDF_t>& hit) const
    {
      const double toa_s = this->getToA(model, hit);
      const double u     = (toa_s - hit.getValue()) / hit.sigma;

      return estimator->getRho(u);
    }


    /**
     * Fit.
     *
     * \param  model              model
     * \param  __begin            begin of hits
     * \param  __end              end   of hits
     * \return                    chi2
     */
    template<class T>
    result_type operator()(const JModel& model, T __begin, T __end)
    {
      this->value = model;

      return JAbstractMinimiser<JModel>::operator()(*this, __begin, __end);
    }
  };


  /**
   * Template specialisation of fit function of acoustic model based on linear approximation.
   */
  template<>
  struct JKatoomba<JEstimator> : 
    public JKatoomba_t
  {
    /**
     * Constructor
     *
     * \param  detector           detector
     * \param  velocity           sound velocity
     */
    JKatoomba(const JDetector&      detector,
              const JSoundVelocity& velocity) :
      JKatoomba_t(detector, velocity)
    {};


    /**
     * Get start values of string parameters.\n
     * Note that this method may throw an exception.
     *
     * \param  __begin            begin of hits
     * \param  __end              end   of hits
     * \return                    model
     */
    template<class T>
    const JModel& operator()(T __begin, T __end) const
    {
      using namespace std;
      using namespace JPP;
      using namespace JGEOMETRY;

      value = JModel(__begin, __end);                 // set up global model according data

      H_t H;                                          // H-equation as per hit
      I_t i;                                          // indices of H-equation in global model 

      const size_t N = (getOption() ? value.getN() : value.emitter.getN());
      
      JMatrixNS      V(N);                            // V = H^T x H
      vector<double> Y(N, 0.0);                       // Y = H^T x y
      

      for (T hit = __begin; hit != __end; ++hit) {

        const JString&    string   = detector[hit->getString()];
        const JPosition3D position = string.getPosition(hit->getFloor());
        const double      Vi       = velocity.getInverseVelocity(hit->getDistance(position), hit->getZ(), position.getZ());

        const double      h1 = string.getHeight(hit->getFloor());
        const JPosition3D p1 = string.getPosition()  -  hit->getPosition();
        const double      ds = sqrt(p1.getLengthSquared()  +  h1*h1  +  2.0*p1.getZ()*h1);
        const double      y  = hit->getValue()  -  Vi*ds;
        const double      W  = sqrt(hit->getWeight());

        H.t1  =  W * 1.0;
        H.tx  =  W * Vi * p1.getX() * h1 / ds;
        H.ty  =  W * Vi * p1.getY() * h1 / ds;
        
        i.t1  =  value.getIndex(hit->getEKey(),   &H_t::t1);
        i.tx  =  value.getIndex(hit->getString(), &H_t::tx);
        i.ty  =  value.getIndex(hit->getString(), &H_t::ty);

        V(i.t1, i.t1) += H.t1 * H.t1;

        Y[i.t1] += W * H.t1 * y;

        if (getOption()) {
          
          V(i.t1, i.tx) += H.t1 * H.tx;  V(i.t1, i.ty) += H.t1 * H.ty;
          V(i.tx, i.t1) += H.tx * H.t1;  V(i.ty, i.t1) += H.ty * H.t1;

          V(i.tx, i.tx) += H.tx * H.tx;  V(i.tx, i.ty) += H.tx * H.ty;
          V(i.ty, i.tx) += H.ty * H.tx;  V(i.ty, i.ty) += H.ty * H.ty;

          Y[i.tx] += W * H.tx * y;
          Y[i.ty] += W * H.ty * y;
        }
      }

      // evaluate (H^T x H)^-1 x H^T x y 

      V.invert();

      for (size_t row = 0; row != N; ++row) {
        for (size_t col = 0; col != N; ++col) {
          value[row] += V(row,col) * Y[col];
        }
      }
      
      return value;
    }

  private:
    mutable JModel value;
  };


  /**
   * Template specialisation of fit function of acoustic model based on JSimplex minimiser.
   */
  template<>
  struct JKatoomba<JSimplex> : 
    public JSimplex<JModel>,
    public JKatoomba_t
  {
    /**
     * Constructor
     *
     * \param  detector           detector
     * \param  velocity           sound velocity
     */
    JKatoomba(const JDetector&      detector,
              const JSoundVelocity& velocity) :
      JKatoomba_t(detector, velocity)
    {};


    /**
     * Fit function.\n
     * This method is used to determine the chi2 of given hit with respect to actual model.
     *
     * \param  model              model
     * \param  hit                hit
     * \return                    chi2
     */
    template<class JPDF_t>
    double operator()(const JModel& model, const JHit<JPDF_t>& hit) const
    {
      const double toa_s = this->getToA(model, hit);
      const double u     = (toa_s - hit.getValue()) / hit.sigma;

      return estimator->getRho(u);
    }


    /**
     * Fit.
     *
     * \param  __begin            begin of hits
     * \param  __end              end   of hits
     * \return                    chi2
     */
    template<class T>
    double operator()(T __begin, T __end)
    {
      this->step.clear();
      
      for (JModel::string_type::const_iterator i = this->value.string.begin(); i != this->value.string.end(); ++i) {

        JModel model;

        model.string[i->first] = JMODEL::JString(1.0e-3, 0.0);

        this->step.push_back(model);

        model.string[i->first] = JMODEL::JString(0.0, 1.0e-3);

        this->step.push_back(model);
      }

      for (JModel::emitter_type::const_iterator i = this->value.emitter.begin(); i != this->value.emitter.end(); ++i) {

        JModel model;

        model.emitter[i->first] = JMODEL::JEmitter(5.0e-5);

        this->step.push_back(model);
      }

      return JSimplex<JModel>::operator()(*this, __begin, __end);
    }
  };


  template<class T>
  class JGandalf {};


  /**
   * Template specialisation of fit function of acoustic model based on JGandalf minimiser.
   */
  template<>
  struct JKatoomba<JGandalf> : 
    public JKatoomba_t
  {
    typedef double result_type;


    /**
     * Constructor
     *
     * \param  detector           detector
     * \param  velocity           sound velocity
     */
    JKatoomba(const JDetector&      detector,
              const JSoundVelocity& velocity) :
      JKatoomba_t(detector, velocity)
    {};


    /**
     * Fit.
     *
     * \param  __begin            begin of hits
     * \param  __end              end   of hits
     * \return                    chi2 and gradient
     */
    template<class T>
    result_type operator()(T __begin, T __end)
    {
      using namespace std;
      using namespace JPP;

      const int N = (getOption() ? value.getN() : value.emitter.getN());

      V.resize(N);
      Y.resize(N);
      h.resize(N);

      lambda    = LAMBDA_MIN;

      result_type precessor = numeric_limits<double>::max();
      
      for (numberOfIterations = 0; numberOfIterations != MAXIMUM_ITERATIONS; ++numberOfIterations) {

        DEBUG("step:     " << numberOfIterations << endl);

        evaluate(__begin, __end);

        DEBUG("lambda:   " << FIXED(12,5) << lambda    << endl);
        DEBUG("chi2:     " << FIXED(12,5) << successor << endl);

        if (successor < precessor) {

          if (numberOfIterations != 0) {

            if (fabs(precessor - successor) < EPSILON*fabs(precessor)) {
              return successor;
            }

            if (lambda > LAMBDA_MIN) {
              lambda /= LAMBDA_DOWN;
            }
          }

          precessor = successor;
          previous  = value;

        } else {

          value   = previous;
          lambda *= LAMBDA_UP;

          if (lambda > LAMBDA_MAX) {
            return precessor;                                   // no improvement found
          }

          evaluate(__begin, __end);
        }


        // force definite positiveness

        for (int i = 0; i != N; ++i) {

          if (V(i,i) < PIVOT) {
            V(i,i) = PIVOT;
          }

          h[i] = 1.0 / sqrt(V(i,i));
        }


        // normalisation

        for (int i = 0; i != N; ++i) {
          for (int j = 0; j != i; ++j) {
            V(j,i) *= h[i] * h[j];
            V(i,j)  = V(j,i);
          }
        }

        for (int i = 0; i != N; ++i) {
          V(i,i) = 1.0 + lambda;
        }


        try {
          V.invert();
        }
        catch (const JException& error) {
          ERROR("JKatoomb<JGandalf>: " << error.what() << endl);
          return precessor;
        }


        for (int i = 0; i != N; ++i) {

          DEBUG("u[" << noshowpos << i << "] = " << showpos << FIXED(15,5) << value[i]);

          double y = 0.0;

          for (int j = 0; j != N; ++j) {
            y += V(i,j) * Y[j] * h[i] * h[j];
          }

          value[i] -= y;

          DEBUG(' ' << FIXED(15,5) << y << noshowpos << endl);
        }
      }

      return 0.0;
    }

    static int     debug;                                      //!< debug level
    static int     MAXIMUM_ITERATIONS;                         //!< maximal number of iterations
    static double  EPSILON;                                    //!< maximal distance to minimum
    static double  LAMBDA_MIN;                                 //!< minimal value control parameter
    static double  LAMBDA_MAX;                                 //!< maximal value control parameter
    static double  LAMBDA_UP;                                  //!< multiplication factor control parameter
    static double  LAMBDA_DOWN;                                //!< multiplication factor control parameter
    static double  PIVOT;                                      //!< minimal value diagonal element of matrix

    double                      lambda;
    JModel                      value;
    int                         numberOfIterations;
    JMATH::JMatrixNS            V;

  private:
    /**
     * Evaluation of fit.
     *
     * \param  __begin        begin of data
     * \param  __end          end   of data
     */
    template<class T>
    inline void evaluate(T __begin, T __end)
    {
      using namespace std;
      using namespace JPP;

      successor = 0.0;

      V.reset();

      for (std::vector<double>::iterator i = Y.begin(); i != Y.end(); ++i) {
        *i = 0.0;
      }

      for (T hit = __begin; hit != __end; ++hit) {

        const JGEOMETRY::JString& string     = detector[hit->getString()];
        const JMODEL   ::JString& parameters = value.string[hit->getString()];
        const JPosition3D         position   = string.getPosition(parameters, hit->getFloor());

        const double D     = hit->getDistance(position);
        const double Vi    = velocity.getInverseVelocity(D, hit->getZ(), position.getZ());
        const double toa_s = value.emitter[hit->getEKey()].t1  +  D * Vi;
          
        const double u     = (toa_s - hit->getValue()) / hit->sigma;
        const double W     = sqrt(hit->getWeight());

        successor += (W*W) * estimator->getRho(u);
        
        H_t H(1.0, string.getGradient(parameters, hit->getPosition(), hit->getFloor()) * Vi);

        H    *= W * estimator->getPsi(u) / hit->sigma;

        I_t i;

        i.t1  =  value.getIndex(hit->getEKey(),   &H_t::t1);
        i.tx  =  value.getIndex(hit->getString(), &H_t::tx);
        i.ty  =  value.getIndex(hit->getString(), &H_t::ty);

        V(i.t1, i.t1) += H.t1 * H.t1;

        Y[i.t1] += W * H.t1;

        if (getOption()) {
          
          V(i.t1, i.tx) += H.t1 * H.tx;  V(i.t1, i.ty) += H.t1 * H.ty;
          V(i.tx, i.t1) += H.tx * H.t1;  V(i.ty, i.t1) += H.ty * H.t1;

          V(i.tx, i.tx) += H.tx * H.tx;  V(i.tx, i.ty) += H.tx * H.ty;
          V(i.ty, i.tx) += H.ty * H.tx;  V(i.ty, i.ty) += H.ty * H.ty;

          Y[i.tx] += W * H.tx;
          Y[i.ty] += W * H.ty;
        }
      }
    }


    std::vector<double>         Y;                             // gradient
    result_type                 successor;
    JModel                      previous;
    std::vector<double>         h;                             // normalisation vector
  };


  /**
   * debug level.
   */
  int JKatoomba<JGandalf>::debug = 0;


  /**
   * maximal number of iterations.
   */
  int JKatoomba<JGandalf>::MAXIMUM_ITERATIONS = 1000;


  /**
   * maximal distance to minimum.
   */
  double JKatoomba<JGandalf>::EPSILON         = 1.0e-3;


  /**
   * minimal value control parameter
   */
  double JKatoomba<JGandalf>::LAMBDA_MIN      =   0.01;


  /**
   * maximal value control parameter
   */
  double JKatoomba<JGandalf>::LAMBDA_MAX      = 100.0;


  /**
   * multiplication factor control parameter
   */
  double JKatoomba<JGandalf>::LAMBDA_UP       =   9.0;


  /**
   * multiplication factor control parameter
   */
  double JKatoomba<JGandalf>::LAMBDA_DOWN     =  11.0;


  /**
   * minimal value diagonal element of matrix
   */
  double JKatoomba<JGandalf>::PIVOT           =   1.0e-3;
}

#endif