JCompass/JModel.hh

Go to the documentation of this file.

#ifndef __JCOMPASS_JMODEL__
#define __JCOMPASS_JMODEL__
 
#include <memory>
 
#include "JGeometry3D/JQuaternion3D.hh"
#include "JGeometry3D/JEigen3D.hh"
#include "JLang/JException.hh"
#include "JMath/JMath.hh"
#include "JMath/JMathToolkit.hh"
#include "JFit/JMEstimator.hh"
 
#include "JCompass/JHit.hh"
 
 
/**
 * \author mdejong
 */
namespace JCOMPASS {}
namespace JPP { using namespace JCOMPASS; }
 
namespace JCOMPASS {
 
  using JMATH::JMath;
  using JGEOMETRY3D::JQuaternion3D;
  using JLANG::JValueOutOfRange;
  using JFIT::JMEstimator;
  using JFIT::getMEstimator;
 
  /**
   * Model.
   */
  struct JModel :
    public JMath<JModel>
  {
    /**
     * Default constructor.
     */
    JModel() :
      Q0(JQuaternion3D::getIdentity()),
      Q1(JQuaternion3D::getIdentity())
    {}
 
 
    /**
     * Constructor.
     *
     * \param Q0              tilt
     * \param Q1              twist
     */
    JModel(const JQuaternion3D& Q0,
           const JQuaternion3D& Q1) :
      Q0(Q0),
      Q1(Q1)
    {}
 
 
    /**
     * Constructor.
     *
     * The data type corresponding to the hits should provide for the following policy methods.
     * <pre>
     *    double        getZ();             // get z-position
     *    JQuaternion3D getQuaternion();    // get quaternion
     * </pre>
     * Note that the input data should be ordered with increasing z-positions.
     *
     * \param __begin         begin of data
     * \param __end           end   of data
     */
    template<class T>
    JModel(T __begin,
           T __end) :
      Q0(JQuaternion3D::getIdentity()),
      Q1(JQuaternion3D::getIdentity())
    {
      using namespace std;
      using namespace JPP;
 
      const int N = distance(__begin, __end);
 
      if (N >= NUMBER_OF_PARAMETERS) {
 
        vector<JQuaternion3D> buffer;
 
        for (T q = __begin, p = q++; q != __end; ++p, ++q) {
 
          const double dz = q->getZ() - p->getZ();
 
          if (dz != 0.0) {
 
            JQuaternion3D Q(p->getQuaternion());
 
            Q.conjugate();
            Q.mul(q->getQuaternion());
            Q.pow(1.0 / dz);
 
            buffer.push_back(Q);
          }
        }
 
        Q1  = getAverage(buffer.begin(), buffer.end());
        Q1  = JQuaternion3D::decomposition(Q1, JVector3Z_t).twist;
 
        const double z1 = getAverage(make_array(__begin, __end, &JHit::getZ));
 
        Q0  = getAverage(make_array(__begin, __end, &JHit::getQuaternion));
        Q0  = pow(Q1, -z1) * Q0;
 
      } else {
        THROW(JValueOutOfRange, "JModel: Not enough data points " << N);
      }
    }
 
 
    /**
     * Add model.
     *
     * \param  model          model
     * \return                this model
     */
    JModel& add(const JModel& model)
    {
      Q0 *= model.Q0;
      Q1 *= model.Q1;
 
      Q0.normalise();
      Q1.normalise();
 
      return *this;
    }
 
 
    /**
     * Subtract model.
     *
     * \param  model          model
     * \return                this model
     */
    JModel& sub(const JModel& model)
    {
      Q0 *= model.Q0.getConjugate();
      Q1 *= model.Q1.getConjugate();
 
      Q0.normalise();
      Q1.normalise();
 
      return *this;
    }
 
 
    /**
     * Scale model.
     *
     * \param  factor         multiplication factor
     * \return                this model
     */
    JModel& mul(const double factor)
    {
      Q0.pow(factor);
      Q1.pow(factor);
 
      return *this;
    }
 
 
    /**
     * Get quaternion at given z-position.
     *
     * \param z               z-position.
     * \return                quaternion
     */
    JQuaternion3D operator()(const double z) const
    {
      using namespace JPP;
 
      return pow(Q1, z) * Q0;
    }
 
    static const int NUMBER_OF_PARAMETERS  =  4;  //!< number of parameters of fit per quaternion
 
    JQuaternion3D Q0;   //!< tilt
    JQuaternion3D Q1;   //!< twist
  };
 
 
  /**
   * Auxiliary data structure for chi2 evaluation.
   */
  struct JChi2 {
    /**
     * Constructor.
     *
     * \param  type        M-Estimator type
     */
    JChi2(const int type) :
      estimator(getMEstimator(type))
    {}
 
 
    /**
     * Fit function.
     *
     * \param  model          model
     * \param  hit            hit
     * \return                chi2
     */
    inline double operator()(const JModel& model, const JHit& hit) const
    {
      return estimator->getRho(getAngle(model(hit.getZ()), hit.getQuaternion()) / hit.getSigma());
    }
 
    std::shared_ptr<JMEstimator> estimator;  //!< M-Estimator function
  };
}
 
#endif