vpCameraParameters.cpp

/****************************************************************************
 *
 * $Id: vpCameraParameters.cpp 5215 2015-01-27 19:03:32Z fspindle $
 *
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
 * 
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact INRIA about acquiring a ViSP Professional 
 * Edition License.
 *
 * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
 * 
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * If you have questions regarding the use of this file, please contact
 * INRIA at visp@inria.fr
 * 
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 *
 * Description:
 * Camera intrinsic parameters.
 *
 * Authors:
 * Eric Marchand
 * Anthony Saunier
 *
 *****************************************************************************/


#include <visp/vpCameraParameters.h>
#include <visp/vpDebug.h>
#include <visp/vpException.h>
#include <visp/vpRotationMatrix.h>
#include <cmath>
#include <limits>
#include <iostream>
#include <sstream>
#include <iomanip>

const double vpCameraParameters::DEFAULT_PX_PARAMETER = 600.0;
const double vpCameraParameters::DEFAULT_PY_PARAMETER = 600.0;
const double vpCameraParameters::DEFAULT_U0_PARAMETER = 192.0;
const double vpCameraParameters::DEFAULT_V0_PARAMETER = 144.0;
const double vpCameraParameters::DEFAULT_KUD_PARAMETER = 0.0;
const double vpCameraParameters::DEFAULT_KDU_PARAMETER = 0.0;
const vpCameraParameters::vpCameraParametersProjType
    vpCameraParameters::DEFAULT_PROJ_TYPE =
    vpCameraParameters::perspectiveProjWithoutDistortion;

vpCameraParameters::vpCameraParameters()
  :
    px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER),
    u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
    kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER),
    width(0), height(0),
    isFov(false), m_hFovAngle(0), m_vFovAngle(0), fovNormals(),
    inv_px(1./DEFAULT_PX_PARAMETER), inv_py(1./DEFAULT_PY_PARAMETER),
    projModel(DEFAULT_PROJ_TYPE)
{
  init() ;
}

vpCameraParameters::vpCameraParameters(const vpCameraParameters &c)
  :
    px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER),
    u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
    kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER),
    width(0), height(0),
    isFov(false), m_hFovAngle(0), m_vFovAngle(0), fovNormals(),
    inv_px(1./DEFAULT_PX_PARAMETER), inv_py(1./DEFAULT_PY_PARAMETER),
    projModel(DEFAULT_PROJ_TYPE)
{
  init(c) ;
}

vpCameraParameters::vpCameraParameters(const double cam_px, const double cam_py,
                                       const double cam_u0, const double cam_v0)
  :
    px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER),
    u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
    kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER),
    width(0), height(0),
    isFov(false), m_hFovAngle(0), m_vFovAngle(0), fovNormals(),
    inv_px(1./DEFAULT_PX_PARAMETER), inv_py(1./DEFAULT_PY_PARAMETER),
    projModel(DEFAULT_PROJ_TYPE)
{
  initPersProjWithoutDistortion(cam_px,cam_py,cam_u0,cam_v0) ;
}

vpCameraParameters::vpCameraParameters(const double cam_px, const double cam_py,
                                       const double cam_u0, const double cam_v0,
                                       const double cam_kud, const double cam_kdu)
  :
    px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER),
    u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
    kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER),
    width(0), height(0),
    isFov(false), m_hFovAngle(0), m_vFovAngle(0), fovNormals(),
    inv_px(1./DEFAULT_PX_PARAMETER), inv_py(1./DEFAULT_PY_PARAMETER),
    projModel(DEFAULT_PROJ_TYPE)
{
  initPersProjWithDistortion(cam_px,cam_py,cam_u0,cam_v0,cam_kud,cam_kdu) ;
}

void
vpCameraParameters::init()
{  
  if (fabs(this->px)<1e-6)
  {
    vpERROR_TRACE("Camera parameter px = 0") ;
    throw(vpException(vpException::divideByZeroError,
          "Camera parameter px = 0")) ;
  }
  if (fabs(this->py)<1e-6)
  {
    vpERROR_TRACE("Camera parameter px = 0") ;
    throw(vpException(vpException::divideByZeroError,
          "Camera parameter px = 0")) ;
  }
  this->inv_px = 1./this->px;
  this->inv_py = 1./this->py;
}

void
vpCameraParameters::initPersProjWithoutDistortion(const double cam_px, const double cam_py,
                                                  const double cam_u0, const double cam_v0)
{
  this->projModel = vpCameraParameters::perspectiveProjWithoutDistortion ;
  
  this->px    = cam_px ;
  this->py    = cam_py ;
  this->u0    = cam_u0 ;
  this->v0    = cam_v0 ;
  this->kud   = 0 ;
  this->kdu   = 0 ;
  
  if (fabs(px)<1e-6)
  {
    vpERROR_TRACE("Camera parameter px = 0") ;
    throw(vpException(vpException::divideByZeroError,
          "Camera parameter px = 0")) ;
  }
  if (fabs(py)<1e-6)
  {
    vpERROR_TRACE("Camera parameter px = 0") ;
    throw(vpException(vpException::divideByZeroError,
          "Camera parameter px = 0")) ;
  }
  this->inv_px = 1./px;
  this->inv_py = 1./py;
}

void
vpCameraParameters::initPersProjWithDistortion(const double cam_px, const double cam_py,
                                               const double cam_u0, const double cam_v0,
                                               const double cam_kud, const double cam_kdu)
{
  this->projModel = vpCameraParameters::perspectiveProjWithDistortion ;

  this->px    = cam_px ;
  this->py    = cam_py ;
  this->u0    = cam_u0 ;
  this->v0    = cam_v0 ;
  this->kud   = cam_kud ;
  this->kdu   = cam_kdu ;
  
  if (fabs(px)<1e-6)
  {
    vpERROR_TRACE("Camera parameter px = 0") ;
    throw(vpException(vpException::divideByZeroError,
          "Camera parameter px = 0")) ;
  }
  if (fabs(py)<1e-6)
  {
    vpERROR_TRACE("Camera parameter px = 0") ;
    throw(vpException(vpException::divideByZeroError,
          "Camera parameter px = 0")) ;
  }
  this->inv_px = 1./px;
  this->inv_py = 1./py;
}

vpCameraParameters::~vpCameraParameters()
{
}

void
vpCameraParameters::init(const vpCameraParameters &c)
{
  *this = c ;
}


void
vpCameraParameters::initFromCalibrationMatrix(const vpMatrix& _K)
{
  if(_K.getRows() != 3 || _K.getCols() != 3 ){
    throw vpException(vpException::dimensionError, "bad size for calibration matrix");
  }
  if( std::fabs(_K[2][2] - 1.0) > std::numeric_limits<double>::epsilon()){
    throw vpException(vpException::badValue, "bad value: K[2][2] must be equal to 1");
  }
  initPersProjWithoutDistortion (_K[0][0], _K[1][1], _K[0][2], _K[1][2]);
}

void
vpCameraParameters::initFromFov(const unsigned int &w, const unsigned int &h, const double &hfov, const double &vfov)
{
  projModel = vpCameraParameters::perspectiveProjWithoutDistortion;
  u0 = (double)w/2.;
  v0 = (double)h/2.;
  px = u0 / tan(hfov/2);
  py = v0 / tan(vfov/2);
  kud = 0;
  kdu = 0;
  inv_px = 1./px;
  inv_py = 1./py;
  computeFov(w, h);
}

vpCameraParameters&
    vpCameraParameters::operator=(const vpCameraParameters& cam)
{
  projModel = cam.projModel ;
  px = cam.px ;
  py = cam.py ;
  u0 = cam.u0 ;
  v0 = cam.v0 ;
  kud = cam.kud ;
  kdu = cam.kdu ;
  
  inv_px = cam.inv_px; 
  inv_py = cam.inv_py;
  
  isFov = cam.isFov;
  m_hFovAngle = cam.m_hFovAngle;
  m_vFovAngle = cam.m_vFovAngle;
  width = cam.width;
  height = cam.height;
  fovNormals = cam.fovNormals;
  
  return *this ;
}

void
vpCameraParameters::computeFov(const unsigned int &w, const unsigned int &h)
{
  if( !isFov && w != width && h != height && w != 0 && h != 0){
    fovNormals = std::vector<vpColVector>(4);
    
    isFov = true;
    
    double half_hFovAngle = atan((double)w / ( 2.0 * px ));
    double half_vFovAngle = atan((double)h / ( 2.0 * py ));
    
    width = w;
    height = h;
    
    vpColVector n(3);
    n = 0;
    n[0] = 1.0;
    
    vpRotationMatrix Rleft (0,-half_hFovAngle,0);
    vpRotationMatrix Rright(0, half_hFovAngle,0);
    
    vpColVector nLeft, nRight;
    
    nLeft = Rleft * (-n);
    fovNormals[0] = nLeft.normalize();
    
    nRight = Rright * n;
    fovNormals[1] = nRight.normalize();
    
    n = 0;
    n[1] = 1.0;
  
    vpRotationMatrix Rup  ( half_vFovAngle,0,0);
    vpRotationMatrix Rdown(-half_vFovAngle,0,0);
    
    vpColVector nUp, nDown;
    
    nUp = Rup * (-n);
    fovNormals[2] = nUp.normalize();
    
    nDown = Rdown * n;
    fovNormals[3] = nDown.normalize();

    m_hFovAngle = 2 * half_hFovAngle;
    m_vFovAngle = 2 * half_vFovAngle;
  }
}


vpMatrix
vpCameraParameters::get_K() const
{
  vpMatrix K;
  switch(projModel){
    case vpCameraParameters::perspectiveProjWithoutDistortion :
      K.resize(3,3) ;
      K = 0.0 ;
      K[0][0] = px ;
      K[1][1] = py ;
      K[0][2] = u0 ;
      K[1][2] = v0 ;
      K[2][2] = 1.0 ;
      break;
    case vpCameraParameters::perspectiveProjWithDistortion :
    default :
      vpERROR_TRACE("\n\t getting K matrix in the case of projection \
          with distortion has no sense");   
      throw(vpException(vpException::notImplementedError,
            "\n\t getting K matrix in the case of projection \
                  with distortion has no sense"));
  }
  return K; 
}
vpMatrix
vpCameraParameters::get_K_inverse() const
{
  vpMatrix K_inv;
  switch(projModel){
    case vpCameraParameters::perspectiveProjWithoutDistortion :
      K_inv.resize(3,3) ;
      K_inv = 0.0 ;
      K_inv[0][0] = inv_px ;
      K_inv[1][1] = inv_py ;
      K_inv[0][2] = -u0*inv_px ;
      K_inv[1][2] = -v0*inv_py ;
      K_inv[2][2] = 1.0 ;
      break;
    case vpCameraParameters::perspectiveProjWithDistortion :
    default :
      vpERROR_TRACE("\n\t getting K^-1 matrix in the case of projection \
          with distortion has no sense");
      throw(vpException(vpException::notImplementedError,
            "\n\t getting K matrix in the case of projection \
                  with distortion has no sense"));
  }
  return K_inv;
}


void
vpCameraParameters::printParameters()
{
  std::ios::fmtflags original_flags( std::cout.flags() );
  switch(projModel){
    case vpCameraParameters::perspectiveProjWithoutDistortion :
      std::cout.precision(10);
      std::cout << "Camera parameters for perspective projection without distortion:"
                << std::endl ;
      std::cout << "  px = " << px <<"\t py = "<< py << std::endl ;
      std::cout << "  u0 = " << u0 <<"\t v0 = "<< v0 << std::endl ;
      break;
    case vpCameraParameters::perspectiveProjWithDistortion :
      std::cout.precision(10);
      std::cout << "Camera parameters for perspective projection with distortion:"
                << std::endl ;
      std::cout << "  px = " << px <<"\t py = "<< py << std::endl ;
      std::cout << "  u0 = " << u0 <<"\t v0 = "<< v0 << std::endl ;
      std::cout << "  kud = " << kud << std::endl ;
      std::cout << "  kdu = " << kdu << std::endl ;
      break;
  } 
  // Restore ostream format
  std::cout.flags(original_flags);
}
VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpCameraParameters &cam)
{
  switch(cam.get_projModel()){
  case vpCameraParameters::perspectiveProjWithoutDistortion :
    os << "Camera parameters for perspective projection without distortion:"
       << std::endl ;
    os << "  px = " << cam.get_px() <<"\t py = "<< cam.get_py()
       << std::endl ;
    os << "  u0 = " << cam.get_u0() <<"\t v0 = "<< cam.get_v0()
       << std::endl ;
    break;
  case vpCameraParameters::perspectiveProjWithDistortion :
    std::ios_base::fmtflags original_flags = os.flags();
    os.precision(10);
    os << "Camera parameters for perspective projection with distortion:"
       << std::endl ;
    os << "  px = " << cam.get_px() <<"\t py = "<< cam.get_py()
       << std::endl ;
    os << "  u0 = " << cam.get_u0() <<"\t v0 = "<< cam.get_v0()
       << std::endl ;
    os << "  kud = " << cam.get_kud() << std::endl ;
    os << "  kdu = " << cam.get_kdu() << std::endl ;

    os.flags(original_flags); // restore os to standard state
    break;
  }
  return os;
}