bundle.hpp

#ifndef BUNDLE_BUNDLE_HPP
#define BUNDLE_BUNDLE_HPP

#include <iostream>
#include <string>
#include <cstdlib>

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

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

#include "boost/timer.hpp"

namespace jafar {
  
  namespace bundle {    

    template <typename CamType>
    void computeResidual (observationsContainer &obs, bundleVariables<CamType>  &variables)
    {     

      jblas::vec v2tmp(2), v3tmp(3), v6tmp(6);
      
      // Compute 2D Residuals
      for (unsigned int cpt =0; cpt<obs.obs2D.size() ; cpt++)
  {   
    variables.projection((*(obs.obs2D[cpt])).idFeature,(*(obs.obs2D[cpt])).idView,v2tmp);
    (*(obs.obs2D[cpt])).setResidual(v2tmp);       
  }

      // Compute 3D structure  Residuals
      for (unsigned int cpt =0; cpt<obs.obs3D.size() ; cpt++)
  {   
    variables.getObj3D((*(obs.obs3D[cpt])).idFeature,v3tmp);
    (*(obs.obs3D[cpt])).setResidual(v3tmp);       
  }

      // Compute Motion Residuals
      for (unsigned int cpt =0; cpt<obs.obsCPDV.size() ; cpt++)
  {   
    variables.getCPDV((*(obs.obsCPDV[cpt])).idView,v6tmp);
    (*(obs.obsCPDV[cpt])).setResidual(v6tmp);       
  }    

    }
          

    template <typename CamType>
    void bundleGN (std::string paramFile)
    {
      
      boost::timer t1, t2;
      
      bool stop=false;
      double currentcritvalue, tps;
      double DPnorm = 0.0, Pnorm = 0.0, Ginfnorm = 0.0; 
      int LmIterCpt=0;             
      std::stringstream msg;

      // 
      // Load experience description
      //
      bundleParameters bparam;
      bparam.load(paramFile);
      bparam.bDim.cpdvSize = 6;
      bparam.check();

      //
      // Load initial values of parameters 
      // 
      bundleVariables<CamType> Bvar(bparam);
      Bvar.initFromFile(bparam);      

      //
      // Load observations
      //
      observationsContainer Bobs(bparam.bDim); 
      Bobs.initFromFile(bparam);

      // 
      // 1st Step : Initialisation of Linearsys
      // 
      
      computeResidual<CamType>(Bobs,Bvar);
      currentcritvalue = Bobs.residualWL2Norm();
      bundleLinearSys BLS(Bobs);           

      t2.restart();

      std::cout.setf(std::ios_base::dec);
      std::cout.unsetf(std::ios_base::showbase);
      std::cout << std::endl;
      std::cout << std::endl;
      std::cout << "|  #  "; // 5 -2 : 3
      std::cout << "|    critere    "; // 15 - 2 : 13
      std::cout << "| normL2(gradient) "; // 18 - 2 : 16
      std::cout << "|  normL2(deltap)  "; // 18 -2 : 16
      std::cout << "| Tps execution |" << std::endl; //15 -2 : 13
      std::cout << "|-----|---------------|------------------|------------------|---------------|" << std::endl;
                 
      for ( LmIterCpt=1 ; (!stop) && (LmIterCpt<=bparam.nbIterLmMax) ; LmIterCpt++)    
  {

    t1.restart();
    BLS.processData(Bobs,Bvar);         
    if (ublas::norm_inf(BLS.gradient)<=bparam.eps1)
      {
        msg << " Gradient infinite norm is too small : " << Ginfnorm << " for " << bparam.eps1 << std::endl;
        stop = true;
        break;
      }
    
    BLS.solveByGC();
      
    
    DPnorm = ublas::norm_2(BLS.deltap);
    Pnorm = Bvar.L2normP();
    
    Bvar.update(BLS.deltap);
    computeResidual<CamType>(Bobs,Bvar);
    
    tps = t1.elapsed();

    // Formatted output
    
    std::cout << "| ";
    std::cout.width(3);
    std::cout.setf(std::ios_base::right);
    std::cout.setf(std::ios_base::scientific);
    std::cout.precision(7);
    std::cout << LmIterCpt <<" | ";

    std::cout.width(13);    
    std::cout << currentcritvalue << " | ";

    std::cout.width(15);
    std::cout.precision(10);
    std::cout << ublas::norm_2(BLS.gradient)  << " | ";
    
    std::cout.precision(10);
    std::cout << DPnorm << " | ";
    
    std::cout.setf(std::ios_base::fixed);
    std::cout.width(9);
    std::cout << tps << " sec |" << std::endl;
    std::cout.unsetf(std::ios_base::fixed);       
    
    currentcritvalue = Bobs.residualWL2Norm();     
    
    if ( DPnorm <= (bparam.eps2*Pnorm) )
      {
        msg << " Change in p is too small : " << DPnorm << " for " << bparam.eps2*Pnorm << std::endl;
        stop = true;
        break;
      }
    
  } // Loop on the  iteration

      tps = t2.elapsed();

      std::cout << "|-----|---------------|------------------|------------------|---------------|\n" << std::endl;
      
      std::cout << msg.str() << std::endl;
      
      std::cout.setf(std::ios_base::fixed);
      std::cout.width(9);
      std::cout << " Optimization time : " << tps << " sec." << std::endl;
      std::cout.unsetf(std::ios_base::fixed);

      // Export Results
      Bvar.exportInFile(bparam);

    };

    template <typename CamType>
    void bundleLM (std::string paramFile)
    {
      
      boost::timer t1;
      boost::timer t2;

      bool stop = false;
      double rho, nu = 2.0, mu =0.0, currentcritvalue, newcritvalue, rhoDenom, rhoNum, test1, tau = 1.0e-3;
      double nuMax = pow(2.0,15)+1.0;
      double DPnorm = 0.0, Pnorm = 0.0, tps; 
      double Ginfnorm = 0.0;
      int LmIterCpt=0;        
      
      std::stringstream msg;

      // 
      // Load experience description
      //
      bundleParameters bparam;
      bparam.load(paramFile);
      bparam.bDim.cpdvSize = 6;
      bparam.check();

      //
      // Load initial values of parameters 
      // 
      bundleVariables<CamType> Bvar(bparam);
      Bvar.initFromFile(bparam);      
      bundleVariables<CamType> BvarNew(Bvar);

      //
      // Load observations
      //
      observationsContainer Bobs(bparam.bDim); 
      Bobs.initFromFile(bparam);


      // Create LinearSys object
      bundleLinearSys BLS(Bobs);
      
            
      jblas::vec mudpg(BLS.deltap.size());
      

      // Compute residuals
      computeResidual<CamType>(Bobs,Bvar);
      currentcritvalue = Bobs.residualWL2Norm();

      t2.restart();

      std::cout.setf(std::ios_base::dec);
      std::cout.unsetf(std::ios_base::showbase);
      std::cout << std::endl;
      std::cout << std::endl;
      std::cout << "|  #  "; // 5 -2 : 3
      std::cout << "|    critere    "; // 15 - 2 : 13
      std::cout << "| normL2(gradient) "; // 18 - 2 : 16
      std::cout << "|  normL2(deltap)  "; // 18 -2 : 16
      std::cout << "| Tps execution |" << std::endl; //15 -2 : 13
      std::cout << "|-----|---------------|------------------|------------------|---------------|" << std::endl;

      
      for ( LmIterCpt=1 ; (LmIterCpt<=bparam.nbIterLmMax) && (!stop) ; LmIterCpt++ )
  {
      
    // Compute Jacobian, Jacobian'xJacobian and gradient
    t1.restart();
    BLS.processData(Bobs,Bvar); 
    Ginfnorm = ublas::norm_inf(BLS.gradient);
    if (Ginfnorm<=bparam.eps1)
      {
        msg << " Gradient infinite norm is too small : " << Ginfnorm << " for " << bparam.eps1 << std::endl;
        stop = true;
        break;
      }
    
    if ( LmIterCpt==1 )     
      mu = tau*BLS.maxDiagA();    
    
    // Damping parameter adaptation
    while (true)
      {
        BLS.solveByGC(mu);
       

        DPnorm = ublas::norm_2(BLS.deltap);
        Pnorm = Bvar.L2normP();
        
        // Test to break the iterative process
        // Warning : bundleVariables::L2normP return a ublas::norm_2 = sqrt(sum squared elements)
        //
        if ( DPnorm <= (bparam.eps2*Pnorm) )
    {
      msg << " Change in p is too small : " << DPnorm << " for " << bparam.eps2*Pnorm << std::endl;
      stop = true;
      break;
    }
        
        // to test if p+dp reduce global criterion we use a temporary copy of p       
        BvarNew.update(BLS.deltap);

        // We compute residuals for pnew = p+dp
        computeResidual<CamType>(Bobs,BvarNew);

        // We compute the new criterion value
        newcritvalue = Bobs.residualWL2Norm();
        
        // To accept the update, rhoNum and rhoDENOM must be strictly positive
        rhoNum = currentcritvalue-newcritvalue;
        mudpg = mu*BLS.deltap;
        mudpg += BLS.gradient;
        rhoDenom = ublas::inner_prod(BLS.deltap,mudpg);

        if ( (rhoNum>0.0) && (rhoDenom>0.0) )
    {             

      rho = rhoNum/rhoDenom;
      
      // we update the damping parameter
      test1 = 1.0-pow((2.0*rho-1.0),3.0);
      mu = (0.33333>test1)?mu/3.0:mu*test1;
      nu = 2.0;

      // we update p with pnew = p+dp
      Bvar.update(BvarNew);

      // Formatted output
      tps = t1.elapsed();
      std::cout << "| ";
      std::cout.width(3);
      std::cout.setf(std::ios_base::right);
      std::cout.setf(std::ios_base::scientific);
      std::cout.precision(7);
      std::cout << LmIterCpt <<" | ";
      
      std::cout.width(13);    
      std::cout << currentcritvalue << " | ";
      
      std::cout.width(15);
      std::cout.precision(10);
      std::cout << ublas::norm_2(BLS.gradient)  << " | ";
      
      std::cout.precision(10);
      std::cout << DPnorm << " | ";
      
      std::cout.setf(std::ios_base::fixed);
      std::cout.width(9);
      std::cout << tps << " sec |" << std::endl;
      std::cout.unsetf(std::ios_base::fixed);

      // we save the new criterion value
      currentcritvalue = newcritvalue;          

      // We have accepted the p-update. Then we must
      // go out the while-loop
      break;
    }
                        
        mu *= nu;
        nu *= 2.0;        
        
        if (nu>=nuMax)
    {
      msg << "Too many attemps to increase the damping factor" << std::endl;
      stop = true;
      break;
    }                                             
        
      } // The inner loop
            
  } // Levenberg Marquardt iteration
      
      tps = t2.elapsed();

      std::cout << "|-----|---------------|------------------|------------------|---------------|\n" << std::endl;
      
      std::cout << msg.str() << std::endl;
      
      std::cout.setf(std::ios_base::fixed);
      std::cout.width(9);
      std::cout << " Optimization time : " << tps << " sec." << std::endl;
      std::cout.unsetf(std::ios_base::fixed);

      // Export Results
      Bvar.exportInFile(bparam);
            
    };     
    
  } // end namespace bundle
  
} // end namespace jafar



#endif