ProjectionParametersPerspective.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/ProjectionParametersPerspective.hh"

#include "SphericalCoordinates.hh"

using namespace BIAS;
using namespace std;

bool ProjectionParametersPerspective::
GetCutOutParameters(const Vector2<int>& centerPoint,
                    const unsigned int halfWidth,
                    const unsigned int halfHeight,
                    ProjectionParametersPerspective& cutOutParams) const
{
  cutOutParams = (*this);
  Vector2<int> UL(centerPoint[0]-halfWidth, centerPoint[1]-halfHeight);
  Vector2<int> LR(centerPoint[0]+halfWidth, centerPoint[1]+halfHeight);
  cutOutParams.width_ = 2 * halfWidth + 1;
  cutOutParams.height_ = 2 * halfHeight + 1;

  cutOutParams.SetPrincipal(principalX_ - UL[0], principalY_ - UL[1]);

  return ((UL[0]>=0) && (UL[0]<(int)width_)  &&
          (UL[1]>=0) && (UL[1]<(int)height_) &&
          (LR[0]>=0) && (LR[0]<(int)width_)  &&
          (LR[1]>=0) && (LR[1]<(int)height_));
}

int ProjectionParametersPerspective::
GetMinimalAngleInCorner_(const HomgPoint2D& corner, double& minAngle)
{
  HomgPoint2D neighbour[3]; //looking for the 3 neighbours of a corner at distance 1 or sqrt(2)
  Vector3<double> ray, p;
  UnProjectLocal(corner, p, ray);
  if(ray.NormL2()<1e-12) return -1;
  ray.Normalize();

  Vector3<double> neighbourRay[3];

  const int x = 0;
  const int y = 1;
  const int diag = 2;

  neighbour[x] = corner; //neighbour in x direction
  neighbour[y] = corner; //neighbour in y direction
  neighbour[diag] = corner; //diagonal neighbour
  if(neighbour[x][0] == 0) {
    neighbour[x][0]+=1;
    neighbour[diag][0]+=1;
  } else {
    neighbour[x][0]-=1;
    neighbour[diag][0]-=1;
  }

  if(neighbour[y][1] == 0) {
    neighbour[y][1]+=1;
    neighbour[diag][1]+=1;
  } else {
    neighbour[y][1]-=1;
    neighbour[diag][1]-=1;
  }

  double angleDiff[3];
  for(unsigned int i=0; i<3; i++) {
    Vector3<double> p;
    UnProjectLocal(neighbour[i], p, neighbourRay[i]);
    if(neighbourRay[i].NormL2() < 1e-12) {
      return -1;
    }
    neighbourRay[i].Normalize();
    angleDiff[i] = acos(ray.ScalarProduct(neighbourRay[i]));
  }

  minAngle = (angleDiff[x] < angleDiff[y]) ? angleDiff[x] : angleDiff[y];
  if(minAngle > angleDiff[diag]) {
    minAngle = angleDiff[diag];
  }

  return 0;

}

// looks into the corners and calculates angle differences to extremal
// optical rays
int ProjectionParametersPerspective::
GetMinimalAngularSamplingStep(double& minAngleStep)
{
  HomgPoint2D corner[4];

  corner[0][0] = 0;
  corner[0][1] = 0;
  corner[0][2] = 1;

  corner[1][0] = 0;
  corner[1][1] = height_-1;
  corner[1][2] = 1;

  corner[2][0] = width_-1;
  corner[2][1] = height_-1;
  corner[2][2] = 1;

  corner[3][0] = width_-1;
  corner[3][1] = 0;
  corner[3][2] = 1.0;

  double minAngle[4];
  for(unsigned int i=0; i<4; i++) {
    if(GetMinimalAngleInCorner_(corner[i], minAngle[i])!=0) {
      return -1;
    }
  }
  double minAngleStepTemp[2];
  minAngleStepTemp[0] = (minAngle[0]<minAngle[1]) ? minAngle[0] : minAngle[1];
  minAngleStepTemp[1] = (minAngle[2]<minAngle[3]) ? minAngle[2] : minAngle[3];

  minAngleStep =
    (minAngleStepTemp[0]<minAngleStepTemp[1]) ?
    minAngleStepTemp[0] : minAngleStepTemp[1];

  return 0;
}

const bool ProjectionParametersPerspective::
DoIntrinsicsDiffer(const ProjectionParametersBase* p) const
{
  if(ProjectionParametersBase::DoIntrinsicsDiffer(p))
    return true;

  const ProjectionParametersPerspective* ppp =
    dynamic_cast<const ProjectionParametersPerspective*>(p);
  if(ppp == NULL)
    return true;

  if(focallength_ != ppp->focallength_)
    return true;

  if(kc1_ != ppp->kc1_ || kc2_ != ppp->kc2_ ||
     kc3_ != ppp->kc3_ || kc4_ != ppp->kc4_)
    return true;

  return false;
}

bool ProjectionParametersPerspective::
DoesPointProjectIntoImageLocal(const Vector3<double>& localX,
                               HomgPoint2D& x,
                               bool IgnoreDistortion) const
{
  // check if in front of camera (max. allowed FoV 168 degree)
  // The FoV check is needed because near zero radial distortion
  // produces really huge numbers
  if(localX[2]/localX.NormL2() > minZlocal_) {
    // now compute coordinates
    x = ProjectLocal(localX, IgnoreDistortion);

    // check if ProjectLocal worked
    if(x[0] == 0.0 && x[1]==0.0 && x[2]==0.0){
      return false;
    }

    // check if in image
    return (x[0]>=-0.5 && x[1]>=-0.5 &&
            x[0]<=(double)(width_)-0.5 && x[1]<=(double)(height_)-0.5 );
  }
  return false;
}


HomgPoint2D ProjectionParametersPerspective::
ProjectLocal(const Vector3<double>& point, bool IgnoreDistortion) const
{
  // make sure we dont project points behind the camera. Also prevent radial
  // distortion from double overflow !
  if (point[2] <= minZlocal_) return HomgPoint2D(0,0,0);

  HomgPoint2D pos2(point), pos;
  if (!IgnoreDistortion) {
    int res = DistortNormalized(pos2);
    if(distType_ == DISTYPE_INVERSE_RAD){
      if(res!=0){
        return HomgPoint2D(0,0,0);
      }
    }
  }
  // in-plane affine coordinate transformation with KMatrix
  K_.Mult(pos2, pos);

  return pos.Homogenize();
}


int ProjectionParametersPerspective::
ProjectLocal(const Vector3<double>& point, HomgPoint2D &p2d,
             bool IgnoreDistortion) const
{
  if (point.NormL2()<1e-6) { // point coincides with camera center
    p2d.Set(0., 0., 0.);
    return -1;
  }
  HomgPoint2D pos2(point);
  if (!IgnoreDistortion) {
    int res = DistortNormalized(pos2);
    if (res!=0) { return -2; }
  }
  // in-plane affine coordinate transformation with KMatrix
  K_.Mult(pos2, p2d);
  p2d.Homogenize();
  return 0;
}


HomgPoint3D ProjectionParametersPerspective::
UnProjectToPointByZ(const HomgPoint2D& pos,
                    const double& zDistance,
                    bool ignoreDistortion) const
{
  Vector3<double> d, p;
  UnProjectLocal(pos, p,d, ignoreDistortion);

  d *= (zDistance/d[2]);

  HomgPoint3D local(d);
  return Pose_.LocalToGlobal(local);
}


HomgPoint3D ProjectionParametersPerspective::
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  ProjectionParametersPerspective::
UnProjectLocal(const HomgPoint2D& cPos, Vector3<double>& pointOnRay,
        Vector3<double>& direction, bool IgnoreDistortion) const {
  HomgPoint2D mpos(cPos); // de-const pos
  pointOnRay = Vector3<double>(0.);
  // radial undistortion here
  if (!IgnoreDistortion) {
    if (!Undistort(mpos)){
      BIASWARNONCE("ProjectionParametersPerspective::UnProjectLocal(): "
           << "Error undistorting point: " << cPos)
      direction = Vector3<double>(0., 0., 0.);
              //mpos = HomgPoint2D(cPos);
    }
  }
  //cout << "distorted: "<<cPos<<"\nundistorted: "<<mpos<<endl;
  // in-plane affine coordinate transformation with KMatrix
  invK_.Mult(mpos, direction);
  const double n = direction.NormL2();
  if (!Equal(n,0.0, 1e-15)){
    direction /= n;
  } else {
    BIASWARNONCE("cannot normalize local ray - division by zero otherwise")
  }
}


int ProjectionParametersPerspective::
DistortNormalized(HomgPoint2D& point2d) const
{
  // do nothing in case of ideal lense
  if (kc1_==0 && kc2_==0 && kc3_==0 && kc4_==0) return 0;
  if (point2d.IsAtInfinity()) { return -1; }
  point2d.Homogenize();
  double dx = -1;
  double dy = -1;
  Distort_(point2d[0], point2d[1], dx, dy);
  if(distType_==DISTYPE_INVERSE_RAD && dx == -1 && dy == -1){
    BIASWARNONCE("Error distorting point with inverse rad distortion" << point2d)
    return -1;
  }
  point2d[0] = dx;
  point2d[1] = dy;
  return 0;
}


bool ProjectionParametersPerspective::
Distort(HomgPoint2D& point2d) const
{
  // do nothing in case of ideal lens
  if (kc1_==0 && kc2_==0 && kc3_==0 && kc4_==0 && r0_==0) return true;

  // copy of distorted point
  /*double distx,disty;
  double x,y;*/
  // this is effectively: (x,y,1) =  K^-1 * point2D
  // Points must be Bougouet-normalized
  // (subtract principal point, then divide by focal length)
  /*x = (point2d[0]-principalX_)/focallength_;
  y = (point2d[1]-principalY_)/(GetAspectratio()*focallength_);*/
  point2d = invK_*point2d;

  //Distort_(x, y, distx, disty);
  Distort_(point2d[0],point2d[1], point2d[0],point2d[1]);
  if(distType_ == DISTYPE_INVERSE_RAD && point2d[0]==-1 && point2d[1]==-1){
    BIASWARN("Error distorting point with inverse rad distortion " << point2d)
    point2d.Set(0,0,0);
    return false;
  }
  
  // invert Bougouet-normalization
  /*point2d[0] = distx*focallength_ + principalX_;
  point2d[1] = disty*GetAspectratio()*focallength_ + principalY_;*/
  point2d = K_*point2d;
  return true;
}


bool ProjectionParametersPerspective::
Undistort(HomgPoint2D& point2d) const
{
  BIASASSERT(!point2d.IsAtInfinity());
  // do nothing in case of ideal lens
  if (kc1_==0 && kc2_==0 && kc3_==0 && kc4_==0 && r0_==0){
    return true;
  }
  
  point2d.Homogenize();

  /*double distx,disty;
  double x,y;*/
  // Points must be Bouguet-normalized
  // (subtract principal point, then divide by focal length)
  point2d = invK_*point2d;
  /*x = point2d[0] = (point2d[0]-principalX_)/focallength_;
  y = point2d[1] = (point2d[1]-principalY_)/(GetAspectratio()*focallength_);*/
  
  if (!Undistort_(point2d[0], point2d[1], point2d[0], point2d[1])){
    point2d.Set(0., 0., 0.);
    return false;
  }
  // invert Bouguet-normalization
  /*point2d[0] = point2d[0]*focallength_ + principalX_;
  point2d[1] = point2d[1]*GetAspectratio()*focallength_ + principalY_;*/
  point2d = K_*point2d;
  return true;
}

std::string ProjectionParametersPerspective::
GetRadialDistModelString(BIAS_ProjParaPersp_DISTORTION_TYPE model){
    switch(model){
    case DISTYPE_NONE:
        return string("None");
    case DISTYPE_RADIAL:
            return string("Bouguet_Radial");
    case DISTYPE_DEF:
        return string("Bouguet_Full");
    case DISTYPE_BROWN:
        return string("Brown");
  case DISTYPE_INVERSE_RAD:
    return string("Inverse_Radial");
    default:
        return string("Unknown");
    }
}

#ifdef BIAS_HAVE_XML2

int ProjectionParametersPerspective::
XMLGetClassName(std::string& TopLevelTag, double& Version) const {
  TopLevelTag = "ProjectionParametersPerspective";
  Version = 0.1;
  return 0;
}

int ProjectionParametersPerspective::
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;
  XMLObject.addAttribute(Node,"Focallength", focallength_);
  XMLObject.addAttribute(Node,"Skew", skew_);
  childNode = XMLObject.addChildNode(Node,string("Distortion"));

  //handling of advanced distortion types
  if(distType_ != DISTYPE_NONE){
    XMLObject.addAttribute(childNode, "k1", kc1_);
    XMLObject.addAttribute(childNode, "k2", kc2_);
    XMLObject.addAttribute(childNode, "p1", kc3_);
    XMLObject.addAttribute(childNode, "p2", kc4_);
  }
  switch(distType_)  {
  case DISTYPE_NONE:
    XMLObject.addAttribute(childNode, "Type", "DISTYPE_NONE");
    break;
  case DISTYPE_BROWN:
    XMLObject.addAttribute(childNode, "Type", "DISTYPE_BROWN");
    XMLObject.addAttribute(childNode, "r0", r0_);
    break;
  case DISTYPE_DEF:
    XMLObject.addAttribute(childNode, "Type", "DISTYPE_DEF");
    break;
  case DISTYPE_RADIAL:
    XMLObject.addAttribute(childNode, "Type", "DISTYPE_RADIAL");
    break;
  case DISTYPE_INVERSE_RAD:
    XMLObject.addAttribute(childNode, "Type", "DISTYPE_INVERSE_RAD");
    break;
  case DISTYPE_RAD3:
    XMLObject.addAttribute(childNode, "Type", "DISTYPE_RAD3");
    break;
  case DISTYPE_INVRAD3:
    XMLObject.addAttribute(childNode, "Type", "DISTYPE_INVRAD3");
    break;
  default:
    BIASWARN("Unknown distortion type. only default parameters have been saved." << distType_);
    break;
  }
  return 0;
}


int ProjectionParametersPerspective::
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;

  focallength_  = XMLObject.getAttributeValueDouble(Node, "Focallength");
  if (XMLObject.getAttributeByName(Node, "Skew")!=NULL)
    skew_  = XMLObject.getAttributeValueDouble(Node, "Skew");
  if ((childNode = XMLObject.getChild(Node, "Distortion"))!=NULL) {
    kc1_ = XMLObject.getAttributeValueDouble(childNode, "k1");
    kc2_ = XMLObject.getAttributeValueDouble(childNode, "k2");
    kc3_ = XMLObject.getAttributeValueDouble(childNode, "p1");
    kc4_ = XMLObject.getAttributeValueDouble(childNode, "p2");

    //check if distortion is not default case
    xmlAttrPtr typeAttr = NULL;
    typeAttr = XMLObject.getAttributeByName(childNode, "Type");
    if(typeAttr!=NULL)
    {
       string typestr;
       typestr = XMLObject.getAttributeValueString(typeAttr);

       if(typestr == string("DISTYPE_NONE"))
       {
         distType_ = DISTYPE_NONE;
       }
       else if(typestr == string("DISTYPE_BROWN"))
       {
          distType_ = DISTYPE_BROWN; //set type of distortion parameters
          r0_ = XMLObject.getAttributeValueDouble(childNode, "r0");
       }
       else if(typestr == string("DISTYPE_DEF"))
       {
          distType_ = DISTYPE_DEF;
       }
       else if(typestr == string("DISTYPE_RADIAL"))
       {
          distType_ = DISTYPE_RADIAL;
       }
       else if(typestr == string("DISTYPE_INVERSE_RAD"))
       {
          distType_ = DISTYPE_INVERSE_RAD;
       }
       else if(typestr == string("DISTYPE_INVRAD3"))
       {
         distType_ = DISTYPE_INVRAD3;
       }
       else if(typestr == string("DISTYPE_RAD3"))
       {
         distType_ = DISTYPE_RAD3;
       }
    }
    else
    {
       distType_ = DISTYPE_DEF;
    }
  }
  ComputeK_();
  return 0;
}

#endif // BIAS_HAVE_XML2

void ProjectionParametersPerspective::
GetIdealK(KMatrix& K) const
{
  unsigned int imageWidth, imageHeight;
  GetImageSize(imageWidth, imageHeight);

  HomgPoint2D p(0, 0);
  Undistort(p);
  p = invK_ * p;
  p.Homogenize();

  double minX = p[0], maxX = p[0], minY = p[1], maxY = p[1];

  for (unsigned int x = 0; x < imageWidth; x++) {
    p = HomgPoint2D(x, 0);
    Undistort(p);
    p = invK_ * p;
    p.Homogenize();
    if (p[0] < minX) minX = p[0];
    if (p[0] > maxX) maxX = p[0];
    if (p[1] < minY) minY = p[1];
    if (p[1] > maxY) maxY = p[1];

    p = HomgPoint2D(x, imageHeight-1);
    Undistort(p);
    p = invK_ * p;
    p.Homogenize();
    if (p[0] < minX) minX = p[0];
    if (p[0] > maxX) maxX = p[0];
    if (p[1] < minY) minY = p[1];
    if (p[1] > maxY) maxY = p[1];
  }

  for (unsigned int y = 0; y < imageHeight; y++) {
    p = HomgPoint2D(0, y);
    Undistort(p);
    p = invK_ * p;
    p.Homogenize();
    if (p[0] < minX) minX = p[0];
    if (p[0] > maxX) maxX = p[0];
    if (p[1] < minY) minY = p[1];
    if (p[1] > maxY) maxY = p[1];

    p = HomgPoint2D(imageWidth-1, y);
    Undistort(p);
    p = invK_ * p;
    p.Homogenize();
    if (p[0] < minX) minX = p[0];
    if (p[0] > maxX) maxX = p[0];
    if (p[1] < minY) minY = p[1];
    if (p[1] > maxY) maxY = p[1];
  }

  double focalLengthX = idealImageWidth_ / (maxX - minX);
  double focalLengthY = idealImageHeight_ / (maxY - minY);
  double principalX = -focalLengthX * minX;
  double principalY = -focalLengthY * minY;

  K.SetIdentity();
  K[0][0] = focalLengthX;
  K[1][1] = focalLengthY;
  K[0][1] = skew_;
  K[0][2] = principalX;
  K[1][2] = principalY;
}

void ProjectionParametersPerspective::
SetIdealImageSize(unsigned int width, unsigned int height)
{
  idealImageWidth_ = width;
  idealImageHeight_ = height;
}

int ProjectionParametersPerspective::
SetIntrinsics(double minPhi, double maxPhi,
              double minTheta, double maxTheta,
              double angleStep, double aspectratio)
{
  bool wrongAngles = false;
  wrongAngles = (minPhi <= -M_PI/2.0) || (maxPhi >= M_PI/2.0) || (minPhi>=maxPhi);
  wrongAngles = wrongAngles || (minTheta <= 0) || (maxTheta >= M_PI)
    || (minTheta>=maxTheta);
  wrongAngles = wrongAngles || (angleStep<=0);
  if(wrongAngles) {
    BIASERR("invalid angle values");
    return -1;
  }

  //establish an spherical coordinate frame that interpretes
  //angles like described in the docu:
  //  phi around the H-Axis relative to the A-Axis and
  //  theta relative to the -H-Axis.
  //For this the internal base of the spherical frame
  //    has to be rotated around the V-Axis by -90Deg.
  SphericalCoordinates sphericalCoord;
  CoordinateTransform3D sphericalBase; // will contain the canonical (cartesian) base,
  // since calculations are performed relative to the local camera frame
  // only rotation mentioned above has to be applied:

  Quaternion<double> rot(0, -sin(M_PI_4), 0, cos(M_PI_4));
  sphericalBase.RotateLocalFrame(rot);
  sphericalCoord.SetAffineBase(sphericalBase);

  //first determine the minimal stepsize within the plane
  double minStepSize = 0;
  // there are three cases of interest:
  //  a. the principle point lies in the image
  //  b. the other case
  bool ppIsInPhiRange = (minPhi<0) && (0<maxPhi);
  bool ppIsInThetaRange = (minTheta<M_PI/2.0) && (M_PI/2.0<maxTheta);
  //case a
  if(ppIsInPhiRange && ppIsInThetaRange){
    minStepSize = tan(angleStep);
  } else {
    //determine angles closest to the pp
    double phiClose = (fabs(minPhi) < fabs(maxPhi)) ? minPhi : maxPhi;
    double thetaClose = (fabs(minTheta-M_PI/2.0) < fabs(maxTheta-M_PI/2.0)) ? minTheta : maxTheta;
    if(ppIsInPhiRange) phiClose = 0.0;
    if(ppIsInThetaRange) thetaClose = M_PI/2.0;

    Vector3<double> p;
    HomgPoint3D ray;
    sphericalCoord.GetCartesianRayFromFullPhi(phiClose, thetaClose, ray);
    ray.GetEuclidean(p);
    p/=p[2]; //delivers point on plane since all is calculated in the local cam-frame,
    //therefore this is now the closest point to the principle axis on the border of the image, within the plane
    //at distance z=1.
    double radius = sqrt(p[0]*p[0] + p[1]*p[1]);
    double alpha = atan(radius);
    minStepSize = tan(alpha + angleStep) - radius;
  }

  double idealSampleDistanceX = minStepSize / sqrt( 1.0 + 1.0/(aspectratio*aspectratio) );
  BIASASSERT(idealSampleDistanceX>0);
  //upper left corner
  Vector3<double> pUL;
  HomgPoint3D rayUL;
  sphericalCoord.GetCartesianRayFromFullPhi(minPhi, minTheta, rayUL);
  rayUL.GetEuclidean(pUL);
  pUL/=pUL[2];
 // cerr<<pUL<<endl;

  //lower right corner
  Vector3<double> pLR;
  HomgPoint3D rayLR;
  sphericalCoord.GetCartesianRayFromFullPhi(maxPhi, maxTheta, rayLR);
  rayLR.GetEuclidean(pLR);
  pLR/=pLR[2];
  //cout<<pLR<<endl;
  //cout<<" determining intrinsics \n";

  // minStepSize is the chosen equidistant stepsize within the image plane
  // at distance of z=1. pUL and pLR code the diagonal corners, hence the
  // visible image size on the image plane at z=1.
  double widthInSpace = pLR[0] - pUL[0];
  BIASASSERT(widthInSpace>0);
  double heightInSpace = pLR[1] - pUL[1];
  BIASASSERT(heightInSpace>0);

  unsigned int width = static_cast<unsigned int>(rint( ceil(widthInSpace/idealSampleDistanceX) ));
  unsigned int height = static_cast<unsigned int>(rint( ceil(aspectratio*heightInSpace/idealSampleDistanceX) ));
  SetImageSize(width, height);
  skew_ = 0;
  focallength_ = static_cast<double>(width)/widthInSpace;
  double focallengthY = static_cast<double>(height)/heightInSpace;
  aspectratio_ = focallengthY/focallength_;

  principalX_ = -focallength_*pUL[0] - 0.5;
  principalY_ = -focallengthY*pUL[1] - 0.5;
  ComputeK_();

  return 0;
}

int ProjectionParametersPerspective::
        SetIntrinsics(Vector3<double>& p, Vector3<double>& q,
                      unsigned int imgWidth, unsigned int imgHeight)
{

    if(imgWidth <= 0 && imgHeight <= 0) {
        return -1;
    }

    // points must lie on a plane orthogonal to the image plane
    if (p[2]!=q[2]) {
        return -2;
    }

    if (p[2]==0) {
        return -3;
    }

    p = p / p[2];
    q = q / q[2];

    SetImageSize(imgWidth, imgHeight);
    skew_ = 0;

    Vector2<double> a(0.5,0.5);
    Vector2<double> b(imgWidth - 0.5,imgHeight - 0.5);

    double k = 0;

    if (p[0]!=0) {
        principalX_ = (b[0]*p[0]-a[0]*q[0]) / (p[0]-q[0]);
        focallength_ = (a[0]-principalX_) / p[0];
    } else {
        principalX_ = a[0];
        focallength_ = (b[0] - principalX_) / q[0];
    }

    if (p[1]!=0) {
        principalY_ = (b[1]*p[1]-a[1]*q[1]) / (p[1]-q[1]);
        k = (a[1]-principalY_) / p[1];
    } else {
        principalY_ = a[1];
        k = (b[1] - principalY_) / q[1];
    }

    aspectratio_ = k / focallength_;

    ComputeK_();

    return 0;
}

void ProjectionParametersPerspective::
GetIdealImageSize(unsigned int& width, unsigned int& height) const
{
  width = idealImageWidth_;
  height = idealImageHeight_;
}

void ProjectionParametersPerspective::
ComputeK_() {
  K_.SetIdentity();
  K_[0][0] = focallength_;
  K_[1][1] = aspectratio_ * focallength_;
  K_[0][1] = skew_;
  K_[0][2] = principalX_;
  K_[1][2] = principalY_;
  invK_ = K_.Invert();
}

////////////////////////////////////////////////////////////////////////////////////
int ProjectionParametersPerspective::
TransformCartesianToPolarCoordinates(const Image<float>& cartesianDepth, Image<float>& polarDepth){
    unsigned w = cartesianDepth.GetWidth();
    unsigned h = cartesianDepth.GetHeight();
    unsigned c = cartesianDepth.GetChannelCount();

    if(polarDepth.IsEmpty()) polarDepth.Init(w,h,c);
    else if(polarDepth.GetWidth() != w || polarDepth.GetHeight() != h) polarDepth.ReInit(w,h,c);

    float ** idaPol = polarDepth.GetImageDataArray();
    const float ** idaCart = cartesianDepth.GetImageDataArray();

//  for(unsigned y=0;y<h;y++){
//      for(unsigned x=0;x<w;x++){
//          for(unsigned k=0;k<c;k++){
//              idaPol[y][x*c+k] =
//                      TransformCartesianToPolarCoordinates(HomgPoint2D(x,y,1.0),idaCart[y][x*c+k]);
//          }
//      }
//  }

    double px=0,py=0; GetPrincipal(px,py);
    double fx=0,fy=0; GetFocalLength(fx); fy=fx*GetAspectratio();
    for(unsigned y=0;y<h;y++){
        for(unsigned x=0;x<w;x++){
            float alpha = atan(float((x-px)/fx));
            float beta = atan(float((py-y)/ sqrt( pow((x-px)*(fy/fx),2) + pow(fy,2))));

            for(unsigned k=0;k<c;k++){
                idaPol[y][x*c+k] = idaCart[y][x*c+k]/(cos(alpha)*cos(beta));
            }
        }
    }
    return 0;
}

float ProjectionParametersPerspective::
TransformCartesianToPolarCoordinates(const HomgPoint2D& pos, const float depthCartesian){

//  Vector3<double> pl = UnProjectLocal(pos);
//  BIASASSERT(!Equal(pl[2], 0.0));
//  pl /= pl[2];
//  return depthCartesian * pl.NormL2();

    double px=0,py=0; GetPrincipal(px,py);
    double fx=0,fy=0; GetFocalLength(fx); fy=fx*GetAspectratio();

    float alpha = atan(float((pos[0]-px)/fx));
    float beta = atan(float((py-pos[1])/ sqrt( pow((pos[0]-px)*(fy/fx),2) + pow(fy,2))));

    return depthCartesian/(cos(alpha)*cos(beta));
}

////////////////////////////////////////////////////////////////////////////////////
int ProjectionParametersPerspective::
TransformPolarToCartesianCoordinates(const Image<float>& polarDepth, Image<float>& cartesianDepth){
    unsigned w = polarDepth.GetWidth();
    unsigned h = polarDepth.GetHeight();
    unsigned c = polarDepth.GetChannelCount();

    if(cartesianDepth.IsEmpty())
        cartesianDepth.Init(w,h,c);
    else if(cartesianDepth.GetWidth() != w || cartesianDepth.GetHeight() != h ||
                    cartesianDepth.GetChannelCount() != c)
        cartesianDepth.ReInit(w,h,c);

    const float ** idaPol = polarDepth.GetImageDataArray();
    float ** idaCart = cartesianDepth.GetImageDataArray();

//  for(unsigned y=0;y<h;y++){
//      for(unsigned x=0;x<w;x++){
//          for(unsigned k=0;k<c;k++){
//              idaCart[y][x*c+k] =
//                      TransformPolarToCartesianCoordinates(HomgPoint2D(x,y,1.0),idaPol[y][x*c+k]);
//          }
//      }
//  }

    double px=0,py=0; GetPrincipal(px,py);
    double fx=0,fy=0; GetFocalLength(fx); fy=fx*GetAspectratio();
    for(unsigned y=0;y<h;y++){
        for(unsigned x=0;x<w;x++){
            float alpha = atan(float((x-px)/fx));
            float beta = atan(float((py-y)/ sqrt( pow((x-px)*(fy/fx),2) + pow(fy,2))));
            for(unsigned k=0;k<c;k++){
                idaCart[y][x*c+k] = cos(alpha)*cos(beta)*idaPol[y][x*c+k];
            }
        }
    }
    return 0;
}

float ProjectionParametersPerspective::
TransformPolarToCartesianCoordinates(const HomgPoint2D& pos, const float depthPolar){
//
//  Vector3<double> ray;
//  ray = UnProjectToRay(pos);
//  ray.Normalize();
//  ray.MultIP(depthPolar);
//  return ray[2];

    double px=0,py=0; GetPrincipal(px,py);
    double fx=0,fy=0; GetFocalLength(fx); fy=fx*GetAspectratio();
    float alpha = atan(float((pos[0]-px)/fx));
    float beta = atan(float((py-pos[1])/ sqrt( pow((pos[0]-px)*(fy/fx),2) + pow(fy,2))));
    return cos(alpha)*cos(beta)*depthPolar;
}
/////////////////////////////////////////////////////////////////////////////////////
double ProjectionParametersPerspective::
ViewDifference(const ProjectionParametersBase* pPPB) const {
  const ProjectionParametersPerspective *pPPP =
     dynamic_cast<const ProjectionParametersPerspective* >(pPPB);
  // different projection model has a very different view
  if (pPPP==NULL) {
    BIASERR("View difference for different projection models requested !");
    return DBL_MAX;
  }

  Vector3<double> C0 = pPPP->GetC(), A0, A;

  RMatrix R = GetR(), R0 = pPPP->GetR();

  A[0] = R[0][2]; A[1] = R[1][2]; A[2] = R[2][2];
  A.Normalize();

  A0[0] = R0[0][2]; A0[1] = R0[1][2]; A0[2] = R0[2][2];
  A0.Normalize();

  // compute field of view, assuming const K !
  KMatrix K0 =  GetK();
  double f = 0.5*(K0[0][0] + K0[1][1]);
  double halfw = double(width_ + height_) / 4.0;
  double coshalffieldofview = cos(atan(halfw/f));

  double newness = (GetC()-C0).NormL2();
  newness += 1.0/(1.0-coshalffieldofview)*(1.0-fabs(A.ScalarProduct(A0)));
  return newness;
}


Matrix3x3<double> ProjectionParametersPerspective::
UnProjectCovLocal(const HomgPoint2D& pos, const Matrix3x3<double>& cov2D,
                  bool IgnoreDistortion, bool Normalize)
{
  if (!IgnoreDistortion || Normalize){
    return ProjectionParametersBase::
      UnProjectCovLocal(pos, cov2D, IgnoreDistortion, Normalize);
  }
  // cout << "goooooood\n";
  // linear error propagation:
  // the function for the point is given by pu = Ki * p
  // and hence the Jacobian J is given by Ki
  // the cov transforms by J * cov * J^T = Ki * cov * KiT
  Matrix3x3<double> Ki = K_.Invert(), KiT = Ki.Transpose(), tmp, tmp2;

  Ki.Mult(cov2D, tmp);
  tmp.Mult(KiT, tmp2);

  return tmp2;
}

void ProjectionParametersPerspective::
UpdateMinZLocal()
{

  minZlocal_ = 1e100; // 179.9... deg fov

  // run over all border pixels of image
  PixelIterator it;
  GetFirstBorderPixel(it);
  do {
    // compute ray in camera coordinate system
    Vector3<double> ray, p;
    UnProjectLocal(HomgPoint2D(it.x, it.y), p, ray);
    BIASASSERT(Equal(ray.NormL2(), 1.0, 1e-8));
    if (ray[2] < minZlocal_) {
      minZlocal_ = ray[2];
    }
  } while (GetNextBorderPixel(it));
  // add 3% safety margin
  if (minZlocal_<1e-10) minZlocal_=1e-10;
  else minZlocal_ *= 0.97;

  cout<<"maximum viewing angle of this camera is "
      <<acos(minZlocal_)*180.0/M_PI<<" degrees"
      <<endl;
}



Matrix4x4<double>
ProjectionParametersPerspective::
GetGLModelviewProjectionMatrix(const double zNear,
             const double zFar,
             const bool flip,
             const bool transpose)
{

  Matrix4x4<double> projGL = GetGLProjectionMatrix(zNear, zFar, flip);
  Matrix4x4<double> res;

  projGL.Mult(Pose_.GetMatrix4x4(), res);
  if(transpose)
    res.Transpose();
  return res;

}

Matrix4x4<double>
ProjectionParametersPerspective::
GetGLProjectionMatrix(const double zNear,
                      const double zFar,
                      const bool flip,
                      const bool transpose)
{
  return GetGLProjectionMatrix(zNear, zFar, Vector2<unsigned int>(0,0), width_, height_, flip);
}

Matrix4x4<double>
ProjectionParametersPerspective::
GetGLProjectionMatrix(const double zNear,
                      const double zFar,
                      const Vector2<unsigned int> upperLeft,
                      const unsigned int width,
                      const unsigned int height,
                      const bool flip,
                      const bool transpose)
{
  BIASASSERT(zNear<zFar);
  BIASASSERT(zNear>0.0);

  KMatrix K;

//   if (useIdealK) {
//     params.GetIdealK(K);
//   } else   {
    K = GetK();
//   }

  Matrix4x4<double> embededK;
  embededK.SetZero();
  for(unsigned int i=0; i<3; i++) {
    for(unsigned int j=0; j<3; j++) {
      embededK[i][j] = K[i][j];//
    }
    //embededK[i][3] = K[i][2];//principle point
  }
  embededK[2][2] = 1;//propagating z to new z
  embededK[3][3] = 1;//propagating w to new w

 // std::cout<<"embededK = "<<embededK<<std::endl;

  const double x0 = static_cast<double>(upperLeft[0]);
  const double y0 = static_cast<double>(upperLeft[1]);

  Matrix4x4<double> normalizingK;
  normalizingK.SetZero();
  normalizingK[0][0] = 2.0/static_cast<double>(width); //kxx
  normalizingK[0][2] = (1.0 - 2.0*x0)/static_cast<double>(width) - 1.0; //kxz
  if(!flip) {
    normalizingK[1][1] = -2.0/static_cast<double>(height); //kyy
    normalizingK[1][2] = (2.0*y0-1.0)/static_cast<double>(height) + 1.0; //kyz
  } else {
    normalizingK[1][1] = 2.0/static_cast<double>(height); //kyy
    normalizingK[1][2] = (1.0-2.0*y0)/static_cast<double>(height) - 1.0; //kyz
  }
  normalizingK[2][2] = (zFar + zNear)/(zFar - zNear); //kzz
  normalizingK[2][3] = -(1+normalizingK[2][2])*zNear; //kzw

  normalizingK[3][2] = 1.0;

 // std::cout<<"normalizingK = "<<normalizingK<<std::endl;

  Matrix4x4<double> res = normalizingK*embededK;
  if(transpose) res.Transpose();
  return res;
}