hypothesis3D.h

Go to the documentation of this file.

#ifndef MMVL_HYPOTHESIS_3D_HH
#define MMVL_HYPOTHESIS_3D_HH

#ifndef NDEBUG
#include <iostream>
#endif
#include "hypothesis.h"
#include "dice.h"

namespace mimas{
  namespace hf{

    class hypothesis3D: public hypothesis{
    public:
      static float transX;
      static float transY;
      static float transZ;
      static float rotX;
      static float rotY;
      static float rotZ;
      
      float x;
      float y;
      float z;
      float Rx;
      float Ry;
      float Rz;

#ifndef NDEBUG
      void debug(void) const
  {
    std::cerr<<"X: "  <<x<<std::endl;
    std::cerr<<"Y: "  <<y<<std::endl;
    std::cerr<<"Z: "  <<z<<std::endl;
    std::cerr<<"Rx: " <<Rx<<std::endl;
    std::cerr<<"Ry: " <<Ry<<std::endl;
    std::cerr<<"Rz: " <<Rz<<std::endl;
  }
#endif
      
      hypothesis3D drift(void) const
      {
  return hypothesis3D(x + Dice::gaussian_random() * transX,
          y + Dice::gaussian_random() * transY,
          z + Dice::gaussian_random() * transZ,
          Rx + Dice::gaussian_random() * rotX,
          Ry + Dice::gaussian_random() * rotY,
          Rz + Dice::gaussian_random() * rotZ,
          weight);
      }

      hypothesis3D mean(const hypothesis3D &h)
      {
  if(h.weight != 0)
    {
      float coord1 = (x * weight  + h.x  * h.weight) / (weight + h.weight);
      float coord2 = (y * weight  + h.y  * h.weight) / (weight + h.weight);
      float coord3 = (z * weight  + h.z  * h.weight) / (weight + h.weight);
      float coord4 = (Rx * weight + h.Rx * h.weight) / (weight + h.weight);
      float coord5 = (Ry * weight + h.Ry * h.weight) / (weight + h.weight);
      float coord6 = (Rz * weight + h.Rz * h.weight) / (weight + h.weight);
  
      return hypothesis3D(coord1,
        coord2,
        coord3,
        coord4,
        coord5,
        coord6,
        weight + h.weight
        );
    }
  return *this;
      }

      float distance(const hypothesis3D &h)
      {
  /*
    !!!!!!! Research work can be carry out here
    this tuning is quite complicated and probably very important to do
    the underlying ideas I used is to vary by the same amount the dimension and
    to see how it influence the projection of the object (another problem is
    that the amount of variation is dependant with the value)

    Here I still haven't done any test but I consider that under a distance value of around 60
    the position is similar.
    For the angle 3 degree * 3 angle * 4 + 3 * 4mm * 2 horizontal_mvt + 15
  */
  float TO_DETERMINE_ANGLE = 5;
  float TO_DETERMINE_XY = 4;
    
  float angular_distance = sqrt((h.Rx - Rx) * (h.Rx - Rx) + (h.Ry - Ry) * (h.Ry - Ry));
  float XYpositional_distance = sqrt((h.x - x) * (h.x - x) + (h.y - y) * (h.y - y));
  float Zpositional_distance = h.z - z;
  return Zpositional_distance + XYpositional_distance * TO_DETERMINE_XY + angular_distance * TO_DETERMINE_ANGLE;
      }
      
      hypothesis3D(float x = 0, float y = 0, float z = 0, float Rx = 0, float Ry = 0, float Rz = 0, double weight = 1.)
  :hypothesis(weight),x(x), y(y), z(z), Rx(Rx), Ry(Ry), Rz(Rz){}
      
    };
  }
} 
#endif

---