ProjectionParametersZoom.cpp

/* 
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 "Geometry/ProjectionParametersZoom.hh" 


using namespace BIAS;
using namespace std;
 

HomgPoint2D ProjectionParametersZoom::
ProjectLocal(const Vector3<double>& point, bool IgnoreDistortion) const {
  // in-plane affine coordinate transformation with KMatrix
  HomgPoint2D pos;
  K_.Mult(point, pos);
  // radial distortion here 
  if (!IgnoreDistortion) Distort(pos);
  return pos.Homogenize();
}

int ProjectionParametersZoom::
ProjectLocal(const Vector3<double>& point, HomgPoint2D &p2d,
             bool IgnoreDistortion) const
{ 
  BIASERR("unfinished\n");
  BIASABORT;
  return -1;
}


HomgPoint3D ProjectionParametersZoom::
UnProjectToImagePlane(const HomgPoint2D& pos, const double& depth,
                      bool IgnoreDistortion) const 
{
  Vector3<double> d, p;
  UnProjectLocal(pos, p, d, IgnoreDistortion);
  if(Equal(d[2], 0.0))
    return HomgPoint3D(0,0,0,0);
  d*=depth/d[2];
  if(Equal(d.NormL2(), 0.0))
    return HomgPoint3D(0,0,0,0);
  HomgPoint3D local(d);
  return Pose_.LocalToGlobal(local);
}
   
 
void ProjectionParametersZoom::
UnProjectLocal(const HomgPoint2D& cPos, Vector3<double>& pointOnRay,
          Vector3<double>& direction, bool IgnoreDistortion) const {
  HomgPoint2D mpos(cPos); // de-const pos
  // radial undistortion here 
  if (!IgnoreDistortion) Undistort(mpos);
  // in-plane affine coordinate transformation with KMatrix
  invK_.Mult(mpos, direction);
  direction = direction.GetNormalized();
  pointOnRay = Vector3<double>(0.0);
}



int ProjectionParametersZoom::AddZoomStep(double zoom, double focallength,
                                          double kc1, double kc2, 
                                          double kc3, double kc4)
{
  if (knownparams_vect_.size()>0) {
    if (knownparams_vect_[knownparams_vect_.size()-1].zoom<zoom) {
      BIASERR("Zoom parameters only in descending order.");
      return -1;
    }
  }
  CPDiscreteParam dp;
  dp.zoom = zoom;
  dp.focallength = focallength;      
  dp.kc1 = kc1;
  dp.kc2 = kc2;
  dp.kc3 = kc3;
  dp.kc4 = kc4;
  knownparams_vect_.push_back(dp);
  SetUnzoomed();
  ComputeK_();        
  return 0;
}


void ProjectionParametersZoom::ComputeK_() {
  
  K_.SetIdentity(); 
  K_[0][0] = focallengthZoom_;
  K_[1][1] = aspectratio_ * focallengthZoom_;
  K_[0][1] = 0;//skew_;
  K_[0][2] = principalX_;
  K_[1][2] = principalY_;
  invK_ = K_.Invert();

}

#ifdef BIAS_HAVE_XML2
 
int ProjectionParametersZoom::XMLGetClassName(std::string& TopLevelTag,
                                                     double& Version) const {
  TopLevelTag = "ProjectionParametersZoom";
  Version = 1.0;
  return 0;
}

 
int ProjectionParametersZoom::XMLOut(const xmlNodePtr Node, 
                                            XMLIO& XMLObject) const {
  xmlNodePtr theNode;
  string TopLevelName;
  double Version;
  ProjectionParametersBase::XMLGetClassName(TopLevelName, Version);
  theNode = XMLObject.addChildNode(Node, TopLevelName);
  XMLObject.addAttribute(theNode, "Version", Version);
  ProjectionParametersBase::XMLOut(theNode, XMLObject);
  xmlNodePtr childNode;
  for (unsigned int i=0; i<knownparams_vect_.size(); i++) {
    childNode = XMLObject.addChildNode(Node,"ZoomStep");
    XMLObject.addAttribute(childNode, "Zoom", knownparams_vect_[i].zoom);
    XMLObject.addAttribute(childNode, "Focallength", 
                           knownparams_vect_[i].focallength);
    XMLObject.addAttribute(childNode, "k1", knownparams_vect_[i].kc1);
    XMLObject.addAttribute(childNode, "k2", knownparams_vect_[i].kc2);
    XMLObject.addAttribute(childNode, "p1", knownparams_vect_[i].kc3);
    XMLObject.addAttribute(childNode, "p2", knownparams_vect_[i].kc4);
  }
  return 0; 
}

 
int ProjectionParametersZoom::XMLIn(const xmlNodePtr Node, 
                                           XMLIO& XMLObject) {
  string TopLevelName;
  double Version;
  ProjectionParametersBase::XMLGetClassName(TopLevelName, Version);

  xmlNodePtr childNode; 
  if ((childNode = XMLObject.getChild(Node, TopLevelName))==NULL) {
    BIASERR("Error in xml, no tag" << TopLevelName);
    return -1;
  }
  if (ProjectionParametersBase::XMLIn(childNode, XMLObject)!=0) return -1;
  knownparams_vect_.clear();
  childNode = XMLObject.getFirstChild(Node);
  while (childNode!=NULL) {
    string nodename = XMLObject.getNodeName(childNode);
    if (nodename.compare("ZoomStep")==0) {
      CPDiscreteParam kpv;
      kpv.zoom = XMLObject.getAttributeValueDouble(childNode, "Zoom");
      kpv.focallength = 
        XMLObject.getAttributeValueDouble(childNode, "Focallength");
      kpv.kc1 = XMLObject.getAttributeValueDouble(childNode, "k1");
      kpv.kc2 = XMLObject.getAttributeValueDouble(childNode, "k2");
      kpv.kc3 = XMLObject.getAttributeValueDouble(childNode, "p1");
      kpv.kc4 = XMLObject.getAttributeValueDouble(childNode, "p2");
      knownparams_vect_.push_back(kpv);
    }
    childNode = XMLObject.getNextChild(childNode);
  }  
  SetUnzoomed();
  return 0;
}

#endif


void ProjectionParametersZoom::InterpolateDistortionFromIndex_(unsigned int i,
                                                          double relnorm){
  //todo: brown: adequate methode finden (lin. interp. ist
  //schlechter)
  
  kc1Zoom_=relnorm*(knownparams_vect_[i].kc1-knownparams_vect_[i-1].kc1)+
    knownparams_vect_[i-1].kc1;

  kc2Zoom_=relnorm*(knownparams_vect_[i].kc2-knownparams_vect_[i-1].kc2)+
    knownparams_vect_[i-1].kc2;
  
  kc3Zoom_=relnorm*(knownparams_vect_[i].kc3-knownparams_vect_[i-1].kc3)+
    knownparams_vect_[i-1].kc3;
  
  kc4Zoom_=relnorm*(knownparams_vect_[i].kc4-knownparams_vect_[i-1].kc4)+
    knownparams_vect_[i-1].kc4;
  
}


double ProjectionParametersZoom::InterpolateFocalLength_(){
  
  double relnorm;
  double absdist;
 
  bool found;
  unsigned int i;
  found=false;

  if (knownparams_vect_[0].zoom<zoom_){ // zoom is higher than the 
    // known values:
    zoom_=knownparams_vect_[0].zoom;  // set to highest available
  }
  
  for (i=0;i<knownparams_vect_.size();i++){
    if (knownparams_vect_[i].zoom<=zoom_){
        found=true;
        break;
    }
  }
  
  if (!found){ // zoom is lower than the known values
    focallengthZoom_=knownparams_vect_[i-1].focallength;
    return knownparams_vect_[i-1].focallength;
  }
  
  if (knownparams_vect_[i].zoom==zoom_){
    // interpolate distortion parameters
    InterpolateDistortionFromIndex_(i+1,0);
    return knownparams_vect_[i].focallength;
  } else {
    relnorm=(zoom_ - knownparams_vect_[i].zoom) /
      (knownparams_vect_[i-1].zoom - knownparams_vect_[i].zoom);
    relnorm=1.0-relnorm;
    // interpolate distortion parameters
    InterpolateDistortionFromIndex_(i,relnorm);
    absdist=relnorm*(knownparams_vect_[i].focallength-
                     knownparams_vect_[i-1].focallength);
    return (absdist+knownparams_vect_[i-1].focallength);
  }
}


bool ProjectionParametersZoom::Distort(HomgPoint2D& point2d) const {
  
  // do nothing in case of ideal lense
  if (kc1Zoom_==0 && kc2Zoom_==0 && kc3Zoom_==0 && kc4Zoom_==0) return true;

  // copy of distorted point
  double distx,disty;
  double x,y;
  double twoxy;
  
  double rsquared;
  double raddist;
  double tandistx, tandisty;
  
  point2d.Homogenize();
  
  // Points must be Bougouet-normalized 
  // (subtract principal point, then divide by focal length)
  x = (point2d[0]-principalX_)/focallengthZoom_;
  y = (point2d[1]-principalY_)/(GetAspectratio()*focallengthZoom_);
      
  twoxy = 2 * x*y;
  rsquared = x*x + y*y;
    
  // radial distortion:
  raddist = 1 + kc1Zoom_*rsquared + kc2Zoom_*rsquared*rsquared;
  // tangential distortion:
  tandistx = kc3Zoom_*twoxy + kc4Zoom_ * (rsquared + 2*x*x);
  tandisty = kc4Zoom_*twoxy + kc3Zoom_ * (rsquared + 2*y*y);
  
  // apply distortion:
  distx = raddist * x + tandistx;
  disty = raddist * y + tandisty;
   
  // invert Bougouet-normalization
  point2d[0] = distx*focallengthZoom_ + principalX_;
  point2d[1] = disty*GetAspectratio()*focallengthZoom_ + principalY_;
  return true;
}


bool ProjectionParametersZoom::Undistort(HomgPoint2D& point2d) const {

  // do nothing in case of ideal lense
  if (kc1Zoom_==0 && kc2Zoom_==0 && kc3Zoom_==0 && kc4Zoom_==0) return true;

// copy of distorted point
  double distx,disty;
  double x,y;
  double twoxy;
  
  double rsquared;
  double raddist;
  double tandistx, tandisty;
  
  point2d.Homogenize();

  // Points must be Bougouet-normalized 
  // (subtract principal point, then divide by focal length)
  x = point2d[0] = (point2d[0]-principalX_)/focallengthZoom_;
  y = point2d[1] = (point2d[1]-principalY_)/(GetAspectratio()*
                                             focallengthZoom_);
  
  // 5 iterations are enough
  for (unsigned int i=0;i<5;i++){
        
    twoxy = 2 * x*y;
    rsquared = x*x + y*y;
    
    // radial distortion:
    raddist = 1 + kc1Zoom_*rsquared + kc2Zoom_*rsquared*rsquared;
    // tangential distortion:
    tandistx = kc3Zoom_*twoxy + kc4Zoom_ * (rsquared + 2*x*x);
    tandisty = kc4Zoom_*twoxy + kc3Zoom_ * (rsquared + 2*y*y);
    
    // apply distortion:
    distx = raddist * x + tandistx;
    disty = raddist * y + tandisty;
    
    // subtract distortion from distorted coordinates
    x = point2d[0] - (distx-x);
    y = point2d[1] - (disty-y);
    
  }

  // invert Bougouet-normalization
  point2d[0] = x*focallengthZoom_ + principalX_;
  point2d[1] = y*GetAspectratio()*focallengthZoom_ + principalY_;
  return true;
}


double ProjectionParametersZoom::InterpolateAbsZoom_(){
  
  double relnorm;
  double absdist;
  
  bool found;
  unsigned int i;
  found=false;
  
  if (knownparams_vect_[0].focallength<focallengthZoom_){
    focallengthZoom_=knownparams_vect_[0].focallength;
  }
  
  for (i=0;i<knownparams_vect_.size();i++){
    if (knownparams_vect_[i].focallength<=focallengthZoom_){
        found=true;
        break;
    }
  }
  if (!found){
    return -1;
  }
  
  if (knownparams_vect_[i].focallength==focallengthZoom_){
    // interpolate distortion parameters
    InterpolateDistortionFromIndex_(i+1,0);
    return knownparams_vect_[i].zoom;
  } else { 
    relnorm=(focallengthZoom_ - knownparams_vect_[i].focallength) /
      (knownparams_vect_[i-1].focallength - knownparams_vect_[i].focallength);
    relnorm=1.0-relnorm;
    // interpolate distortion parameters
    InterpolateDistortionFromIndex_(i,relnorm);
    absdist=relnorm*(knownparams_vect_[i].zoom-
                     knownparams_vect_[i-1].zoom);    
    return (absdist+knownparams_vect_[i-1].zoom);
  }
}