DistortionCamera.hpp

Go to the documentation of this file.

/*
 * Copyright(c):
 * Signal Image and Communications (SIC) Department 
 * http://www.sic.sp2mi.univ-poitiers.fr/ 
 *  - University of Poitiers, France http://www.univ-poitiers.fr 
 *  - XLIM  Institute UMR CNRS 7252  http://www.xlim.fr/ 
 * 
 * and 
 * 
 * D2 Fluid, Thermic and Combustion 
 *  - University of Poitiers, France http://www.univ-poitiers.fr 
 *  - PPRIME Institute -  UPR CNRS 3346  http://www.pprime.fr
 *  - ISAE-ENSMA http://www.ensma.fr
 *
 * Contributor(s):
 * The SLIP team,
 * Benoit Tremblais <tremblais_AT_sic.univ-poitiers.fr>,
 * Laurent David <laurent.david_AT_lea.univ-poitiers.fr>, 
 * Ludovic Chatellier <ludovic.chatellier_AT_univ-poitiers.fr>, 
 * Lionel Thomas <lionel.thomas_AT_univ-poitiers.fr>, 
 * Denis Arrivault <arrivault_AT_sic.univ-poitiers.fr>, 
 * Julien Dombre <julien.dombre_AT_univ-poitiers.fr>.
 *
 * Description:
 * The Simple Library of Image Processing (SLIP) is a new image processing 
 * library. It is written in the C++ language following as much as possible 
 * the ISO/ANSI C++ standard. It is consequently compatible with any system  
 * satisfying the ANSI C++ complience. It works on different Unix , Linux ,  
 * Mircrosoft Windows and Mac OS X plateforms. SLIP is a research library that  
 * was created by the Signal, Image and Communications (SIC) departement of  
 * the XLIM, UMR 7252 CNRS Institute in collaboration with the Fluids, Thermic  
 * and Combustion departement of the P', UPR 3346 CNRS Institute of the  
 * University of Poitiers.
 * 
 * The SLIP Library source code has been registered to the APP (French Agency 
 * for the Protection of Programs) by the University of Poitiers and CNRS, 
 * under  registration number IDDN.FR.001.300034.000.S.P.2010.000.21000.

 * http://www.sic.sp2mi.univ-poitiers.fr/slip/
 *
 * This software is governed by the CeCILL-C license under French law and
 * abiding by the rules of distribution of free software.  You can  use, 
 * modify and/ or redistribute the software under the terms of the CeCILL-C 
 * license as circulated by CEA, CNRS and INRIA at the following URL 
 * http://www.cecill.info.
 * As a counterpart to the access to the source code and  rights to copy,
 * modify and redistribute granted by the license, users are provided only
 * with a limited warranty  and the software's author,  the holder of the
 * economic rights,  and the successive licensors  have only  limited
 * liability.
 * 
 * In this respect, the user's attention is drawn to the risks associated
 * with loading,  using,  modifying and/or developing or reproducing the
 * software by the user in light of its specific status of free software,
 * that may mean  that it is complicated to manipulate,  and  that  also
 * therefore means  that it is reserved for developers  and  experienced
 * professionals having in-depth computer knowledge. Users are therefore
 * encouraged to load and test the software's suitability as regards their
 * requirements in conditions enabling the security of their systems and/or
 * data to be ensured and,  more generally, to use and operate it in the 
 * same conditions as regards security. 
 *
 * The fact that you are presently reading this means that you have had
 * knowledge of the CeCILL-C license and that you accept its terms.
 */


#ifndef SLIP_DISTORTIONCAMERA_HPP
#define SLIP_DISTORTIONCAMERA_HPP

#include <iostream>
#include <cassert>
#include "Point2d.hpp"
#include "Point3d.hpp"
#include "Matrix.hpp"
#include "io_tools.hpp"

#include "CameraModel.hpp"
#include "camera_algo.hpp"
#include "levenberg_marquardt.hpp"

#include <boost/serialization/access.hpp>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/version.hpp>
#include <boost/serialization/base_object.hpp>

namespace slip
{
  template <typename Type>
  struct DistStruct 
  {
    DistStruct():
      R1(0),
      R2(0),
      R3(0),
      d1(0),
      d2(0),
      p1(0),
      p2(0) 
    {
    }

    DistStruct(const DistStruct<Type>& rhs):
      R1(rhs.R1),
      R2(rhs.R2),
      R3(rhs.R3),
      d1(rhs.d1),
      d2(rhs.d2),
      p1(rhs.p1),
      p2(rhs.p2)
    {}

    Type R1;
    Type R2;
    Type R3;
    Type d1;
    Type d2;
    Type p1;
    Type p2;

     private:
    friend class boost::serialization::access;
    template<class Archive>
    void save(Archive & ar, const unsigned int version) const
    {
      ar & R1 & R2 & R3 & d1 & d2 & p1 & p2;
    }
  template<class Archive>
    void load(Archive & ar, const unsigned int version)
    {
      ar & R1 & R2 & R3 & d1 & d2 & p1 & p2;
    }
   BOOST_SERIALIZATION_SPLIT_MEMBER()
  };
}

namespace slip 
{
  template <typename Type>
  class DistortionCamera;
  
  template <typename Type>
  std::ostream& operator<<(std::ostream & out, const DistortionCamera<Type>& c);
  
  template <typename Type>
  class DistortionCamera: public CameraModel<Type> 
  {
    typedef DistortionCamera<Type> self;
    typedef slip::CameraModel<Type> base;
  public:

    DistortionCamera():
      calibration_matrix_(new slip::Matrix<Type>(3,4)),
      calibration_distortions_(new slip::DistStruct<Type>()),
      calibration_vector_(new slip::Vector<Type>(21)) 
    {}

    DistortionCamera(slip::DistStruct<Type>* dist_struct):
      calibration_matrix_(new slip::Matrix<Type>(3,4)),
      calibration_distortions_(dist_struct),
      calibration_vector_(new slip::Vector<Type>(21)) 
    {}

    DistortionCamera(const self& rhs):
      base(rhs),
      calibration_matrix_(new slip::Matrix<Type>(*(rhs.calibration_matrix_))),
      calibration_distortions_(new slip::DistStruct<Type>(*(rhs.calibration_distortions_))),
      calibration_vector_(new slip::Vector<Type>(*(rhs.calibration_vector_)))
    {}

    ~DistortionCamera()
    {
      delete calibration_matrix_;
      delete calibration_distortions_;
      delete calibration_vector_;
    }

    friend std::ostream& operator<< <>(std::ostream & out, const self& c);
    void read(const std::string& file) 
    {
      slip::read_ascii_1d(file,*calibration_vector_);
    }

    void write(const std::string& file) 
    {
      slip::write_ascii_1d<typename slip::Vector<Type>::iterator,Type>((*calibration_vector_).begin(),(*calibration_vector_).end(),file);
    }
    self& operator=(const self & rhs)
    {
       if(this != &rhs)
       {
                this->base::operator=(rhs);
                if(this->calibration_matrix_ != 0)
                  {
                    delete this->calibration_matrix_;
                  }
                this->calibration_matrix_ = new slip::Matrix<Type>(*(rhs.calibration_matrix_));

                if(this->calibration_distortions_ != 0)
                  {
                    delete this->calibration_distortions_; 
                  }
                this->calibration_distortions_ = new slip::DistStruct<Type>(*(rhs.calibration_distortions_));

                if(this->calibration_vector_ != 0)
                  {
                    delete this->calibration_vector_;
                  }
                this->calibration_vector_ = new slip::Vector<Type>(*(rhs.calibration_vector_));
       }
       return *this;
    }
    slip::Point2d<Type> projection(const slip::Point3d<Type>& p) 
    {
      //Type X=p[0]; Type Y=p[1]; Type Z=p[2];
      Type den = ((*calibration_vector_)[6]*p[0] + (*calibration_vector_)[7]*p[1] + (*calibration_vector_)[8]*p[2] + (*calibration_vector_)[11]);
      Type xr  = ((*calibration_vector_)[0]*p[0] + (*calibration_vector_)[1]*p[1] + (*calibration_vector_)[2]*p[2] + (*calibration_vector_)[9])/den;
      Type yr  = ((*calibration_vector_)[3]*p[0] + (*calibration_vector_)[4]*p[1] + (*calibration_vector_)[5]*p[2] + (*calibration_vector_)[10])/den;
      Type xr_p = xr-(*calibration_vector_)[12];
      Type yr_p = yr-(*calibration_vector_)[13];
      Type xr_p_2 = xr_p*xr_p;
      Type yr_p_2 = yr_p*yr_p;
      Type xr_p_yr_p = xr_p *yr_p;
      Type rhoi2 = xr_p_2 + yr_p_2;
      Type rhoi2_2 = rhoi2*rhoi2;
      Type rhoi2_3 = rhoi2_2*rhoi2;
      Type rad = ((*calibration_vector_)[14]*rhoi2 + (*calibration_vector_)[15]*rhoi2_2 + (*calibration_vector_)[16]*rhoi2_3);
      Type u =  xr 
                      + xr_p*rad 
                      + static_cast<Type>(2.0)*(*calibration_vector_)[17]*xr_p_yr_p
                      + (*calibration_vector_)[18]*(static_cast<Type>(3.0)*xr_p_2 + yr_p_2) 
                      + (*calibration_vector_)[19]*rhoi2;
      Type v =  yr 
                      + yr_p*rad 
                      + static_cast<Type>(2.0)*(*calibration_vector_)[18]*xr_p*yr_p  
                      + (*calibration_vector_)[17]*(static_cast<Type>(3.0)*yr_p_2 + xr_p_2) 
                      + (*calibration_vector_)[20]*rhoi2;
      return slip::Point2d<Type>(u,v);
    }
    
    slip::Point3d<Type> backprojection(const slip::Point2d<Type>& p2, const Type& z) 
    {
      //data --> attributes
      slip::Vector<Type> data(24);
      std::copy(calibration_vector_->begin(),calibration_vector_->end(),data.begin());
         
      data(21) = p2[0];
      data(22) = p2[1];
      data(23) = z;
      //TO OPTIMIZE
      slip::funLM_bp<Type> h(data);
     
      slip:: Vector<Type> r(2);
      slip:: Vector<Type> p(2);
      
      slip::LMDerivFunctor<slip::funLM_bp<Type>, Type> dh(h,r.size(),p.size());
      double chisq = 0.0;
      slip::marquardt(h,dh,p,r,chisq);
       
      return slip::Point3d<Type>(p[0],p[1],z);
    }
    void compute(const slip::Matrix<Type>& P) 
    {
      // 1.) get initial parameters
      slip::Vector<Type> y(21,static_cast<Type>(0.0));
      slip::Vector<Type> p9(9,static_cast<Type>(0.0));
      slip::Matrix<Type> M(3,4);
      typename slip::Vector<Type>::iterator y_begin = y.begin();
      typename  slip::Vector<Type>::iterator y_begin12 = y.begin() + 12;
      typename slip::Vector<Type>::iterator y_end = y.end();
      typename slip::Vector<Type>::iterator p9_begin = p9.begin();
      typename slip::Vector<Type>::iterator p9_end  = p9.end();
      typename slip::Vector<Type>::iterator calibration_vector_begin = calibration_vector_->begin();
      typename slip::Vector<Type>::iterator calibration_vector_begin12 = calibration_vector_->begin() + 12;

      getpars_DLT_norm(P,M);
      
      y(0)=M[0][0]; y(1)=M[0][1]; y(2)=M[0][2]; y(9)=M[0][3];
      y(3)=M[1][0]; y(4)=M[1][1]; y(5)=M[1][2]; y(10)=M[1][3];
      y(6)=M[2][0]; y(7)=M[2][1]; y(8)=M[2][2]; y(11)=M[2][3];
      std::fill(y_begin12,y_end,static_cast<Type>(0.0));
      // 2.) put all data in one vector
      const std::size_t N = P.rows();
      const std::size_t _5N = 5 * N;
      const std::size_t _5N_p_19 = _5N+19;
      const std::size_t _5N_p_7  = _5N+7;
      slip::Vector<Type> adatad (_5N_p_19);
      typename slip::Vector<Type>::iterator adatad_begin_5N_p_7 = adatad.begin()+_5N_p_7;
      for(std::size_t i = 0; i<N; ++i) 
                {
                  adatad[i]=P[i][0];                            // u_i
                  adatad[N+i]=P[i][1];                  // v_i
                  adatad[2*N+i]=P[i][2];        // X_i
                  adatad[3*N+i]=P[i][3];        // Y_i
                  adatad[4*N+i]=P[i][4];        // Z_i
                }
      
      adatad[_5N]   = calibration_distortions_->R1;
      adatad[_5N+1] = calibration_distortions_->R2;
      adatad[_5N+2] = calibration_distortions_->R3;
      adatad[_5N+3] = calibration_distortions_->d1;
      adatad[_5N+4] = calibration_distortions_->d2;
      adatad[_5N+5] = calibration_distortions_->p1;
      adatad[_5N+6] = calibration_distortions_->p2;

      std::copy(y_begin,y_begin12,adatad_begin_5N_p_7);
      //3.) Calcul param�tres
      std::copy(y_begin12,y_end,p9_begin);
      slip::Matrix<Type> Q(P);
      //      Q=P;
      slip:: Vector<Type> r(2*N);
      double chisq = 1000.0;
      double new_chisq = 0.0;
      bool done = 0;
      int iter = 0;
      double eps = 10e-5;
      
      //main loop
      while(!done) 
                {
                  slip::funLM_DLT<Type> f(adatad);
                  //              slip::LMDerivFunctor<funLM_DLT<Type>, Type> df(f,adatad.size(),p9.size());
                  slip::LMDerivFunctor<funLM_DLT<Type>, Type> df(f,r.size(),p9.size());
                
                  slip::marquardt (f, df,p9, r,new_chisq);
                  std::copy(y_begin,y_begin12,calibration_vector_begin);
                  std::copy(p9_begin,p9_end,calibration_vector_begin12);
                  //estimation nouvelle matrice lin�aire
                  undistort(P,Q);
                  getpars_DLT_norm(Q,M);
                 
                  y(0)=M[0][0]; y(1)=M[0][1]; y(2)=M[0][2]; y(9)=M[0][3];
                  y(3)=M[1][0]; y(4)=M[1][1]; y(5)=M[1][2]; y(10)=M[1][3];
                  y(6)=M[2][0]; y(7)=M[2][1]; y(8)=M[2][2]; y(11)=M[2][3];

                  std::copy(y_begin,y_begin12,adatad_begin_5N_p_7);

                  if (std::abs(((chisq - new_chisq)/chisq)) < eps)
                    {
                      done = 1;
                    }
                  if (new_chisq>chisq)
                    {
                      done = 1;
                    }
                  if (iter>1000)
                    {
                      done = 1;
                    }
                  chisq=new_chisq;
                  iter++;
                }
                std::cout<<" Chisq final = "<<new_chisq<<"   iteration = "<<iter<<std::endl;
                
                // 4.) Ecriture mod�le distorsion
                std::copy(y_begin,y_begin12,calibration_vector_begin);
                std::copy(p9_begin,p9_end,calibration_vector_begin12);
    }
    
   
    
    void undistort(const slip::Matrix<Type>& P, slip::Matrix<Type>& Q) 
    {

      Q.resize(P.rows(),P.cols());
      slip::Point2d<Type> pd;
      slip::Point2d<Type> pr;
      const std::size_t P_rows = P.rows();
      for(std::size_t i = 0; i < P_rows; ++i) 
                {
                pd[0] = P[i][0];
                pd[1] = P[i][1];
                pr = slip::invert_distortion_model(pd,*calibration_vector_);
                Q[i][0] = pr[0];
                Q[i][1] = pr[1];
                Q[i][2] = P[i][2];
                Q[i][3] = P[i][3];
                Q[i][4] = P[i][4];
                }
    }
   
    void distort(const slip::Point2d<Type>& p, slip::Point2d<Type>& pd) 
    {
      Type U0=(*calibration_vector_)[12];Type V0=(*calibration_vector_)[13];
      Type R1=(*calibration_vector_)[14];Type R2=(*calibration_vector_)[15];Type R3=(*calibration_vector_)[16];
      Type D1=(*calibration_vector_)[17];Type D2=(*calibration_vector_)[18];
      Type P1=(*calibration_vector_)[19];Type P2=(*calibration_vector_)[20];
      Type x = p[0];
      Type y = p[1];
      Type x_p = x-U0;
      Type y_p = y-V0;
      Type x_p2 = x_p*x_p;
      Type y_p2 = y_p*y_p;
      Type two_x_p_y_p = static_cast<Type>(2.0)*x_p * y_p;
      Type rhoi2 = x_p2 + y_p2;
      Type rhoi2_2 = rhoi2*rhoi2;
      Type rhoi2_3 = rhoi2_2*rhoi2;
      Type rad=(R1*rhoi2 + R2*rhoi2_2 + R3*rhoi2_3);
      Type xd = x + x_p*rad + D1*two_x_p_y_p + D2*(static_cast<Type>(3.0)*x_p2+y_p2) + P1*rhoi2;
      Type yd = y + y_p*rad + D2*two_x_p_y_p + D1*(static_cast<Type>(3.0)*y_p2+x_p2) + P2*rhoi2;
      pd[0]=xd; 
      pd[1]=yd;
    }
    

    //protected:
    slip::Matrix<Type>* calibration_matrix_;
    slip::DistStruct<Type>* calibration_distortions_;
    slip::Vector<Type>* calibration_vector_;

     private:
    friend class boost::serialization::access;
    template<class Archive>
    void save(Archive & ar, const unsigned int version) const
    {
      ar & boost::serialization::base_object<slip::CameraModel<Type> >(*this);
      ar & calibration_matrix_;
      ar & calibration_distortions_;
      ar & calibration_vector_;
    }
  template<class Archive>
    void load(Archive & ar, const unsigned int version)
    {
      ar & boost::serialization::base_object<slip::CameraModel<Type> >(*this);
      ar & calibration_matrix_;
      ar & calibration_distortions_;
      ar & calibration_vector_;
    }
   BOOST_SERIALIZATION_SPLIT_MEMBER()
  };

}//slip::


namespace slip 
{

  template<typename Type>
  std::ostream& operator<<(std::ostream & out, const DistortionCamera<Type>& c) 
  {
    out<<*c.calibration_matrix_<<std::endl;
    out<<c.calibration_distortions_->R1<<std::endl;
    out<<c.calibration_distortions_->R2<<std::endl;
    out<<c.calibration_distortions_->R3<<std::endl;
    out<<c.calibration_distortions_->d1<<std::endl;
    out<<c.calibration_distortions_->d2<<std::endl;
    out<<c.calibration_distortions_->p1<<std::endl;
    out<<c.calibration_distortions_->p2<<std::endl;
    out<<*c.calibration_vector_;
    return out;
  }


}//slip::
#endif //SLIP_DISTORTIONCAMERA_HPP