Ccorrelation_opticalFlow.h

Go to the documentation of this file.

/**********************************************************************
 * Copyright (C) 2012, The YaDICs Project Developers. 
 * See the COPYRIGHT file at the top-level directory of this distribution ./COPYRIGHT.
 * See ./COPYING file for copying and redistribution conditions.
 * 
 * This file is part of YaDICs.
 * YaDICs is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * YaDICs is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with YaDICs.  If not, see <http://www.gnu.org/licenses/>.
 * 
 * Information about how to use the software are provided at http://yadic.univ-lille1.fr/
 **********************************************************************/


#ifndef CCORRELATION_OPTICALFLOW
#define CCORRELATION_OPTICALFLOW


#include "Ccorrelation.h"

template<typename T,typename Timg>
class Ccorrelation_opticalFlow: public Ccorrelation<T,Timg>
{
  
  public:
  
    std::string m_shape_type, m_interpLoop, m_interpFinal;
    double m_epsilon;
    std::vector<T> m_ref;

    Ccorrelation_opticalFlow() : Ccorrelation<T,Timg>()
    {// constructor
      
      this->class_name = "Correlation : Optical Flow";
      
    }

    virtual void init(CParameterNetCDF &fp)
    {// init from parameter file
      
      std::string attribute_name("shape");
      int error = fp.loadAttribute(attribute_name,m_shape_type);
      if (this->m_verbose){printf("\tShape function type : %s\n", m_shape_type.c_str());}
      
      attribute_name = "convergence_speed";
      error = fp.loadAttribute(attribute_name,m_epsilon);
      if (this->m_verbose){printf("\tLimit speed of convergence : %f\n", m_epsilon);}
      
      attribute_name = "interpolation_loop";
      m_interpLoop = "linear";
      error = fp.loadAttribute(attribute_name,m_interpLoop);
      if (this->m_verbose){printf("\tLoop interpolator : %f\n", m_interpLoop.c_str());}

      attribute_name = "interpolation_final";
      m_interpFinal = "linear";
      error = fp.loadAttribute(attribute_name,m_interpFinal);
      if (this->m_verbose){printf("\tFinal iterpolator : %f\n", m_interpFinal.c_str());}
      
      
    }
 
    virtual void exec(const Cimage<Timg> &oImage, const Cmesh<T,Timg> &oMesh, Cfield<T,Timg> &oField)
    {// exec optical flow correlation
    
      //-----------------------------------------------------------------------
      //monoprocessing asignments
      this->_3D = oMesh._3D;

      int iter(0); //number of iterations
      int converged(0); //convergence parameter
      T previous_res(1e10); //residue at previous step
      
      CImgList<T> K,Ke;//global and local tensors for linear system K[0].U=K[1] with K[0] = sum_(V.Vt) and K[1] = sum_((Img1(X)-Img2(X-U)).V)
      CImg<int> p(this->m_threadNB,1,1,1,0);
      CImgList<T> solution(oField.m_mode);//intermediate solution vector
      CImg<T> current_res(this->m_threadNB,1,1,1,0);//global mean residue
      assignG(oMesh, K,Ke);//global assignments
      //-----------------------------------------------------------------------
      
      #pragma omp parallel
      {//beginning of parallel section
  
        //-----------------------------------------------------------------------
        //multithread initializations
        //for elements
        CImg<Timg> list; //elementary pixel list
        
        //for pixels
        CImgList<T> Ve; //shape function to image gradient product at pix
        CImgList<T> N(oMesh.num_var(), oMesh.num_mod(),1,1,1, 0);//Shape function at pix
        T residue(0), deformed(0); //residue and corrected GL at pix
        std::vector<T> ref, offset, lCoor, gCoor, grad, U;
        ref.assign(3,0);offset.assign(3,0);lCoor.assign(3,0); gCoor.assign(3,0); U.assign(3,0); grad.assign(oMesh.num_var(),0);
        
        assignL(oMesh, Ve);
      //-----------------------------------------------------------------------
        
        //calcul init value of the metric
        metric(oImage,oMesh,solution,oField,N,Ve,Ke,ref,offset,lCoor,gCoor,U,grad,residue,deformed,list, p,current_res,K);
        

        do
        {//while the convergency velocity is higher than a certain level

          #pragma omp single
          {
            solve(K,oMesh,solution);
            if (this->m_medianFilter){this->regularize(oMesh, oImage, solution);}
          }

          metric(oImage,oMesh,solution,oField,N,Ve,Ke,ref,offset,lCoor,gCoor,U,grad,residue,deformed,list, p,current_res,K);

          #pragma omp single
          {
            test(solution, m_epsilon, oField, previous_res, iter, converged);
          }

        }
        while(!converged);
      
        if (oImage.m_storeActiv)
        {export_res(oImage,oMesh,oField, N,ref,offset,lCoor,gCoor,U,residue,deformed,list);}
        
      }// pragma omp parallel
      
    }
        
    virtual void assignG(const Cmesh<T,Timg> &oMesh, CImgList<T> &K, CImgList<T> &Ke)
    {//assign approriate size for Ke and Fe tensors

        K.assign(2);
        Ke.assign(2);

    }

    virtual void assignL(const Cmesh<T,Timg> &oMesh, CImgList<T> &Ve) = 0;

    virtual void reset(const int &thread, CImgList<T> &Ke)
    {//reset tensors
    
      cimglist_for(Ke, var)
      {
        cimg_forXYZ(Ke[var], x,y,z)
        {
          Ke[var](x,y,z,thread) = 0;
        }
      }
      
    }
    
    virtual void evalGN(const std::vector<T> &grad,const CImgList<T> &N, CImgList<T> &Ve) = 0;
    
    virtual void e_ref(const Cimage<Timg> &oImage, const Cmesh<T,Timg> &oMesh, const int &elem, std::vector<T> &ref) = 0;
    
    virtual void evalU(const CImgList<T> &solution, const Cmesh<T,Timg> &oMesh, const int &elem, const CImgList<T> &N, std::vector<T> &U) = 0;

    virtual void evalShape(const CImg<Timg> &list, const Cimage<Timg> &oImage, const Cfield<T,Timg> &oField, const std::vector<T> &ref, const std::vector<T> &lCoor, const std::vector<T> &gCoor, CImgList<T> &N) = 0;    
    
    virtual void gradient(const Cimage<Timg> &oImage, const std::vector<T> &x, std::vector<T> &grad)
    {// image gradient calcul at "x, y, z" image location
    
      grad[0] = (T)( oImage.m_curImg[0](x[0]+1,x[1],x[2], 0) - oImage.m_curImg[0](x[0]-1,x[1],x[2], 0) )/(2.*oImage.m_pix[0]);
      grad[1] = (T)( oImage.m_curImg[0](x[0],x[1]+1,x[2], 0) - oImage.m_curImg[0](x[0],x[1]-1,x[2], 0) )/(2.*oImage.m_pix[1]);

      if (this->_3D)
      {grad[2] = (T)( oImage.m_curImg[0](x[0],x[1],x[2]+1, 0) - oImage.m_curImg[0](x[0],x[1],x[2]-1, 0) )/(2.*oImage.m_pix[2]);}

    }
    
    void evalRes_loop(const Cimage<Timg> &oImage,const std::vector<T> &U, const std::vector<T> &x, T &residue, T &deformed)
    {//eval Corrected deformed image and residue at pix
      
      //corrected deformed image
      if (m_interpLoop == "cubic")
      {
        deformed =      oImage.m_curImg[1].cubic_atXYZ(x[0]+U[0]/oImage.m_pix[0],x[1]+U[1]/oImage.m_pix[1],x[2]+U[2]/oImage.m_pix[2], 0);
      }
      else
      {
        deformed =      oImage.m_curImg[1].linear_atXYZC(x[0]+U[0]/oImage.m_pix[0],x[1]+U[1]/oImage.m_pix[1],x[2]+U[2]/oImage.m_pix[2], 0,0);
      }
      
      //residue between ref and corrected deformed image
      residue = 0;
      if (deformed != 0)
      {residue = (T)oImage.m_curImg(0, x[0],x[1],x[2])- deformed;}
      
    }

    void evalRes_final(const Cimage<Timg> &oImage,const std::vector<T> &U, const std::vector<T> &x, T &residue, T &deformed)
    {//eval Corrected deformed image and residue at pix
      
      if (m_interpFinal == "cubic")
      {
        deformed =      oImage.m_curImg[1].cubic_atXYZ(x[0]+U[0]/oImage.m_pix[0],x[1]+U[1]/oImage.m_pix[1],x[2]+U[2]/oImage.m_pix[2],0);
      }
      else
      {
        deformed =      oImage.m_curImg[1].linear_atXYZC(x[0]+U[0]/oImage.m_pix[0],x[1]+U[1]/oImage.m_pix[1],x[2]+U[2]/oImage.m_pix[2], 0,0);
      }
     
      //residue between ref and corrected deformed image
      residue = 0;
      if (deformed != 0)
      {residue = (T)oImage.m_curImg(0, x[0],x[1],x[2])- deformed;}
      
    }
    
    
    virtual void tensors(const int &thread, const CImgList<T> &Ve, const T &residue, CImgList<T> &Ke)
    {//eval stiffness matrix & force vector at element
      
      cimg_forZ(Ke[0], dim)
      {//in the case of OFI dim = 0
        cimg_forX(Ke[0],modi)
        {//for every modes or nodes
          cimg_forY(Ke[0],modj)
          {
            Ke[0](modi,modj,dim,thread) += Ve[dim][modi] * Ve[dim][modj];//Ke
          }
          Ke[1](0, modi,dim,thread) += Ve[dim][modi] * residue;//Fe
        }
      }
      
    }
    
    virtual void assembly(const int &thread, const Cmesh<T,Timg> &oMesh, const int &elem, const CImgList<T> &Ke, CImgList<T> &K) = 0;
    
    virtual void metric(const Cimage<Timg> &oImage, const Cmesh<T,Timg> &oMesh, CImgList<T> &solution, Cfield<T,Timg> &oField, CImgList<T> &N, CImgList<T> &Ve, CImgList<T> &Ke, std::vector<T> &ref, std::vector<T> &offset, std::vector<T> &lCoor, std::vector<T> &gCoor, std::vector<T> &U, std::vector<T> &grad,  T &residue, T &deformed, CImg<Timg> &list, CImg<int> &p, CImg<T> &current_res, CImgList<T> &K)
    {//calcul K
    
      #pragma omp single
      {//init
        current_res.fill(0);
        cimglist_for(K,var){K[var].fill(0);}
      }
      
      int thread(0),enter(1);

      #pragma omp for schedule(runtime)
      cimglist_for(oMesh.m_connect, elem)
      {//for every elements
        
        if (oMesh.cropList(oImage, elem, list, offset))
        {//if the list is not entirely masked
        
          //check the thread number
          thread = omp_get_thread_num();
      
          //look for the reference system of the element
          e_ref(oImage, oMesh, elem, ref);
      
          //reset tensors
          reset(thread,Ke);
          
          for (int pix=0; pix<list.size(); pix++)
          {//eval Ke over pix

            //define gloMesh.obal coordonates : find back "x,y,z" triplet
            list.contains(list[pix],lCoor[0],lCoor[1],lCoor[2]);
            for (int i=0; i<gCoor.size(); i++){gCoor[i]=lCoor[i]+offset[i];}

            enter = 1;
            //if pix is unmasked
            if (!oImage.m_mask.is_empty()){enter = oImage.m_curMask(gCoor[0], gCoor[1], gCoor[2]);}
            
            if (enter)
            {//if pixels are unmasked and in bounds
            
              // Evaluates grad[dim] at (x,y,z) in physical unit
              gradient(oImage, gCoor, grad);

              // Evaluates N[dim][mode] at (x,y,z) in physical unit
              evalShape(list, oImage, oField, ref, lCoor, gCoor, N);

              // Evaluates Ve[dim][mode] at (x,y,z) in physical unit
              evalGN(grad,N, Ve);

              // Evaluates U[dim] in physical unit
              evalU(solution, oMesh, elem, N, U);
          
              // Evaluates residue between ref and corrected deformed image at (x,y,z)
              evalRes_loop(oImage, U, gCoor, residue, deformed);

              //sum of square residues over pixels and elements
              current_res[thread] += std::pow(residue,2);

              //evaluate Ke
              tensors(thread, Ve, residue, Ke);

             }//endif unmasked or out-of-bound
        
          }//for implied barrier
         
         assembly(thread, oMesh, elem, Ke, K);
          
        }
        
      }
     
      #pragma omp single
      {//sum of square residues over threads and normalization and store residue of previous increment
        oField.m_meanRes.push_back(std::sqrt(current_res.sum())*100/oImage.m_res);
      }

    }
    
    virtual void solve(CImgList<T> &K, const Cmesh<T,Timg> &oMesh, CImgList<T> &solution)
    {// K.U = F
    
      solution = K[1].get_solve(K[0]);
    
    }
    
    virtual void test(const CImgList<T> &solution, const double &epsilon, Cfield<T,Timg> &oField, T &previous_res, int &iter, int &converged)
    {//convergency test
      
      if (std::abs(oField.m_meanRes.back() - previous_res) >= epsilon and oField.m_meanRes.back()<=previous_res)
      {
        
        iter++;
        previous_res = oField.m_meanRes.back();
        oField.m_mode = solution;
        
        if(this->m_verbose){std::cout<<"(iter "<<iter<<") : "<<oField.m_meanRes.back()<<" \%\n\n";}
        
      }
      else
      {
        
        converged = 1;
        if (oField.m_meanRes.back()>previous_res){printf("Warning : last step has diverged\n");}
        else{printf("Last step has converged\n");}
        oField.m_meanRes.back() = previous_res;
        std::cout<<this->class_name.c_str()<<" -> ultimate residue ("<<iter<<" iter) : "<<previous_res<<" \%\n";
        
      }
      
    }
    
    virtual void export_res(const Cimage<Timg> &oImage, const Cmesh<T,Timg> &oMesh, Cfield<T,Timg> &oField, CImgList<T> &N, std::vector<T> &ref, std::vector<T> &offset, std::vector<T> &lCoor, std::vector<T> &gCoor, std::vector<T> &U,  T &residue, T &deformed, CImg<Timg> &list)
    {//actualize residue

      int thread(0), enter(1);
      
      #pragma omp for schedule(runtime)
      cimglist_for(oMesh.m_connect, elem)
      {//for every elements
        
        if (oMesh.cropList(oImage, elem, list, offset))
        {//if the list is not entirely masked
        
          //check the thread number
          thread = omp_get_thread_num();
        
          //look for the reference system of the element
          e_ref(oImage, oMesh, elem, ref);
        
          for (int pix=0; pix<list.size(); pix++)
          {//eval Ke over pix
        
            //define global coordonates : find back "x,y,z" triplet
            list.contains(list[pix],lCoor[0],lCoor[1],lCoor[2]);
            for (int i=0; i<gCoor.size(); i++){gCoor[i]=lCoor[i]+offset[i];}

            enter = 1;
            //if pix is unmasked
            if (!oImage.m_mask.is_empty()){enter = oImage.m_curMask(gCoor[0], gCoor[1], gCoor[2]);}
            
            if (enter)
            {//if pixels are unmasked and in bounds
          
              // Evaluates N[dim][mode] at (x,y,z) in physical unit
              evalShape(list, oImage, oField, ref, lCoor, gCoor, N);

              // Evaluates U[dim] in physical unit
              evalU(oField.m_mode, oMesh, elem, N, U);
          
              // Evaluates residue between ref and corrected deformed image at (x,y,z)
              evalRes_final(oImage, U, gCoor, residue, deformed);
          
              //export data
              store(oField,oImage,gCoor,U,deformed,residue);
              
            }
          
          }//for
        
        }//if croplist
        
      }//for
      
    }
    
    virtual void store(Cfield<T,Timg> &oField, const Cimage<Timg> &oImage, const std::vector<T> gCoor, const std::vector<T> &U,const T &deformed,const T &residue)
    {//store output data
      
      if (oImage.m_storeField)
      {//store field a iter - 1 to avoid storing the last diverging increment
        cimglist_for(oField.m_field, dim)
        {
          #pragma omp critical
          {
            oField.m_field(dim, gCoor[0],gCoor[1],gCoor[2]) = U[dim];
          }
        }
      }

      if (oImage.m_storeDef)
      {//store imDef a iter - 1
//                #pragma omp critical
//        {
            oField.m_def(gCoor[0],gCoor[1],gCoor[2]) = deformed;
//        }
      }

      if (oImage.m_storeRes)
      {//store imRes a iter - 1
//                #pragma omp critical
//        {
            oField.m_res(gCoor[0],gCoor[1],gCoor[2]) = residue;
//        }
      }
      
    }
   

};
 
#endif