ProjectionParametersGreatCircles.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/ProjectionParametersGreatCircles.hh"
#include <Base/Common/CompareFloatingPoint.hh>
#include <math.h>

using namespace BIAS;
using namespace std;

#define M_PI_TOL M_PI*1.1
#define M_PI_2_TOL M_PI_2*1.1


double ProjectionParametersGreatCircles::
GetFocallengthTheta()
{
  return focallengthTheta_;
}

bool ProjectionParametersGreatCircles::
Distort(BIAS::HomgPoint2D& point2d) const
{
  BIASERR("unfinished");
  BIASABORT;
  return false;
}

bool ProjectionParametersGreatCircles::
Undistort(BIAS::HomgPoint2D& point2d) const
{
  BIASERR("unfinished");
  BIASABORT;
  return false;
}


bool ProjectionParametersGreatCircles::
LocalEuclideanCamCoordinates2SphericalAngles(const BIAS::Vector3<double>& euc,
                                             double& phi,
                                             double& theta)
{
  //component x corresponds to H
  const double x = euc[0];
  //component y corresponds to V
  const double y = euc[1];
  //component z corresponds to A
  const double z = euc[2];

  double l = euc.NormL2();
  if((l<1e-12) || (fabs(z)<1e-12 && fabs(y)<1e-12)) {
    phi = 0;
    theta = 0;
    return false;
  }

  phi = atan2(y,z);
  theta = acos(-x/l);
  return true;
}

bool ProjectionParametersGreatCircles::
SphericalAngles2LocalEuclideanRay(const double phi,
                                  const double theta,
                                  BIAS::Vector3<double>& ray)
{
  //BIASASSERT(phi>=-M_PI && phi<=M_PI);
  //BIASASSERT(theta=0 && theta<=M_PI);
  ray.Set(0,0,0);
  if(phi<-M_PI || phi>M_PI || theta<0 || theta>M_PI)
    return false;

  if(theta<=0) {
    ray.Set(-1, 0, 0);
    return false; //result is not unique;
  }
  if(theta>=M_PI) {
    ray.Set(1, 0, 0);
    return false; //result is not unique;
  }
  ray[0] = -cos(theta);//the sign is correct! see above.
  ray[1] = sin(theta)*sin(phi);
  ray[2] = sin(theta)*cos(phi);
  return true;
}

void ProjectionParametersGreatCircles::
Angle2Image(double phi, double theta, BIAS::HomgPoint2D& imgPoint) const
{
  imgPoint[0] = theta*focallengthTheta_ + principalX_;
  imgPoint[1] = phi*focallengthTheta_*aspectratio_ + principalY_;
  imgPoint[2] = 1.0;
}

void ProjectionParametersGreatCircles::
Image2Angle(const BIAS::HomgPoint2D& imgPoint,
            double& phi, double& theta) const
{
  theta = (imgPoint[0]-principalX_)/focallengthTheta_;
  phi   = (imgPoint[1]-principalY_)/(focallengthTheta_*aspectratio_);
}

void ProjectionParametersGreatCircles::
SetInternals(const double minPhi, const double maxPhi,
             const double minTheta, const double maxTheta,
             const unsigned int imgWidth, const unsigned int imgHeight)
{
  BIASASSERT(minPhi>=-M_PI && minPhi<=M_PI);
  BIASASSERT(minTheta>=0 && maxTheta<=M_PI);
  BIASASSERT(maxTheta>minTheta);
  BIASASSERT(maxTheta<=M_PI);

  BIASASSERT(maxPhi>minPhi);
  BIASASSERT(maxPhi<=M_PI);

  //BIASASSERT(imgWidth>0);
  //BIASASSERT(imgHeight>0);

  width_ = imgWidth;
  height_ = imgHeight;

  focallengthTheta_ = width_/(maxTheta-minTheta);
  principalX_ = -focallengthTheta_*minTheta-0.5;

  double focallengthPhi = height_/(maxPhi-minPhi);
  if (width_ > 0)
    aspectratio_ = focallengthPhi/focallengthTheta_;
  else
    aspectratio_ = 1.0;
  //produce same rounding errors like in other methods:
  principalY_ = -focallengthTheta_*aspectratio_*minPhi-0.5;
}


ProjectionParametersGreatCircles& ProjectionParametersGreatCircles::
operator=(const ProjectionParametersGreatCircles& P)
{
  ProjectionParametersBase::operator=(P);
  focallengthTheta_ = P.focallengthTheta_;
  return *this;
}

void ProjectionParametersGreatCircles::
Rescale(double ratio, const double offset)
{
  // attention - offset is being ignored

  double deltaTheta = width_/focallengthTheta_;
  double deltaPhi = height_/(focallengthTheta_*aspectratio_);
  principalX_+=0.5;
  principalX_/=focallengthTheta_;
  principalY_+=0.5;
  principalY_/=(focallengthTheta_*aspectratio_);

  double widthTemp = (double)width_/ratio;
  double heightTemp = (double)height_/ratio;
  width_  = (int)rint(widthTemp);
  height_ =  (int)rint(heightTemp);
  focallengthTheta_ = width_/deltaTheta;
  double focallengthPhi = height_/deltaPhi;
  aspectratio_ = focallengthPhi/focallengthTheta_;

  principalX_*=focallengthTheta_;
  principalY_*=(focallengthTheta_*aspectratio_);
  principalX_-=0.5;
  principalY_-=0.5;
}


void ProjectionParametersGreatCircles::
Rescale(unsigned int width, unsigned int height)
{
  // attention - offset is being ignored

  double deltaTheta = width_/focallengthTheta_;
  double deltaPhi = height_/(focallengthTheta_*aspectratio_);
  principalX_+=0.5;
  principalX_/=focallengthTheta_;
  principalY_+=0.5;
  principalY_/=(focallengthTheta_*aspectratio_);

  double widthTemp = (double)width_;
  double heightTemp = (double)height_;
  width_  = (int)rint(widthTemp);
  height_ =  (int)rint(heightTemp);
  focallengthTheta_ = width_/deltaTheta;
  double focallengthPhi = height_/deltaPhi;
  aspectratio_ = focallengthPhi/focallengthTheta_;

  principalX_*=focallengthTheta_;
  principalY_*=(focallengthTheta_*aspectratio_);
  principalX_-=0.5;
  principalY_-=0.5;
}


bool ProjectionParametersGreatCircles::
GetAngleInterval(const HomgPoint3D& worldPoint,
                 const double halfAngleRange,
                 double& thetaMin, double& thetaMax,
                 double& phiMin, double& phiMax) const
{
  //first determine angles of worldPoint
  Vector3<double> localEucPoint;
  Pose_.GlobalToLocal(worldPoint).GetEuclidean(localEucPoint);
  double phi, theta;
  if(!LocalEuclideanCamCoordinates2SphericalAngles(localEucPoint,
                                                   phi, theta)) {
    BIASERR("unable to transform given point into angle representation!");
    return false;
  }

  double thetaExtendedMin = theta - halfAngleRange;
  double thetaExtendedMax = theta + halfAngleRange;

  //clamp to [0, M_PI]
  thetaMin = (thetaExtendedMin<=0) ? 0 : thetaExtendedMin;
  thetaMax = (thetaExtendedMax>=M_PI) ? M_PI : thetaExtendedMax;

  //if theta in [0,M_PI] is crossed consequently phi in [-M_PI, M_PI]
  if((thetaExtendedMin<=0) || (thetaExtendedMax>=M_PI)) {
    phiMin = -M_PI;
    phiMax = M_PI;
    return true;
  }

  double phiExtendedMin = phi - halfAngleRange;
  if(phiExtendedMin < -M_PI) {
    phiMin = -M_PI;
    phiMax = M_PI;
    return true;
  }

  double phiExtendedMax = phi + halfAngleRange;
  if(phiExtendedMax > M_PI) {
    phiMin = -M_PI;
    phiMax = M_PI;
    return true;
  }

  phiMin = phiExtendedMin;
  phiMax = phiExtendedMax;

  return true;
}

bool ProjectionParametersGreatCircles::
SetAngleInterval(const HomgPoint3D& worldPoint,
                      const double& halfAngleRange)
{
  bool result;
  double thetaMin, thetaMax, phiMin, phiMax;
  result = GetAngleInterval(worldPoint,
                            halfAngleRange,
                            thetaMin, thetaMax, phiMin, phiMax);
  if(result) {
    SetInternals(phiMin, phiMax,
                 thetaMin, thetaMax,
                 width_, height_);
  }

  return result;
}


const bool ProjectionParametersGreatCircles::
DoIntrinsicsDiffer(const ProjectionParametersBase* p) const
{
  const ProjectionParametersGreatCircles* ppbc =
    dynamic_cast<const ProjectionParametersGreatCircles*>(p);
  if(ppbc == NULL)
    return true;

  if(ppbc->focallengthTheta_ != focallengthTheta_)
    return true;

  if(ProjectionParametersBase::DoIntrinsicsDiffer(p))
    return true;



  return false;
}


bool ProjectionParametersGreatCircles::
DoesPointProjectIntoImageLocal(const Vector3<double>& localX,
                               HomgPoint2D& x,
                               bool IgnoreDistortion) const
{
  //an ray intersects the image plane if it maps to

  if(localX[2] > 0.0) {
    x = ProjectLocal(localX, IgnoreDistortion);
    if(Equal(x[2], 0.0))
      return false;
    return (x[0]>=0.0 && x[1]>=0.0 &&
            x[0]<=(double)(width_-1) && x[1]<=(double)(height_-1) );
  }
  return false;
}


HomgPoint2D ProjectionParametersGreatCircles::
ProjectLocal(const Vector3<double>& point, bool IgnoreDistortion) const
{
  HomgPoint2D res(0,0,0);
  double theta, phi;
  if(LocalEuclideanCamCoordinates2SphericalAngles(point, phi, theta)) {
    //successfull transformation to angles:
    Angle2Image(phi, theta, res);
  }
  return res;

  /*  if(CamCoordVectorToShiftedAngles_(point, theta, phi) != 0) {
    return HomgPoint2D(0,0,0);
    }

    return HomgPoint2D(ShiftedAnglesToImage(theta, phi)); */

}

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



HomgPoint3D ProjectionParametersGreatCircles::
UnProjectToImagePlane(const HomgPoint2D& pos,
                      const double& depth,
                      bool ignoreDistortion) const
{
  Vector3<double> d, p;
  UnProjectLocal(pos, p, d, ignoreDistortion);
  double norm = d.NormL2();
  if (Equal(norm, 0.0))
    return HomgPoint3D(0,0,0,0);
  HomgPoint3D local(d * (depth/norm));
  local[3] = 1.0;
  return Pose_.LocalToGlobal(local);
}


void ProjectionParametersGreatCircles::
UnProjectLocal(const HomgPoint2D& cPos, Vector3<double>& pointOnRay,
          Vector3<double>& direction, bool ignoreDistortion) const {
  double theta, phi;
  direction = Vector3<double>(0.0);
  Image2Angle(cPos, phi, theta);
  SphericalAngles2LocalEuclideanRay(phi, theta, direction);
  pointOnRay = Vector3<double>(0.0);
}

#ifdef BIAS_HAVE_XML2

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

int ProjectionParametersGreatCircles::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);

  XMLObject.addAttribute(Node,"focallengthTheta", focallengthTheta_);
  return 0;
 }


int ProjectionParametersGreatCircles::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;
  focallengthTheta_ = XMLObject.getAttributeValueDouble(Node,
                                                        "focallengthTheta");
  return 0;
}

#endif // BIAS_HAVE_XML2