linearsys.hpp

#ifndef BUNDLE_LINEARSYS_HPP
#define BUNDLE_LINEARSYS_HPP

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

#include "jmath/matofmat.hpp"
#include "jmath/precond.hpp"
#include "jmath/pcg.hpp"
#include "jmath/ublasExtra.hpp" 

#include "bundle/bundleData.hpp"
#include "bundle/bundleVariables.hpp"

namespace jafar {
  
  namespace bundle {

    class bundleLinearSys {
      
    public : 
      
      jblas::vec gradient;
 
      jblas::vec deltap;
      
    private :
      
      // Not commented. All next fields correspond to variables used 
      // to compute coefficients o the system linear and to resolve it

      unsigned int nbCam;
      
      unsigned int nbObj3D;
      
      unsigned int nbObs2D;
      
      jmath::matofmat<double,2,6> B1;
      
      jmath::matofmat<double,2,3> B2;
      
      std::vector<jblas::mat66> U;
      
      std::vector<jblas::mat33> V;
      
      std::vector<jblas::mat33> invV;
      
      jblas::mat Y;
      
      std::vector<jblas::mat_range*> rangeY;

      jblas::vec T;
      
      std::vector<jblas::vec_range*> rangeT;
      
      jblas::mat S;

      std::vector<jblas::mat_range*> rangeS;
      
      jblas::mat D;
      
      std::vector<jblas::mat_range*> rangeD;
      
      std::vector<jblas::vec_range*> gradCpdv;
      
      std::vector<jblas::vec_range*> grad3d;
      
      jblas::vec_range dpcpdv;
      
      jblas::vec_range dp3d;
      
      std::vector<jblas::vec_range*> dp1cpdv;
      
      std::vector<jblas::vec_range*> dp1obj3d;

      
    public : 
      
      //
      // Constructor
      //
      //=============
      
      bundleLinearSys (const observationsContainer obsCon_) :
  gradient(6*obsCon_.pbDim.nbCam + 3*obsCon_.pbDim.nbObj3d),
  deltap(6*obsCon_.pbDim.nbCam + 3*obsCon_.pbDim.nbObj3d),
  nbCam(obsCon_.pbDim.nbCam),
  nbObj3D(obsCon_.pbDim.nbObj3d),
  nbObs2D(obsCon_.pbDim.nbObs2d),
  B1(obsCon_.sparseSB1),
  B2(obsCon_.sparseSB2),
  U(nbCam),
  V(nbObj3D),
  invV(nbObj3D),
  Y(nbCam*6,3*nbObj3D),
  rangeY(nbCam*nbObj3D),
  T(6*nbCam),
  rangeT(nbCam),
  S(6*nbCam,6*nbCam),
  rangeS(nbCam*nbCam),
  D(6*nbCam,3*nbObj3D),
  rangeD(nbCam*nbObj3D),
  gradCpdv(nbCam),
  grad3d(nbObj3D),
  dpcpdv(deltap,ublas::range(0,6*nbCam)),
  dp3d(deltap,ublas::range(6*nbCam,6*nbCam+3*nbObj3D)),
  dp1cpdv(nbCam),
  dp1obj3d(nbObj3D)
      {
  unsigned int cptcam, cptcam2, cptObj3d, indice;
  
  
  /* Allocation for rangeT, gradCPDV and dp1CPDV */
  for (cptcam=0;cptcam<nbCam;cptcam++)
    {
      ublas::range rglocal(cptcam*6,(cptcam+1)*6);
      rangeT[cptcam] = new jblas::vec_range(T,rglocal);
      gradCpdv[cptcam] = new jblas::vec_range(gradient,rglocal);
      dp1cpdv[cptcam] = new jblas::vec_range(deltap,rglocal);
    }
      
  /* Allocation for grad3d and dp1obj3d */
  for (cptObj3d=0;cptObj3d<nbObj3D;cptObj3d++)
    {
      ublas::range rglocal(6*nbCam+cptObj3d*3,6*nbCam+(cptObj3d+1)*3);
      grad3d[cptObj3d] = new jblas::vec_range(gradient,rglocal);
      dp1obj3d[cptObj3d] = new jblas::vec_range(deltap,rglocal);
    }
  
  /* Allocation for rangeS */
  for (indice=0,cptcam=0;cptcam<nbCam;cptcam++)
    {
      ublas::range rglocal(cptcam*6,(cptcam+1)*6);
      for (cptcam2=0;cptcam2<nbCam;cptcam2++,indice++)
        {
    ublas::range rglocal2(cptcam2*6,(cptcam2+1)*6);
    rangeS[indice] = new jblas::mat_range(S,rglocal,rglocal2);
        }
    }
  
  /* Allocation for rangeD and rangeY */
  for ( indice=0, cptcam=0; cptcam<nbCam ; ++cptcam)
    {
      ublas::range rglocal(cptcam*6,(cptcam+1)*6);
      for (cptObj3d=0; cptObj3d<nbObj3D ; ++cptObj3d, ++indice)
        {
    ublas::range rglocal2(cptObj3d*3,(cptObj3d+1)*3);
    rangeY[indice] = new jblas::mat_range(Y,rglocal,rglocal2);
    rangeD[indice] = new jblas::mat_range(D,rglocal,rglocal2);
        }
    }
  
      };
      
      //
      // Destructor
      //
      //============
      ~bundleLinearSys ()
      {       
  unsigned int cpt;     
      
  for (cpt=0;cpt<nbCam;cpt++)
    {
      delete rangeT[cpt];
      delete gradCpdv[cpt];
      delete dp1cpdv[cpt];
    }

  for (cpt=0;cpt<nbObj3D;cpt++)
    {
      delete grad3d[cpt];
      delete dp1obj3d[cpt];
    }


  for (cpt=0;cpt<nbCam*nbCam;cpt++)
    delete rangeS[cpt];
      }             
  
      //
      // Computation methods
      //
      //=====================
      
      void afficheB1(unsigned int indObs, unsigned int indVue)
      {
  std::cout << "Elem de B1 en (" << indObs << ", "<< indVue << ") : " << B1(indObs,indVue) << std::endl;
      }

      void afficheB2(unsigned int indObs, unsigned int indObj)
      {
  std::cout << "Elem de B2 en (" << indObs << ", "<< indObj << ") : " << B2(indObs,indObj) << std::endl;
      }
      
      template <typename typeCam>
      void processData (bundle::observationsContainer &bobs, bundle::bundleVariables<typeCam> &bvar)
      {
  JFR_PRECOND(nbObs2D==bobs.obs2D.size(),"bundle::linearSys : Error number of 2D observation don't match");

  unsigned int cpt, indView, indObj3d;
  unsigned int nbObsCPDV = bobs.obsCPDV.size(), nbObs3D = bobs.obs3D.size();  
  unsigned int cptcam, indice, cptObj3d;
  unsigned int nbvariables = 6*nbCam+3*nbObj3D;

  jblas::mat jacCPDV(2,6);
  jblas::mat jacOBJ3D(2,3);

  jblas::mat mat66tmp(6,6);
  jblas::mat mat33tmp(3,3);
  jblas::mat mat22tmp(2,2);

  jblas::mat mat63tmp(6,3);
  jblas::mat mat26tmp(2,6);
  jblas::mat mat23tmp(2,3);

  jblas::vec vec2tmp(2), vec2tmp2(2);
  jblas::vec vec3tmp(3);
  jblas::vec vec6tmp(6);


  
  // First Step :: compute non zero elements of B1 and B2
  
  for (cpt=0;cpt<nbObs2D;cpt++)
    {
      indView = (*(bobs.obs2D[cpt])).idView;
      indObj3d = (*(bobs.obs2D[cpt])).idFeature;      

      bvar.projectionJac (indObj3d,indView,jacCPDV,jacOBJ3D);         
      B1(cpt,indView) = jacCPDV;
      B2(cpt,indObj3d) = jacOBJ3D;
    }       

  // Second Step :: zero-initialization of vectors and matrices

  /* Initialisation de Y */
  for (unsigned int i=0;i<6;i++)
    for (unsigned int j=0; j<3;j++)
      mat63tmp(i,j)=0.0;
  for (cptcam=0;cptcam<nbCam;cptcam++)
    for (cptObj3d=0;cptObj3d<nbObj3D;cptObj3d++)
      (*(rangeY[cptcam*nbObj3D+cptObj3d])).assign(mat63tmp);
  
  /* Initialisation de U */
  for (unsigned int i=0;i<6;i++)
    for (unsigned int j=0;j<6;j++)
      mat66tmp(i,j) = 0.0;      
  for(indice=0;indice<nbCam;indice++)
    U[indice].assign(mat66tmp); 
  
  /* Initialisation de V */ 
  for (unsigned int i=0;i<3;i++)
    for (unsigned int j=0;j<3;j++)
      mat33tmp(i,j) = 0.0;      
  for (indice=0;indice<nbObj3D;indice++)
    V[indice].assign(mat33tmp); 
  
  /* Initialisation des vecteurs */
  for (indice=0;indice<nbvariables;indice++)
    {
      gradient(indice) = 0.0;
      deltap(indice) = 0.0;
    }



  // Third Step :: compute JtJ and gradient
  
  if (nbObsCPDV)    
    for (cpt=0;cpt<nbObsCPDV;cpt++)
      {
        indView = (*(bobs.obsCPDV[cpt])).idView;
        
        (*(bobs.obsCPDV[cpt])).getCovMatrix(mat66tmp);
        U[indView].assign(mat66tmp);      
        (*(bobs.obsCPDV[cpt])).getResidual(vec6tmp);            
        *(gradCpdv[indView]) = prod(mat66tmp,vec6tmp);                
      }         
  
  
  if (nbObs3D)    
    for (cpt=0;cpt<nbObs3D;cpt++)
      {
        indObj3d = (*(bobs.obs3D[cpt])).idFeature;
        
        (*(bobs.obs3D[cpt])).getCovMatrix(mat33tmp);
        V[indObj3d].assign(mat33tmp);
        (*(bobs.obs3D[cpt])).getResidual(vec3tmp);
        *(grad3d[indObj3d]) = prod(mat33tmp,vec3tmp);     
        
      } 
  

  for (cpt=0;cpt<nbObs2D;cpt++)
    {
      indView = (*(bobs.obs2D[cpt])).idView;
      indObj3d = (*(bobs.obs2D[cpt])).idFeature;

      (*(bobs.obs2D[cpt])).getCovMatrix(mat22tmp);
      (*(bobs.obs2D[cpt])).getResidual(vec2tmp);
    
      /* Elements of the matrix A (see Lourakis's TR) */
      
      mat26tmp = prod(mat22tmp,B1(cpt,indView));
        U[indView] += prod(trans(B1(cpt,indView)),mat26tmp);
           
      mat23tmp  = prod(mat22tmp,B2(cpt,indObj3d));
      V[indObj3d] += prod(trans(B2(cpt,indObj3d)),mat23tmp);
      *(rangeY[indView*nbObj3D+indObj3d]) += prod(trans(B1(cpt,indView)),mat23tmp);

      /* Elements of Gradient */      
      vec2tmp2 = prod(mat22tmp,vec2tmp);
      *(grad3d[indObj3d]) += prod(trans(B2(cpt,indObj3d)),vec2tmp2);
      *(gradCpdv[indView]) += prod(trans(B1(cpt,indView)),vec2tmp2);

    } 
  
      };
      
      double maxDiagA ()
      {
  double maxValue = -1.369e10;
  unsigned int matUiSize = U[0].size1();
  unsigned int matViSize = V[0].size1();
  
  // Search in U
  for ( unsigned int cpt=0; cpt < nbCam ; ++cpt)
    for ( unsigned int cpt2=0 ; cpt2 < matUiSize; ++cpt2 )
      maxValue = (maxValue>U[cpt](cpt2,cpt2)) ? maxValue : U[cpt](cpt2,cpt2);
  
  // Search in V
  for ( unsigned int cpt=0; cpt < nbObj3D ; ++cpt)
    for ( unsigned int cpt2=0 ; cpt2 < matViSize; ++cpt2 )
      maxValue = (maxValue>V[cpt](cpt2,cpt2)) ? maxValue : V[cpt](cpt2,cpt2);
  
  return maxValue;
      }

      void solveByLU (double mu=0.0)
      {
  
  // First step :: Forward substitution
  //
  // Fill S and T
  bundle_forward_substitution(mu);

  // Secont Step :: Solve the linear system Sx=T
  //
  // Remarks :
  //  1 - No recopy of S-matrix because we don't need contained data
  //  2 - No recopy of T-matrix for the same reason
  ublas::permutation_matrix<std::size_t> pmatrix(S.size1());  
  lu_factorize(S,pmatrix);  
  lu_substitute(S, pmatrix, T);
  
  
  // At this point T contained dpCPDV.
  // We must copy T in dpCPDV       
  dpcpdv = T;
    
  
  // Third Step :: Backward substitution
  //
  // Compute dp3d;
  bundle_backward_substitution();         
    
      };
      

      void solveByGC (double mu=0.0)
      {
  
  // First step :: Forward substitution
  //
  // Fill S and T
  bundle_forward_substitution(mu);

  // Secont Step :: Solve the linear system Sx=T
  CholeskyPreconditioner<jblas::mat> precond(S);
  jblas::vec x(dpcpdv);
  pcg_solve(S, x, T, 
      precond, 
      500,
      1e-10,
      1e-20);
  
  // At this point x contained new dpCPDV
  dpcpdv = x;
  
  
  // Third Step :: Backward substitution
  //
  // Compute dp3d;
  bundle_backward_substitution();         
      }
     

    private :          

      void bundle_forward_substitution (double const mu)
      {
  unsigned int cptcam, cptobj;
  unsigned int cptS1;

  jblas::mat33 mattemp, augmentationV;
  jblas::mat66 augmentationU;

  jblas::identity_mat id3(3);
  jblas::identity_mat id6(6);             
  
  augmentationV = mu*id3;
  augmentationU = mu*id6;

  // Initialisation de T et de S
  for (cptcam=0;cptcam<6*nbCam;cptcam++)
    {     
      T[cptcam] = 0.0;
      for (cptobj=0;cptobj<6*nbCam;cptobj++) 
        S(cptcam,cptobj) = 0.0;
    }
         
  /* Inversion of augmented V-vector */
  for (cptobj=0;cptobj<nbObj3D;cptobj++)
    { 
      mattemp = V[cptobj]+augmentationV;
      jmath::ublasExtra::details::inv3(mattemp,invV[cptobj]);     
    }     

  /* Generate T */
  for ( cptcam=0 ; cptcam<nbCam ; cptcam++ )
    *(rangeT[cptcam]) = *(gradCpdv[cptcam]);
  
  for ( cptcam = 0 ; cptcam<nbCam ; cptcam++)
    for ( cptobj = 0 ; cptobj<nbObj3D ; cptobj++)
      { 
        *(rangeD[cptcam*nbObj3D+cptobj]) = prod(*(rangeY[cptcam*nbObj3D+cptobj]),invV[cptobj]);      
        *(rangeT[cptcam]) -= prod(*(rangeD[cptcam*nbObj3D+cptobj]),*(grad3d[cptobj]));
      } 

  /* Generate S */       
  jblas::mat matTY(Y.size2(),Y.size1());
  
  for (cptS1=0;cptS1<nbCam;cptS1++)
    *(rangeS[cptS1*(nbCam+1)]) = U[cptS1] + augmentationU;
  
//  D.exportFullMatrix(matD);
//  Y.exportFullMatrix(matY);

  matTY = trans(Y); 
  S -= prod(D,matTY);
  
      };


      void bundle_backward_substitution ()
      {
  unsigned int cptobj, cptcam;
  jblas::vec vtemp(3);

  for ( cptobj=0 ; cptobj<nbObj3D ; ++cptobj )
    {
      vtemp = *(grad3d[cptobj]);
      for ( cptcam=0 ; cptcam<nbCam ; ++cptcam )
        vtemp -= prod(trans(*(rangeY[cptcam*nbObj3D+cptobj])),*(dp1cpdv[cptcam]));
      *(dp1obj3d[cptobj]) = prod(invV[cptobj],vtemp);
      
    } 
      };
      
    
    }; // end class bundleLinearSys       
  
  } // end namespace bundle
  
} // end namespace jafar

#endif