PinholeCamera.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_PINHOLECAMERA_HPP
#define SLIP_PINHOLECAMERA_HPP

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


#include "CameraModel.hpp"
#include "camera_algo.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>
  class PinholeCamera;
  
  template <typename Type>
  std::ostream& operator<<(std::ostream & out, const PinholeCamera<Type>& c);
  
  template <typename Type>
  class PinholeCamera: public CameraModel<Type> {
    typedef PinholeCamera<Type> self;
    typedef slip::CameraModel<Type> base;
  public:
    
    PinholeCamera():
      calibration_matrix_(new slip::Matrix<Type>(3,4)),
      d_calibration_matrix_(new slip::Matrix<Type>(5,3))
    {
    }
    PinholeCamera(const self& rhs):
      base(rhs),
      calibration_matrix_(new slip::Matrix<Type>(*(rhs.calibration_matrix_))),
      d_calibration_matrix_(new slip::Matrix<Type>(*(rhs.d_calibration_matrix_)))
    {}

    
    ~PinholeCamera()
    {
      if(calibration_matrix_ != 0)
                {
                  delete calibration_matrix_;
                }
      if(d_calibration_matrix_ != 0)
                {
                  delete d_calibration_matrix_;
                }
    }
    
    friend std::ostream& operator<< <>(std::ostream & out, const self& c);
    void read(const std::string& file) 
    {
      slip::read_ascii_2d(*calibration_matrix_,file);
      this->update_d_calibration_matrix();
    }
    
    void write(const std::string& file) 
    {
      slip::write_ascii_2d<slip::Matrix<Type>,Type>(*calibration_matrix_,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->d_calibration_matrix_ != 0)
                   {
                     delete this->d_calibration_matrix_;
                   }
                 this->d_calibration_matrix_ = new slip::Matrix<Type>(*(rhs.d_calibration_matrix_));
       }
       return *this;
    }
    slip::Matrix<Type>& calibration_matrix() const
    {
      return *calibration_matrix_;
    }

    slip::Matrix<Type>& d_calibration_matrix() const
    {
      return *d_calibration_matrix_;
    }

    void calibration_matrix(const slip::Matrix<Type>& calibration_matrix)
    {
      assert(calibration_matrix.rows() == 3);
      assert(calibration_matrix.cols() == 4);
      *(this->calibration_matrix_) = calibration_matrix;
      this->update_d_calibration_matrix();
    }

    void d_calibration_matrix(const slip::Matrix<Type>& d_calibration_matrix)
    {
      assert(d_calibration_matrix.rows() == 5);
      assert(d_calibration_matrix.cols() == 3);
      *(this->d_calibration_matrix_) = d_calibration_matrix;
    }

   
    inline
    slip::Point2d<Type> projection(const slip::Point3d<Type>& p) 
    {
      Type px =  (*calibration_matrix_)[0][0]*p[0]
                       + (*calibration_matrix_)[0][1]*p[1]
                       + (*calibration_matrix_)[0][2]*p[2]
                       + (*calibration_matrix_)[0][3];
      Type py =  (*calibration_matrix_)[1][0]*p[0]
                       + (*calibration_matrix_)[1][1]*p[1]
                       + (*calibration_matrix_)[1][2]*p[2]
                       + (*calibration_matrix_)[1][3];
      Type s  =  (*calibration_matrix_)[2][0]*p[0]
                       + (*calibration_matrix_)[2][1]*p[1]
                       + (*calibration_matrix_)[2][2]*p[2]
                       + (*calibration_matrix_)[2][3];
      Type inv_s = static_cast<Type>(1)/s;
      px*=inv_s;
      py*=inv_s;

      return slip::Point2d<Type>(px,py);
    }
    
    // inline
    // slip::Point3d<Type> backprojection(const slip::Point2d<Type>& p2, const Type& z) {
      
    //   const Type zero = static_cast<Type>(0.0);
     
    //   Type px = p2[0];
    //   Type py = p2[1];
    
    //   Type P0 = p2[0]*(*calibration_matrix_)[2][0]-(*calibration_matrix_)[0][0];
    //   Type P1 = p2[0]*(*calibration_matrix_)[2][1]-(*calibration_matrix_)[0][1];
    //   Type P2 = p2[0]*(*calibration_matrix_)[2][2]-(*calibration_matrix_)[0][2];
    //   Type p  = -(p2[0]*(*calibration_matrix_)[2][3]-(*calibration_matrix_)[0][3]);
      
    //   Type Q0 = p2[1]*(*calibration_matrix_)[2][0]-(*calibration_matrix_)[1][0];
    //   Type Q1 = p2[1]*(*calibration_matrix_)[2][1]-(*calibration_matrix_)[1][1];
    //   Type Q2 = p2[1]*(*calibration_matrix_)[2][2]-(*calibration_matrix_)[1][2];
    //   Type q  = -(p2[1]*(*calibration_matrix_)[2][3]-(*calibration_matrix_)[1][3]);
      
    //   Type det = (P0*Q1)-(Q0*P1);

    //   if (det != zero)
    //          {
    //            Type inv_det = static_cast<Type>(1)/det;
    //            px=((((P1*Q2)-(Q1*P2))*z)+(p*Q1)-(P1*q))*inv_det;
    //            py=((((P2*Q0)-(Q2*P0))*z)+(q*P0)-(Q0*p))*inv_det;
    //          }

    //   return  slip::Point3d<Type>(px,py,z);
    // }
    

    inline
    slip::Point3d<Type> backprojection(const slip::Point2d<Type>& p2, 
                                                                       const Type& z) {


      const Type zero = static_cast<Type>(0.0);
    
      Type px = 
                (z*(*d_calibration_matrix_)[0][0] + (*d_calibration_matrix_)[2][0]) * p2[0] + 
                (z*(*d_calibration_matrix_)[0][1] + (*d_calibration_matrix_)[2][1]) * p2[1] + 
                (z*(*d_calibration_matrix_)[0][2] + (*d_calibration_matrix_)[2][2]);

      Type py = 
                (z*(*d_calibration_matrix_)[1][0] + (*d_calibration_matrix_)[3][0]) * p2[0] + 
                (z*(*d_calibration_matrix_)[1][1] + (*d_calibration_matrix_)[3][1]) * p2[1] + 
                (z*(*d_calibration_matrix_)[1][2] + (*d_calibration_matrix_)[3][2]);

      Type s =
                ((*d_calibration_matrix_)[4][0]) * p2[0] + 
                ((*d_calibration_matrix_)[4][1]) * p2[1] + 
                ((*d_calibration_matrix_)[4][2]);

      if (s != zero)
                {
                  Type inv_s = static_cast<Type>(1)/s;
                  px*=inv_s;
                  py*=inv_s;
                }

      return  slip::Point3d<Type>(px,py,z);
    }

    virtual void compute(const slip::Matrix<Type>& P) 
    {
      slip::getpars_Faugeras(P,*calibration_matrix_);
    }
 
    
    virtual int decompose(slip::Matrix<Type>& K, 
                                                  slip::Matrix<Type>& R, 
                                                  slip::Vector3d<Type>& c, 
                                                  slip::DECOMP_TYPE flag) 
    {
      int sing = 1;
      if(flag == RQ) 
                {
                  sing=decompose_RQ(*calibration_matrix_,K,R,c);
                }
      
      if(flag==direct) 
                {
                  decompose_direct(*calibration_matrix_,K,R,c);
                }
      
      return sing;
    }
    //protected:
    slip::Matrix<Type>* calibration_matrix_;
    slip::Matrix<Type>* d_calibration_matrix_;
  
 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 & d_calibration_matrix_;
    }
  template<class Archive>
    void load(Archive & ar, const unsigned int version)
    {
      ar & boost::serialization::base_object<slip::CameraModel<Type> >(*this);
      ar & calibration_matrix_;
      ar & d_calibration_matrix_;
    }
   BOOST_SERIALIZATION_SPLIT_MEMBER()

 
     void update_d_calibration_matrix()
    {
      d_calibration_matrix_->fill(static_cast<Type>(0.0));
     
      (*d_calibration_matrix_)[0][0] = (((*calibration_matrix_)[1][1] * (*calibration_matrix_)[2][2])-((*calibration_matrix_)[1][2] * (*calibration_matrix_)[2][1]));
      (*d_calibration_matrix_)[0][1] = (((*calibration_matrix_)[2][1] * (*calibration_matrix_)[0][2]) - ((*calibration_matrix_)[2][2] * (*calibration_matrix_)[0][1]));
      (*d_calibration_matrix_)[0][2] = (((*calibration_matrix_)[0][1] * (*calibration_matrix_)[1][2]) - ((*calibration_matrix_)[0][2] * (*calibration_matrix_)[1][1]));

      (*d_calibration_matrix_)[1][0] = (((*calibration_matrix_)[1][2] * (*calibration_matrix_)[2][0]) - ((*calibration_matrix_)[1][0] * (*calibration_matrix_)[2][2]));
      (*d_calibration_matrix_)[1][1] = (((*calibration_matrix_)[2][2] * (*calibration_matrix_)[0][0]) - ((*calibration_matrix_)[2][0] * (*calibration_matrix_)[0][2]));
      (*d_calibration_matrix_)[1][2] = (((*calibration_matrix_)[0][2] * (*calibration_matrix_)[1][0]) - ((*calibration_matrix_)[0][0] * (*calibration_matrix_)[1][2]));

      (*d_calibration_matrix_)[2][0] = (((*calibration_matrix_)[1][1] * (*calibration_matrix_)[2][3]) - ((*calibration_matrix_)[1][3] * (*calibration_matrix_)[2][1]));
      (*d_calibration_matrix_)[2][1] = (((*calibration_matrix_)[2][1] * (*calibration_matrix_)[0][3]) - ((*calibration_matrix_)[2][3] * (*calibration_matrix_)[0][1]));
      (*d_calibration_matrix_)[2][2] = (((*calibration_matrix_)[0][1] * (*calibration_matrix_)[1][3]) - ((*calibration_matrix_)[0][3] * (*calibration_matrix_)[1][1]));

      (*d_calibration_matrix_)[3][0] = (((*calibration_matrix_)[1][3] * (*calibration_matrix_)[2][0]) - ((*calibration_matrix_)[1][0] * (*calibration_matrix_)[2][3]));
      (*d_calibration_matrix_)[3][1] = (((*calibration_matrix_)[2][3] * (*calibration_matrix_)[0][0]) - ((*calibration_matrix_)[2][0] * (*calibration_matrix_)[0][3]));
      (*d_calibration_matrix_)[3][2] = (((*calibration_matrix_)[0][3] * (*calibration_matrix_)[1][0]) - ((*calibration_matrix_)[0][0] * (*calibration_matrix_)[1][3]));

      (*d_calibration_matrix_)[4][0] = (((*calibration_matrix_)[1][0] * (*calibration_matrix_)[2][1]) - ((*calibration_matrix_)[1][1] * (*calibration_matrix_)[2][0]));
      (*d_calibration_matrix_)[4][1] = (((*calibration_matrix_)[2][0] * (*calibration_matrix_)[0][1]) - ((*calibration_matrix_)[2][1] * (*calibration_matrix_)[0][0]));
      (*d_calibration_matrix_)[4][2] = (((*calibration_matrix_)[0][0] * (*calibration_matrix_)[1][1]) - ((*calibration_matrix_)[0][1] * (*calibration_matrix_)[1][0]));
    }
  };
  
}//slip::


namespace slip 
{
  template<typename Type>
  std::ostream& operator<<(std::ostream & out, const PinholeCamera<Type>& c) 
  {
    out<<*c.calibration_matrix_<<std::endl;
    return out;
  }
}//slip::
#endif //SLIP_PINHOLECAMERA_HPP