tsai.hpp

/* $Id$ */

#ifndef LOCALIZER_TSAI_HPP
#define LOCALIZER_TSAI_HPP

#include "kernel/jafarException.hpp"
#include "localizer/point2d3d.hpp"
#include "jmath/jblas.hpp"
#include "jmath/linearLeastSquares.hpp"
//#include "geom/Point.hpp"
#include "geom/Declarations.hpp"

namespace jafar {

  namespace localizer {

    class TsaiLocalizer {

      protected:

      static const double epsylon;
      double alphaU;
      double alphaV;
      double u0;
      double v0;
      Points2d3d pointsVector;
      jblas::mat a;
      jblas::vec b;
      jblas::vec x;
      jmath::LinearLeastSquares lls;
      jblas::mat44 M;
      void initSystem(const int& _cols = 1, 
                      const int& _rows = 1);
      double sign(const double& d);

      public:

      enum ReferenceType {
        JAFAR,
        CALIFE
      };//ReferenceType
      
      TsaiLocalizer(const double& _alphaU,
                    const double& _alphaV, 
                    const double& _u0, 
                    const double& _v0,
                    const ReferenceType& type=TsaiLocalizer::JAFAR);

      virtual ~TsaiLocalizer();
      void setPointsVector(const Points2d3d& _pointsVector);
      void addPoint2d3d(const Point2d3d& p2d3d)
      {
        pointsVector.push_back(p2d3d);
      }
      void addPoint2d3d(const jblas::vec2& p2d, const jblas::vec3& p3d)
      {
        pointsVector.push_back(localizer::Point2d3d(p2d, p3d));
      }
      void addPoint2d3d(const double &u, const double &v, 
                        const double &x, const double &y, const double &z)
      {
        pointsVector.push_back(localizer::Point2d3d(u,v,x,y,z));
      }
      // void setPointsVector(const std::vector<geom::Point2D>& _2DPointsVector,
      //                      const std::vector<geom::Point3D>& _3DPointsVector);
      void setPointsVector(const std::vector<jblas::vec2>& _2DPointsVector,
                           const std::vector<jblas::vec3>& _3DPointsVector);
      virtual void localize() = 0;

      double computeError(const jblas::vec& a, 
                          const double& tz, 
                          const jblas::vec& b, 
                          const int& nb);

      jblas::mat44 transitionMatrix();
      size_t size()
      {
        return pointsVector.size();
      }
    };//class TsaiLocalizer

  }//namespace localizer

}//namespace jafar
#endif