bundleData.hpp

#ifndef BUNDLE_BUNDLEDATA_HPP
#define BUNDLE_BUNDLEDATA_HPP

#include <iostream>
#include <fstream>
#include <vector>

#include "bundle/bundleDimension.hpp"
#include "bundle/bundleParameters.hpp"

#include "kernel/jafarMacro.hpp"

#include "jmath/ublasExtra.hpp"

namespace jafar {
  
  namespace bundle {
    
    /*
     *
     *   CLASS OBSERVATION
     *
     */

    class observation {

    public :
      
      enum obsType {POINT_2D, POINT_3D, CPDV_EULER, CPDV_QUATERNION};                                   
      
      obsType type;
    
      int id;
      
      int idFeature;

      int idView; 
      
    private :

      jblas::vec x;

      jblas::vec residual;
      
      jblas::mat covX;
            
    public :

      //
      // Constructors and desctructor
      //
      //==============================

      observation (obsType type_=POINT_2D) : 
  type(type_),
  id(),
  idFeature(),
  idView(),
  x(obsSize(type_)),
  residual(obsSize(type_)),
  covX(obsSize(type_),obsSize(type_))
      {
  int cpt;

  for (cpt=0;cpt<obsSize(type_);cpt++)
    residual[cpt] = x[cpt] = 0.0;
      };
      
      observation (observation const& O_) :
  type(O_.type),
  id(O_.id),
  idFeature(O_.idFeature),
  idView(O_.idView),
  x(O_.x),
  residual(O_.residual),
  covX(O_.covX)
      {};

      //
      // Read info from file
      //
      //=====================
      bool readFromFile (std::ifstream& infile, double sigma=0.0) 
      {
  if (!(infile.is_open()))
    {
     // std::cout << "erreur lors de la lecture du fichier" << std::endl;
      return false;
    }
  else if (type == POINT_2D)
    return readFromFile2D (infile,sigma);
  else if (type == POINT_3D)
    return readFromFileSTRMOT (infile);
  else if (type== CPDV_EULER)
    return readFromFileSTRMOT (infile);
  else if (type== CPDV_QUATERNION)
    return readFromFileSTRMOT (infile);
  
  return false;
      };

      

      //
      // Get & Set functions
      //
      //=====================
      
      void getCovMatrix (jblas::mat &covX_) const;

      void setValue (jblas::vec &x_, jblas::mat &covX_,  int id_,  int idFeature_, int idView_);
      void setValue (observation const& o_);

      void setResidual (jblas::vec prediction_);              

      void getObs (jblas::vec &x_) const;

      unsigned int size () const
      {
  return(x.size());
      };

      //
      // Residual function
      //
      //===================
      double residualL2Norm ();
      
      double residualWL2Norm ();


      void getResidual (jblas::vec &R_) const ;

      void getResidual (jblas::vec_range &VR_) const;

    private :
      // Return size of vector x for the given obsType 
      static  int obsSize (obsType type_) 
      {
  switch (type_)
    {
    case POINT_2D :
      return (2);
    case POINT_3D :
      return (3);
    case CPDV_EULER :
      return (6);
    case CPDV_QUATERNION :
      return (7);
    default : 
      JFR_RUN_TIME("Undefined observation size for type: " << type_);
    }
      };  
      
      bool readFromFile2D (std::ifstream& infile, double sigma);
  
      bool readFromFileSTRMOT (std::ifstream& infile);

            
    }; // class observation
    




    /*
     *
     *   CLASS OBSERVATIONSCONTAINER
     *
     */

    class observationsContainer  {

     
    public :      
      
      bundleDimension pbDim;

      jblas::bool_mat sparseSB1;
      
      jblas::bool_mat sparseSB2;

      std::vector<observation*> obs2D;

      std::vector<observation*> obs3D;

      std::vector<observation*> obsCPDV;
      

      //
      // Constructors and Desctructors
      //
      //===============================
      

      observationsContainer (bundleDimension const& bDim_) :
  pbDim(bDim_),
  sparseSB1(pbDim.nbObs2d,pbDim.nbCam),
  sparseSB2(pbDim.nbObs2d,pbDim.nbObj3d),
  obs2D(pbDim.nbObs2d),
  obs3D(pbDim.nbObs3d),
  obsCPDV(pbDim.nbObsMvt)
      {
  unsigned int cpt, cpt1, cpt2;

  // Alloc observation de type observation::POINT_2D
  for (cpt=0;cpt<pbDim.nbObs2d;cpt++)
    obs2D[cpt] = new observation(observation::POINT_2D);

  // Alloc observation de type observation::POINT_3D
  if (pbDim.nbObs3d)
    for (cpt=0;cpt<pbDim.nbObs3d;cpt++)
      obs3D[cpt] = new observation(observation::POINT_3D);

  // Alloc observation de type observation::CPDV_EULER
  if (pbDim.nbObsMvt)
    for (cpt=0;cpt<pbDim.nbObsMvt;cpt++)
      obsCPDV[cpt] = new observation(observation::CPDV_EULER);
    
  // Initialisation des matrices sparseSB1 et sparseSB2
  for ( cpt1=0 ; cpt1<sparseSB1.size1() ; cpt1++)
    for ( cpt2=0 ; cpt2<sparseSB1.size2() ; cpt2++)
      sparseSB1(cpt1,cpt2) = false;

  for ( cpt1=0 ; cpt1<sparseSB2.size1() ; cpt1++)
    for ( cpt2=0 ; cpt2<sparseSB2.size2() ; cpt2++)
      sparseSB2(cpt1,cpt2) = false;      
  
      }

      observationsContainer (observationsContainer const& ODB_) :
  pbDim(ODB_.pbDim),
  sparseSB1(ODB_.sparseSB1),
  sparseSB2(ODB_.sparseSB2),
  obs2D(ODB_.obs2D.size()),
  obs3D(ODB_.obs3D.size()),
  obsCPDV(ODB_.obsCPDV.size())
      {
  unsigned int cpt, L;
  
  // Alloc observation de type observation::POINT_2D
  for (cpt=0;cpt<ODB_.obs2D.size();cpt++)
    {
      obs2D[cpt] = new observation(observation::POINT_2D);
      *(obs2D[cpt]) = *(ODB_.obs2D[cpt]);
    }

  // Alloc observation de type observation::POINT_3D
  if ((L=ODB_.obs3D.size())>0)
    for (cpt=0;cpt<L;cpt++)
      {
        obs3D[cpt] = new observation(observation::POINT_3D);
        *(obs3D[cpt]) = *(ODB_.obs3D[cpt]);
      }
  
  // Alloc observation de type observation::CPDV_EULER
  if ((L=ODB_.obsCPDV.size())>0)
    for (cpt=0;cpt<L;cpt++)
      {
        obsCPDV[cpt] = new observation(observation::CPDV_EULER);
        *(obsCPDV[cpt]) = *(ODB_.obsCPDV[cpt]);
      } 
      }

      ~observationsContainer ()
      {
  unsigned int cpt, L;
  
  if ((L=obsCPDV.size())>0)
    for (cpt=0;cpt<L;cpt++)
      delete obsCPDV[cpt];

  if ((L=obs3D.size())>0)
    for (cpt=0;cpt<L;cpt++)
      delete obs3D[cpt];
  
  if ((L=obs2D.size())>0)
    for (cpt=0;cpt<L;cpt++)
      delete obs2D[cpt];
  
      }
      
      // 
      // File Reader
      //
      //=============
      void initFromFile (const bundleParameters &bP);

      //
      // Norm Tools
      //
      //============
      double residualWL2Norm ();
      
      double residualL2Norm ();
      

    }; // class observationDataBase

    // Print observation
    
    std::ostream& operator <<(std::ostream& s, jafar::bundle::observation const& o_);    


  } // namespace bundle

} // namespace jafar



#endif