CameraParam.cpp

/*
$Id: CameraParam.cpp,v 2.30 2006/07/31 13:47:40 streckel Exp $
*/
/* 
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003-2009    (see file CONTACT for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.

BIAS 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 Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/
#include "CameraParam.hh"
#include <Geometry/ProjectionParametersIO.hh>
#include <iostream>
#include <math.h>
#include <Base/Image/ImageAttributes.hh>

using namespace BIAS;
using namespace std;
  

/*
std::string CameraParam::CameraModelName[]={
  "SONY DFW-X700",
  "SONY DFW-V500",
  "PTGREY DRAGONFLY COLOR",
  "SONY EVI-D31",
  "CAMERA_SONY_DV",
  "UNKNOWN",
};
   
std::string CameraParam::LensModelName[]={
  "SONY EVID31 BUILTIN",
  "SONY DV BUILTIN",
  "PENTAX CMOUNT 4.8",
  "PENTAX CMOUNT 8.0",
  "PENTAX CMOUNT 4.2",
};
*/


CameraParam::CameraParam() {
  Init();
}


CameraParam::CameraParam(const CameraParam &cp) {
  Init(cp);
}


#ifdef BIAS_HAVE_XML2
CameraParam::CameraParam(const std::string &filename)
{
  Init(filename);
}
#endif //BIAS_HAVE_XML2


// #ifdef BIAS_HAVE_XML2
// CameraParam::CameraParam(XMLIO &myXML, xmlNodePtr &cameraNode){
//   Init(myXML, cameraNode);
// }
// #endif //BIAS_HAVE_XML2


CameraParam::~CameraParam(){
}


void CameraParam::Init()
{
  cd_.MaxCamAngle_=0;
  cd_.CamModel_ = "Default Initialization";
  cd_.CamID_ = 1;
  cd_.LensModel_ = "Default Initialization";
  cd_.LensIsSpherical_ = false;

  cd_.width_=cd_.height_=0;
  cd_.aspectratio_ = 1;
  cd_.focallengthDef_ = 0;
  cd_.kc1Def_ = cd_.kc2Def_ = cd_.kc3Def_ = cd_.kc4Def_ = 0;
  cd_.cellSizeX_ = cd_.cellSizeY_ = 0;
  cd_.principalX_ = cd_.principalY_ =0;
  cd_.IlluCorrX_.clear();
  cd_.IlluCorrY_.clear();
  cd_.knownparams_vect_.clear();
  cd_.cellSizeX_ = cd_.cellSizeY_ = 1;
  cd_.zoomCamera_=false;  
  cd_.AngleCorrX_.clear();
  cd_.AngleCorrY_.clear();
  cd_.IlluCorrX_.clear();
  cd_.IlluCorrY_.clear(); 
  cd_.radius_ = 0;
  zoom_ = 0;
  focallengthZoom_ = 0;
  downsamplingFactor_ = 1.0;
}

void CameraParam::Init(const CameraParam &cp) {
  SetCameraData(cp.GetCameraData());
//   cd_.CamModel_ = cp.GetCameraData().CamModel_;
//   cd_.CamID_ = cp.GetCameraData().CamID_;
//   cd_.LensModel_ = cp.GetCameraData().LensModel_;
//   cd_.LensIsSpherical_ = cp.GetCameraData().LensIsSpherical_;
//   cd_.zoomCamera_ = cp.GetCameraData().zoomCamera_;
//   cd_.width_ = cp.GetCameraData().width_;
//   cd_.height_ = cp.GetCameraData().height_;
//   cd_.knownparams_vect_ = cp.GetCameraData().knownparams_vect_;
//   cd_.aspectratio_ = cp.GetCameraData().aspectratio_;
//   cd_.cellSizeX_ = cp.GetCameraData().cellSizeX_;
//   cd_.cellSizeY_ = cp.GetCameraData().cellSizeY_;
//   cd_.principalX_ = cp.GetCameraData().principalX_;
//   cd_.principalY_ = cp.GetCameraData().principalY_;
//   cd_.focallengthDef_ = cp.GetCameraData().focallengthDef_;
//   cd_.kc1Def_ = cp.GetCameraData().kc1Def_;
//   cd_.kc2Def_ = cp.GetCameraData().kc2Def_;
//   cd_.kc3Def_ = cp.GetCameraData().kc3Def_;
//   cd_.kc4Def_ = cp.GetCameraData().kc4Def_;
//   cd_.IlluCorrX_ = cp.GetCameraData().IlluCorrX_;
//   cd_.IlluCorrY_ = cp.GetCameraData().IlluCorrY_;
//   cd_.AngleCorrX_ = cp.GetCameraData().AngleCorrX_;
//   cd_.AngleCorrY_ = cp.GetCameraData().AngleCorrY_;
//   if (cd_.AngleCorrX_.size()>0) 
//     cd_.MaxCamAngle_ = cd_.AngleCorrX_[cd_.AngleCorrX_.size()-1];
//   else 
//     cd_.MaxCamAngle_ = 0;
//   cd_.radius_ = cp.GetCameraData().radius_;
  downsamplingFactor_ = cp.downsamplingFactor_;
  if (cd_.zoomCamera_) {
    zoom_ =  cd_.knownparams_vect_[0].zoom;
    focallengthZoom_ = cd_.knownparams_vect_[0].focallength;
  }
}

#ifdef BIAS_HAVE_XML2
int CameraParam::Init(const std::string &filename) {
  CameraData cd;
  int res = ProjectionParametersIO::ReadCameraData(filename, cd);
  if (res <0) return res;
  SetCameraData(cd);
  return 0;
}
#endif //BIAS_HAVE_XML2


// #ifdef BIAS_HAVE_XML2
// int CameraParam::Init(XMLIO &myXML, xmlNodePtr &cameraNode)
// {
//   int res = ProjectionParametersIO::ReadCameraData(myXML, cameraNode);
//   if (res <0) return res;
//   return 0;
// }
// #endif //BIAS_HAVE_XML2


KMatrix CameraParam::GetKMatrix() const {
  KMatrix K;
  K.SetIdentity();
  if (IsPerspective()) {
    K[0][0] = GetFocalLength();
    K[1][1] = cd_.aspectratio_ * GetFocalLength();
    K[0][1] = 0; //skew_;
    K[0][2] = cd_.principalX_;
    K[1][2] = cd_.principalY_;
  } else {
    BIASERR("No perspective camera-> No K-Matrix!");
  }
  return K;
} 


void CameraParam::SetPrincipal(double x, double y) {
  cd_.principalX_ = x;
  cd_.principalY_ = y;
}

double CameraParam::GetPrincipalX() const {
  return cd_.principalX_/downsamplingFactor_+0.000000001;
}

double CameraParam::GetPrincipalY() const {
  return cd_.principalY_/downsamplingFactor_+0.000000001;
}

unsigned int CameraParam::GetHeight() const {
  return (int)(cd_.height_/downsamplingFactor_+0.000000001);
}

unsigned int CameraParam::GetWidth() const {
  return (int)(cd_.width_/downsamplingFactor_+0.000000001);
}

void CameraParam::SetCellSize(const double CellSizeX, 
                              const double AspectRatio) {
  cd_.cellSizeX_ = CellSizeX;
  if (AspectRatio!=0) {
    cd_.cellSizeY_ = CellSizeX / AspectRatio;
    cd_.aspectratio_ = AspectRatio;
  } else {
    cd_.cellSizeY_ = CellSizeX;
    cd_.aspectratio_ = 1;
  }
}

void CameraParam::SetZoomCamera(const bool zoomCam) {
  cd_.zoomCamera_=zoomCam;
}


bool CameraParam::IsZoomCamera() const  {
  return cd_.zoomCamera_;
}

double CameraParam::GetZoom() const {
  if (IsPerspective()) {
    if (cd_.zoomCamera_) {
      return zoom_;
    } 
    return 0;
  } else {
    BIASERR("No perspective camera!");
    return -1;
  }
}

double CameraParam::GetFocalLength() const {
  if (IsPerspective()) {
    if ( cd_.zoomCamera_) {
      return focallengthZoom_/downsamplingFactor_+0.000000001;
    }
    return cd_.focallengthDef_/downsamplingFactor_+0.000000001;
  } else {
    BIASERR("No perspective camera!");
    return -1;
  }
}


double CameraParam::GetDownsamplingFactor(){
  return downsamplingFactor_;
}

void CameraParam::SetZoom(const double abszoom){
  if (cd_.zoomCamera_ && IsPerspective()) {
    zoom_=abszoom;      
    focallengthZoom_ = InterpolateFocalLength_();
    cout << "interpolating zoom:" << abszoom << " to: " <<
      focallengthZoom_ << endl;
  } else {
    BIASERR("No perspective/zoom camera!");
    return;
  }
}


void CameraParam::SetFocalLength(const double focallength){
  if (IsPerspective()) 
      if (cd_.zoomCamera_){
          focallengthZoom_=focallength;
          zoom_=InterpolateAbsZoom_();
      }
      else
      cd_.focallengthDef_ = focallength;
  else 
    BIASERR("No perspective camera!");
}

void CameraParam::SetDownsampling(const double factor) {
  downsamplingFactor_ = factor;
}

void CameraParam::SetUnzoomed(){
  if (cd_.zoomCamera_ && IsPerspective()) {
    if (cd_.focallengthDef_==0) {
      zoom_=cd_.knownparams_vect_[0].zoom;
      focallengthZoom_=cd_.knownparams_vect_[0].focallength;
    } else {
      zoom_=0;
      focallengthZoom_=cd_.focallengthDef_;
    }
  }
}


#ifdef BIAS_HAVE_XML2
int CameraParam::WriteCameraData(const std::string& Filename) const {
  return ProjectionParametersIO::WriteCameraData(Filename, GetCameraData());
  
}
#endif //BIAS_HAVE_XML2


#ifdef BIAS_HAVE_XML2
// int CameraParam::AddCameraDataToRig(XMLIO &myXML, 
//                                     xmlNodePtr &cameraDataNode) const {

// }
#endif //BIAS_HAVE_XML2



#ifdef BIAS_HAVE_XML2
int CameraParam::ReadCameraData(const std::string& Filename) {
  CameraData cd;
  int res;
  res = ProjectionParametersIO::ReadCameraData(Filename, cd);
  SetCameraData(cd);
  return res;
}

// int CameraParam::ReadCameraData(XMLIO &myXML, xmlNodePtr &cameraNode) {

// }


#endif //BIAS_HAVE_XML2

void CameraParam::InitSpherical(const string CamModel, const int CamID, 
                                const string LensModel,
                                const int imgWidth, const int imgHeight, 
                                const double principalX, 
                                const double principalY, 
                                const double radius,
                                const vector<double> &AngleCorrX,
                                const vector<double> &AngleCorrY,
                                const vector<double> &IlluCorrX, 
                                const vector<double> &IlluCorrY) {

  //reset this
  Init();
  downsamplingFactor_=1;

  cd_.CamModel_ = CamModel;
  cd_.CamID_ = CamID;
  cd_.LensModel_=LensModel;

  cd_.width_  = imgWidth;
  cd_.height_ = imgHeight;
  cd_.principalX_ = principalX;
  cd_.principalY_ = principalY;
  cd_.radius_ = radius;

  cd_.aspectratio_=1;
  cd_.cellSizeX_=1;
  cd_.cellSizeY_=1;
  cd_.LensIsSpherical_ = true;
  cd_.zoomCamera_=false;

  cd_.AngleCorrX_ = AngleCorrX;
  cd_.AngleCorrY_ = AngleCorrY;
  cd_.IlluCorrX_  = IlluCorrX;
  cd_.IlluCorrY_  = IlluCorrY;
}


void CameraParam::InitPerspective(const string CamModel, const int CamID, 
                                const string LensModel,
                                const int imgWidth, const int imgHeight, 
                                const double principalX, 
                                const double principalY, 
                                const double focallength, 
                                const double aspectratio,
                                const double k1, const double k2, const double k3, const double k4,
                                const vector<double> &IlluCorrX, 
                                const vector<double> &IlluCorrY) {

  //reset this
  Init();
  downsamplingFactor_=1;
  cd_.CamModel_ = CamModel;
  cd_.CamID_ = CamID;
  cd_.LensModel_=LensModel;
  cd_.width_  = imgWidth;
  cd_.height_ = imgHeight;
  cd_.principalX_ = principalX;
  cd_.principalY_ = principalY;
  cd_.radius_ = 0.0;
  cd_.focallengthDef_ = focallength;
  cd_.aspectratio_=aspectratio;
  cd_.cellSizeX_=1;
  cd_.cellSizeY_=1/aspectratio;
  cd_.LensIsSpherical_ = false;
  cd_.zoomCamera_=false;
  cd_.kc1Def_ = k1; cd_.kc2Def_ = k2; cd_.kc3Def_ = k3; cd_.kc4Def_ = k4;
  cd_.IlluCorrX_  = IlluCorrX;
  cd_.IlluCorrY_  = IlluCorrY;
}

void CameraParam::DebugOut() const {
  cout << "Camera Parameters:" << endl;
  cout << "CamModel_ = " << cd_.CamModel_ << endl;
  cout << "CamID_ = " << cd_.CamID_ << endl;
  cout << "LensModel_ = " << cd_.LensModel_ << endl;
  cout << "LensIsSpherical_ = " << cd_.LensIsSpherical_ << endl;
  cout << "zoomCamera_ = " << cd_.zoomCamera_ << endl;
  cout << "width_ = " << cd_.width_ << endl;
  cout << "height_ = " << cd_.height_ << endl;
  for (int i=0; i<(int)cd_.knownparams_vect_.size();i++) {
    cout << "knownparams_vect_[" << i << "] = " << endl;
    CPDiscreteParam dp;
    dp=cd_.knownparams_vect_[i];
    cout << "  zoom = " << dp.zoom << endl;
    cout << "  focallength = " << dp.focallength << endl;
    cout << "  kc1 = " << dp.kc1 << endl;
    cout << "  kc2 = " << dp.kc2 << endl;
    cout << "  kc3 = " << dp.kc3 << endl;
    cout << "  kc4 = " << dp.kc4 << endl;
  };
  for (int i=0; i<(int) cd_.AngleCorrX_.size();i++) {
    cout << "AngleCorrX_[" << i << "] = " << cd_.AngleCorrX_[i] << endl;
  }
  for (int i=0; i<(int) cd_.AngleCorrY_.size();i++) {
    cout << "AngleCorrY_[" << i << "] = " << cd_.AngleCorrY_[i] << endl;
  }
  for (int i=0; i<(int) cd_.IlluCorrX_.size();i++) {
    cout << "IlluCorrX_[" << i << "] = " << cd_.IlluCorrX_[i] << endl;
  }
  for (int i=0; i<(int) cd_.IlluCorrY_.size();i++) {
    cout << "IlluCorrY_[" << i << "] = " << cd_.IlluCorrY_[i] << endl;
  }

  cout << "aspectratio_ = " << cd_.aspectratio_ << endl;
  cout << "cellSizeX_ = " << cd_.cellSizeX_ << endl;
  cout << "cellSizeY_ = " << cd_.cellSizeY_ << endl;
  cout << "principalX_ = " << cd_.principalX_ << endl;
  cout << "principalY_ = " << cd_.principalY_ << endl;
  cout << "focallengthDef_ = " << cd_.focallengthDef_ << endl;
  cout << "kc1Def_ = " << cd_.kc1Def_ << endl;
  cout << "kc2Def_ = " << cd_.kc2Def_ << endl;
  cout << "kc3Def_ = " << cd_.kc3Def_ << endl;
  cout << "kc4Def_ = " << cd_.kc4Def_ << endl;
  cout << "downsamplingFactor_ = " << downsamplingFactor_ << endl;
  cout << endl << endl;
}


int CameraParam::ReadFromEXIF(const std::string& filename) {
  CameraData cd;
  int res;
  res=ProjectionParametersIO::ReadFromEXIF(filename, cd);
  SetCameraData(cd);
  return res;
}   


CameraParam::CameraParam(const KMatrix& K, const int w, const int h) {
  CameraData cd;
  ProjectionParametersIO::SetFromKMatrix(K, w, h, cd);
  SetCameraData(cd);
}


void CameraParam::SetData_OMNITECH_ORIFL_190_3 () {
  CameraData cd;
  ProjectionParametersIO::SetData_OMNITECH_ORIFL_190_3(cd);
  SetCameraData(cd);
}


void CameraParam::SetData_EVID31_147094(){
  CameraData cd;
  ProjectionParametersIO::SetData_EVID31_147094(cd);
  SetCameraData(cd);
}


double CameraParam::InterpolateFocalLength_(){

  double relnorm;
  double absdist;

  bool found;
  unsigned int i;
  found=false;
  if (cd_.knownparams_vect_[0].zoom<zoom_){ // zoom is higher than the 
    // known values:
    zoom_=cd_.knownparams_vect_[0].zoom;  // set to highest available
  }
 
  for (i=0;i<cd_.knownparams_vect_.size();i++){
    if (cd_.knownparams_vect_[i].zoom<=zoom_){
        found=true;
        break;
    }
  }

  if (!found){ // zoom is lower than the known values
    focallengthZoom_=cd_.knownparams_vect_[i-1].focallength;
    return cd_.knownparams_vect_[i-1].focallength;
  }
 

  if (cd_.knownparams_vect_[i].zoom==zoom_){
    return cd_.knownparams_vect_[i].focallength;
  }
  relnorm=(zoom_ - cd_.knownparams_vect_[i].zoom) /
    (cd_.knownparams_vect_[i-1].zoom - cd_.knownparams_vect_[i].zoom);

  relnorm=1.0-relnorm;
  absdist=relnorm*(cd_.knownparams_vect_[i].focallength-
                   cd_.knownparams_vect_[i-1].focallength);

  return (absdist+cd_.knownparams_vect_[i-1].focallength);
}


double CameraParam::InterpolateAbsZoom_(){

  double relnorm;
  double absdist;
  bool found;
  unsigned int i;
  found=false;
 
  if (cd_.knownparams_vect_[0].focallength<focallengthZoom_){
    focallengthZoom_=cd_.knownparams_vect_[0].focallength;
  }

  for (i=0;i<cd_.knownparams_vect_.size();i++){
    if (cd_.knownparams_vect_[i].focallength<=focallengthZoom_){
        found=true;
        break;
    }
  }
  if (!found){
    return -1;
  }
 
  if (cd_.knownparams_vect_[i].focallength==focallengthZoom_){
    // interpolate distortion parameters
    return cd_.knownparams_vect_[i].zoom;
  }
  
  relnorm=(focallengthZoom_ - cd_.knownparams_vect_[i].focallength) /
    (cd_.knownparams_vect_[i-1].focallength - 
     cd_.knownparams_vect_[i].focallength);

  relnorm=1.0-relnorm;

  absdist=relnorm*(cd_.knownparams_vect_[i].zoom-
                   cd_.knownparams_vect_[i-1].zoom);
 
  return (absdist+cd_.knownparams_vect_[i-1].zoom);
}