JPoint3DEstimator.hh

Go to the documentation of this file.

#ifndef __JFIT__JPOINT3DESTIMATOR__
#define __JFIT__JPOINT3DESTIMATOR__
 
#include "JMath/JMatrix3S.hh"
#include "JFit/JPoint3D.hh"
#include "JFit/JEstimator.hh"
 
 
/**
 * \file
 * Linear fit of JFIT::JPoint3D.
 * \author mdejong
 */
namespace JFIT {}
namespace JPP { using namespace JFIT; }
 
namespace JFIT {
 
  /**
   * Linear fit of crossing point (position) between axes (objects with position and direction).
   *
   \f{center}\setlength{\unitlength}{0.6cm}\begin{picture}(12,12)
 
   \put( 5.0, 5.0){\circle*{0.3}}
   \put( 4.0, 5.0){\makebox(0,0)[r]{$(x_{0},y_{0},z_{0})$}}
 
   \multiput( 5.0, 5.0)(+0.2, 0.0){18}{\makebox(0,0)[c]{.}}
   \put( 9.0, 5.0){\circle*{0.2}}
   \put( 9.0, 5.0){\vector(+1, 0){0.7}}
   \put(10.0, 5.0){\makebox(0,0)[l]{$(\bar{x}_j,\hat{u}_j)$}}
 
   \multiput( 5.0, 5.0)(-0.2,+0.2){18}{\makebox(0,0)[c]{.}}
   \put( 1.0, 9.0){\circle*{0.2}}
   \put( 1.0, 9.0){\vector(-1,+1){0.6}}
   \put( 0.0,10.0){\makebox(0,0)[r]{$(\bar{x}_k,\hat{u}_k)$}}
 
   \multiput( 5.0, 5.0)(-0.2,-0.2){18}{\makebox(0,0)[c]{.}}
   \put( 1.0, 1.0){\circle*{0.2}}
   \put( 1.0, 1.0){\vector(-1,-1){0.6}}
   \put( 0.0, 0.0){\makebox(0,0)[r]{$(\bar{x}_i,\hat{u}_i)$}}
 
   \end{picture}
   \f}
   *
   * where:
   *
   \f{eqnarray*}{
          \bar{x}_i & = & i^{th} \textrm{ position}        \\
          \hat{u}_i & = & i^{th} \textrm{ direction}       \\
   \f}
   *
   * The parameters \f$\{x_0, y_0, z_0\}\f$ are estimated in the constructor of this class.
   */
  template<>
  class JEstimator<JPoint3D> :
    public JPoint3D
  {
  public:
    /**
     * Constructor.
     *
     * The template argument <tt>T</tt> refers to an iterator of a data structure which should have the following member methods:
     *   - double %getX();       // [m]
     *   - double %getY();       // [m]
     *   - double %getZ();       // [m]
     *   - double %getDX();      // [u]
     *   - double %getDY();      // [u]
     *   - double %getDZ();      // [u]
     *
     * \param  __begin          begin of data
     * \param  __end            end   of data
     */
    template<class T>
    JEstimator(T __begin, T __end)
    {
      using namespace std;
 
      const int N = distance(__begin, __end);
 
      if (N + 1 >= NUMBER_OF_PARAMETERS) {
 
        double x = 0;
        double y = 0;
        double z = 0;
 
        V.reset();
        
        for (T i = __begin; i != __end; ++i) {
          
          const double xx = 1.0  -  i->getDX() * i->getDX();
          const double yy = 1.0  -  i->getDY() * i->getDY();
          const double zz = 1.0  -  i->getDZ() * i->getDZ();
          
          const double xy = -i->getDX() * i->getDY();
          const double xz = -i->getDX() * i->getDZ();
          const double yz = -i->getDY() * i->getDZ();
          
          V.a00 += xx;
          V.a01 += xy;
          V.a02 += xz;
          
          V.a11 += yy;
          V.a12 += yz;
          
          V.a22 += zz;
          
          x  +=  xx * i->getX()  + xy * i->getY()  +  xz * i->getZ();
          y  +=  xy * i->getX()  + yy * i->getY()  +  yz * i->getZ();
          z  +=  xz * i->getX()  + yz * i->getY()  +  zz * i->getZ();
        }
        
        V.a10 = V.a01;
        V.a20 = V.a02;
        V.a21 = V.a12;
        
        V.invert();
        
        __x = V.a00 * x  +  V.a01 * y  +  V.a02 * z;
        __y = V.a10 * x  +  V.a11 * y  +  V.a12 * z;
        __z = V.a20 * x  +  V.a21 * y  +  V.a22 * z;
      
      } else {
        throw JValueOutOfRange("JEstimator<JPoint3D>::JEstimator(): Not enough data points.");
      }
    }
 
 
    static const int NUMBER_OF_PARAMETERS  =  3;  //!< number of parameters of fit
    JMATH::JMatrix3S V;                           //!< co-variance matrix of fit parameters
  };
}
 
#endif