bundleVariables.hpp

#ifndef BUNDLE_BUNDLEVARIABLES_HPP
#define BUNDLE_BUNDLEVARIABLES_HPP

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

#include "kernel/jafarMacro.hpp"
#include "jmath/jblas.hpp"

#include "camera/cameraBarreto.hpp"
#include "camera/cameraPinhole.hpp"

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

#include "geom/t3dEuler.hpp"
#include "geom/t3dPointTools.hpp"


namespace jafar {

  namespace bundle {

    template <typename Sensor>
    class bundleVariables {
      
      bundleDimension pbDim;    
      
      Sensor Cam;
      
      std::vector<jafar::geom::T3DEuler> vecT3D;
      
      std::vector<jblas::vec3> obj3dinCamFrame;

      jblas::vec VoV; // vector of variables 

      std::vector<jblas::vec_range*> oneCamVov;
      
      std::vector<jblas::vec_range*> oneObjVov;
           
      jblas::vec_range motPartVov;
      
      jblas::vec_range strPartVov;

      jafar::geom::T3DEuler Rob2Cam; 


    public :
      //
      // Constructors
      //
      //==============

      bundleVariables (bundleParameters const &bP) :
  pbDim(bP.bDim),
  Cam (),
  vecT3D(pbDim.nbCam),
  obj3dinCamFrame(pbDim.nbCam*pbDim.nbObj3d),
  VoV(pbDim.nbCam*pbDim.cpdvSize+3*pbDim.nbObj3d),
  oneCamVov(pbDim.nbCam),
  oneObjVov(pbDim.nbObj3d),
  motPartVov(VoV,ublas::range(0,pbDim.nbCam*pbDim.cpdvSize)),
  strPartVov(VoV,ublas::range(pbDim.nbCam*pbDim.cpdvSize,VoV.size())),
  Rob2Cam()
      {
  double pR2C[vecT3D[0].size()];
  
  // 
  // Extended Intrinsic Parameters
  //
  Cam.load(bP.camParamFile);

  // Open bP.robToCamFile
  std::ifstream infile(bP.robToCamFile.c_str());
  
  if (infile.is_open())
    {
      // get info from bP.robToCamFIle
      infile.read((char*)pR2C,vecT3D[0].size()*sizeof(double));
      
      // Close file
      infile.close();                
      
      // Set Values of Rob2Cam
      Rob2Cam.setValue(pR2C[0],pR2C[1],pR2C[2],pR2C[3],pR2C[4],pR2C[5]);
          
    }
  else
    {
      JFR_WARNING("Rob2Cam is set to default because of unreachable file");
      Rob2Cam.setValue(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
    }

  //
  // Init vector of pointers on vec_range
  // 
  for (unsigned int cpt=0; cpt<pbDim.nbCam; cpt++)
    oneCamVov[cpt] = new jblas::vec_range(VoV,ublas::range(cpt*pbDim.cpdvSize,(cpt+1)*pbDim.cpdvSize));
  
  unsigned int decalage = pbDim.nbCam*pbDim.cpdvSize;
  
  for (unsigned int cpt=0; cpt<pbDim.nbObj3d; cpt++)
    oneObjVov[cpt] = new jblas::vec_range(VoV,ublas::range(decalage+3*cpt,decalage+3*(cpt+1)));   
  
      };
      
      bundleVariables (bundleVariables const& bv_) :
  pbDim(bv_.pbDim),
  Cam(bv_.Cam),
  vecT3D(bv_.vecT3D),
  obj3dinCamFrame(bv_.obj3dinCamFrame),
  VoV(bv_.VoV),
  oneCamVov(pbDim.nbCam),
  oneObjVov(pbDim.nbObj3d),
  motPartVov(VoV,ublas::range(0,pbDim.nbCam*pbDim.cpdvSize)),
  strPartVov(VoV,ublas::range(pbDim.nbCam*pbDim.cpdvSize,VoV.size())),
  Rob2Cam(bv_.Rob2Cam)  
      {
  //
  // Init vector of vec_range
  // 
  for (unsigned int cpt=0; cpt<pbDim.nbCam; cpt++)
    oneCamVov[cpt] = new jblas::vec_range(VoV,ublas::range(cpt*pbDim.cpdvSize,(cpt+1)*pbDim.cpdvSize));
  
  unsigned int decalage = pbDim.nbCam*pbDim.cpdvSize;
  
  for (unsigned int cpt=0; cpt<pbDim.nbObj3d; cpt++)
    oneObjVov[cpt] = new jblas::vec_range(VoV,ublas::range(decalage+3*cpt,decalage+3*(cpt+1)));   
      };

      //
      // Accessors
      //
      //===========

      bool getCPDV (unsigned int indCam, jblas::vec &cpdvVector)
      {
  JFR_PRECOND(indCam<vecT3D.size(),"bundleVariables::getCPDV - indCam is out of range of vecT3D");
  JFR_PRECOND(cpdvVector.size()==vecT3D[0].size(),"bundleVariables::getCPDV - cpdvVector has an invalid size");
  
  cpdvVector.assign(vecT3D[indCam].getX());

  // Manque un test pour comparer contenu de la T3D et de VoV!
  
  return(true);
      }

      bool getObj3D (unsigned int indObj3D, jblas::vec &objVector)
      {
  JFR_PRECOND(indObj3D<pbDim.nbObj3d,"bundleVariables::getObj3D - Out of range");
  JFR_PRECOND(objVector.size()==3,"bundleVariables::getObj3D - The objVector size is not 3");

  
  objVector.assign(*(oneObjVov[indObj3D]));
  return true;
      };

 
      //
      // Mutators
      //
      //==========      

      void update (jblas::vec const& dp)
      {
  JFR_PRECOND(dp.size()==VoV.size(),"bundleVariables::update - deltap has an invalid size");
  
  unsigned int cptCam;
  
  // First step : add dp to VoV
  // WARNING : if rotation parametrization uses quaternion
  //           we can use a simple addition
  
  VoV += dp;
  
  // Second step : update the T3DEuler
  // WARNING : For Quaternion, it will be different
  for (cptCam=0;cptCam<pbDim.nbCam;cptCam++)
    vecT3D[cptCam].setValue((*(oneCamVov[cptCam]))[0],(*(oneCamVov[cptCam]))[1],(*(oneCamVov[cptCam]))[2],
          (*(oneCamVov[cptCam]))[3],(*(oneCamVov[cptCam]))[4],(*(oneCamVov[cptCam]))[5]);   
      };


      void update (bundleVariables const& bv_)
      {
  JFR_PRECOND(bv_.pbDim.nbCam == pbDim.nbCam,"bundleVariables::update - Invalid number of cameras");
  JFR_PRECOND(bv_.pbDim.nbObj3d == pbDim.nbObj3d,"bundleVariables::update - Invalid number of 3D objects");
  JFR_PRECOND(bv_.VoV.size() == VoV.size(),"bundleVariables:update - Invalid sizw of the new state vector");  
  
  unsigned int cptCam = 0;
  
  // Copy of T3D
  for (cptCam=0;cptCam<pbDim.nbCam;cptCam++)
    vecT3D[cptCam].assign(bv_.vecT3D[cptCam]);
  
  // Copy of data vector
  VoV.assign(bv_.VoV);
  
      };


      void initFromFile (bundle::bundleParameters const &bP)
      {
  
  std::ifstream infile;
  double vtemp[pbDim.cpdvSize], vtemp2[3];
  int numIma;
  
  // Init CPDV  
  infile.open(bP.initCpdvFile.c_str(),std::ios::binary | std::ios::in);
    
  if (infile.is_open())
    {     
      for (unsigned int cpt=0 ; cpt<pbDim.nbCam ; cpt++)
        {
    infile.read((char*)&numIma,sizeof(unsigned int));
    infile.read((char*)vtemp,pbDim.cpdvSize*sizeof(double));      
    
    vecT3D[cpt].setValue(vtemp[0],vtemp[1],vtemp[2],vtemp[3],vtemp[4],vtemp[5]);
    
    for (unsigned int cptt=0 ; cptt<pbDim.cpdvSize ; ++cptt)
      (*(oneCamVov[cpt]))[cptt] = vtemp[cptt];        
    
        }
    }

  infile.close();


  // Init Structure
  infile.open(bP.initObj3dFile.c_str(),std::ios::binary | std::ios::in);
  
  if (infile.is_open())
    {
      for (unsigned int cpt=0 ; cpt<pbDim.nbObj3d ; cpt++)
        {
    infile.read((char*)&numIma,sizeof(unsigned int));
    infile.read((char*)vtemp2,3*sizeof(double));

    for (unsigned int cptt=0 ; cptt<3 ; ++cptt)
      (*(oneObjVov[cpt]))[cptt] = vtemp2[cptt];   
        }
    }
  
  
  
  infile.close();
      };      


      double L2normP ()
      {
  return ublas::norm_2(VoV);
      }
      

      //
      // Projection and Jacobian computing methods
      // 
      //===========================================
      
      void projection (unsigned int const n, unsigned int const k, jblas::vec &projnk)
      {
  JFR_PRECOND(projnk.size()==2,"bundleVariables::projection - Invalid output vector size. Must be equal 2");
  JFR_PRECOND(n<pbDim.nbObj3d,"bundleVariables::projection - You try to project an unknown 3D features");
  JFR_PRECOND(k<pbDim.nbCam,"bundleVariables::projection - You try to project a 3D features on an unknown camera.");
  
  jblas::vec3 vtmp1, vtmp2;
  
  vtmp1 = (*oneObjVov[n]);


  /* First  Step : From World Frame to Robot Frame */
  geom::t3d::pointToFrame<jblas::vec3,jblas::vec3>(vecT3D[k],vtmp1,vtmp2);

  
  /* Second Step : From Robot Frame to Camera Frame */
  geom::t3d::pointToFrame<jblas::vec3,jblas::vec3>(Rob2Cam,vtmp2,vtmp1);          
  obj3dinCamFrame[n*pbDim.nbCam+k].assign(vtmp1);

    
  /* Third  Step : Central projection */
  Cam.project(vtmp1,projnk);
  
      };

      void projectionJac (unsigned int const n, unsigned int const k, jblas::mat &jacCPDVnk, jblas::mat &jacOBJ3Dnk)
      {
  JFR_PRECOND(jacCPDVnk.size1()==2 && jacCPDVnk.size2()==vecT3D[0].size(),"bundleVariables::projectionJac - jacCPDVnk has an invalid size.");
  JFR_PRECOND(jacOBJ3Dnk.size1()==2 && jacOBJ3Dnk.size2()==3,"bundleVariables::projectionJac - jacOBJ3Dnk has an invalid size.");
         

  jblas::mat matTMP1(3,6), matTMP2(3,3);
  jblas::mat matCPDV(3,6), matOBJ3D(3,3);       
  jblas::mat mat2D(2,3);
  
  jblas::mat33 Rr2c, tRr2c; // pour stocker la matrice de rotation de la transfo Rob2Cam
  
  /* First Step  : Jacobian matrix X|rob w.r.t. X|w and CPDV */ 
  geom::t3d::pointToFrameJacEuler(vecT3D[k].getX(),*(oneObjVov[n]),matTMP1,matTMP2);
  
  /* Second Step : Combinaison with Rr2c */
  Rr2c = Rob2Cam.getR();
  tRr2c = trans(Rr2c);
  matCPDV = prod(tRr2c,matTMP1);
  matOBJ3D = prod(tRr2c,matTMP2);

  /* Third Step  : Combinaison with Jacobian matrix */
  Cam.projectJac(obj3dinCamFrame[n*pbDim.nbCam+k],mat2D);
  jacCPDVnk  = prod(mat2D,matCPDV);
  jacOBJ3Dnk = prod(mat2D,matOBJ3D);

      };

      void exportInFile (bundleParameters bP)
      {
  
  std::ofstream outfile;

  // Export 3D position
  outfile.open(bP.outObj3dFile.c_str(),std::ios::binary | std::ios::out);
  
  if (outfile.is_open())      
    {
      for (unsigned int cptObj=0 ; cptObj<pbDim.nbObj3d ; ++cptObj)
        {
    outfile.write((char*)&cptObj,sizeof(unsigned int));
    
    for (unsigned int k=0;k<3;k++)
      outfile.write((char*)&((*(oneObjVov[cptObj]))[k]),sizeof(double));
    
        }
      outfile.close();
    }
  
  // Export extrinsic parameters
  outfile.open(bP.outCpdvFile.c_str(),std::ios::binary | std::ios::out);
  
  if (outfile.is_open())      
    {
      for (unsigned int cptCam=0 ; cptCam<pbDim.nbCam ; ++cptCam)
        {
    outfile.write((char*)&cptCam,sizeof(unsigned int));

    for (unsigned int k=0;k<pbDim.cpdvSize;k++)
      outfile.write((char*)&((*(oneCamVov[cptCam]))[k]),sizeof(double));

        }
      outfile.close();   
    }
  
      };
      

    };

  } // namespace bundle

} // namespace jafar 

#endif