ProjectionParametersPerspective.hh

/*
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
*/


#ifndef __BIAS_ProjectionParametersPerspective_hh__
#define __BIAS_ProjectionParametersPerspective_hh__
#include "bias_config.h"

#include <Base/Common/BIASpragmaStart.hh>
#include <Geometry/ProjectionParametersBase.hh>
#include <Base/Geometry/KMatrix.hh>
#include <Geometry/PMatrix.hh>
#include <Base/Image/Image.hh>

#define LUT_RES 10
#define UNDISTORT_ITERS 20

namespace BIAS {

  enum BIAS_ProjParaPersp_DISTORTION_TYPE
    {
      DISTYPE_RADIAL = 0,     // standard distortion, only radial parameters
      DISTYPE_DEF=1,          // standard distortion
      DISTYPE_BROWN=2,        // brown distortion
      DISTYPE_NONE=3,         // No radial distortion
      DISTYPE_INVERSE_RAD=4,  // like radial distortion, but applied inversely    
      DISTYPE_RAD3=5,         // radial distortion with 3 coefficients
      DISTYPE_INVRAD3=6       // inverse radial distortion with 3 coefficients
    };

  /** @class ProjectionParametersPerspective
   *  @ingroup g_geometry
   *  @brief camera parameters which define the mapping between rays in the
   *  camera coordinate system and pixels in the image as well as external pose
   *  @author koeser/evers/streckel */
  class BIASGeometry_EXPORT ProjectionParametersPerspective
    : public  ProjectionParametersBase {
  public:

    ProjectionParametersPerspective(const unsigned int width = 0,
                                    const unsigned int height = 0)
      : ProjectionParametersBase(width, height)
    {
      InitParams_();
    }

    explicit ProjectionParametersPerspective(const BIAS::PMatrix& P,
                                             const unsigned int width = 10000,
                                             const unsigned int height = 10000)
      : ProjectionParametersBase(width, height)
    {
      InitParams_();
      SetP(P);
    }

    ProjectionParametersPerspective(const ProjectionParametersPerspective & P){
      *this = P;
    };

    ProjectionParametersPerspective&
      operator=(const ProjectionParametersPerspective & P){
      ProjectionParametersBase::operator=(P);
      this->K_ = P.K_;
      this->invK_ = P.invK_;
      this->focallength_= P.focallength_;
      this->skew_=P.skew_;
      this->kc1_= P.kc1_;
      this->kc2_= P.kc2_;
      this->kc3_= P.kc3_;
      this->kc4_= P.kc4_;
      this->r0_ = P.r0_;
      this->distType_ = P.distType_;
      this->idealImageWidth_ = P.idealImageWidth_;
      this->idealImageHeight_ = P.idealImageHeight_;
      this->minZlocal_ =  P.minZlocal_;
      return *this;
    };


    /** Method calculates K-Matrix and image size from specified angles.
     * Angle phi is determining the opening angle and the principle point shift in y direction,
     * while theta is determining the opening angle and the principle point shift in x direction.
     * angleStep is used to derive the resolution, hence the focallength and the aspectratio.
     * The K-Matrix is free of skew.
     *
     * Valid values for phi are (-M_PI/2, M_PI/2), while theta must lie within (0,M_PI).
     * Phi's reference direction is the A-Vector, theta's is the -H-Vector (negated H-Vector).
     * The extrinsics are left unchanged (which you certainly guessed already).
     *
     * \returns -1 if angle values made no sense to the method.
     *
     * \author bartczak 10/2007
     **/
    int SetIntrinsics(double minPhi, double maxPhi,
                      double minTheta, double maxTheta,
                      double angleStep, double aspectratio = 1.0);

    int SetIntrinsics(Vector3<double>& p, Vector3<double>& q,
                      unsigned int width, unsigned int height);

    virtual ~ProjectionParametersPerspective(){}

    virtual int GetMinimalAngularSamplingStep(double& minAngleStep);

    virtual const bool DoIntrinsicsDiffer(const ProjectionParametersBase* p) const;


    bool DoesPointProjectIntoImageLocal(const Vector3<double>& localX,
                                        HomgPoint2D& x,
                                        bool IgnoreDistortion = false) const;


    /** @brief calculates the projection of a point
        in the camera coordinate system
        to a pixel in the image plane of the camera
        with lens distortion
        In the simplest case perspective pinhole projection x = K * y
        where y is the projection of X using  y = (R^T | -R^T C) X
    */
    virtual HomgPoint2D ProjectLocal(const Vector3<double>& point,
                                     bool IgnoreDistortion = false) const;

    /** @brief calculates the projection of a point in the local camera
        coordinate system to a pixel in the image plane of the camera. The
        only case when this function may not compute the 2d point is when the
        camera center and the 3d point coincide. This case must be indicated
        by a negative return value. In all other cases, a 2d point must be
        computed, *particularily* when the 3d point is behind the camera
        or when the resulting 2d point is at infinity.

        In the simplest case perspective pinhole projection x = K * point
        where point is transformed of X using  point = (R^T | -R^T C) X
        @author woelk 08/2008 (c) www.vision-n.de */
    virtual int ProjectLocal(const Vector3<double>& point, HomgPoint2D &p2d,
                             bool IgnoreDistortion = false) const;

    /** @brief map points from image onto unit diameter image plane in 3D.
     * Chosen is the image plane with distance of one from the image center.
     */
    HomgPoint3D UnProjectToImagePlane(const HomgPoint2D& pos,
                                      const double& depth = 1.0,
                                      bool IgnoreDistortion = false) const;


    /** @brief calculates the viewing ray from the camera center (in the
        camera coordinate system) which belongs to the image position pos
        with lens distortion
        e.g. (0,0,1) means optical axis
    */
    virtual void UnProjectLocal(const HomgPoint2D& pos,
                                Vector3<double>& pointOnRay,
                                Vector3<double>& direction,
                                bool IgnoreDistortion = false)const;

    /** @brief unprojects the covariance matrices to K=Identity camera
        Fast implementation for IgnorDistortion = true and Normalize = false
        @author woelk 11/2007 */
    virtual Matrix3x3<double>
      UnProjectCovLocal(const HomgPoint2D& pos, const Matrix3x3<double>& cov2D,
                        bool IgnoreDistortion = false, bool Normalize = false);


    /** **/
    virtual HomgPoint3D UnProjectToPointByZ(const HomgPoint2D& pos,
                                            const double& zDistance,
                                            bool IgnoreDistortion = false) const;

    /** @brief Using the Matlab camera calibration toolbox parameters
        for an undistortion of distorted coordinates.
        TODO: faster implementation via Lookup-table
        @author streckel 06/2004 */
    virtual bool Undistort(BIAS::HomgPoint2D& point2d) const;

    /** @brief Using the Matlab camera calibration toolbox parameters
        for a distortion of undistorted coordinates. Inverse to the
        undistort function.
        TODO: faster implementation via Lookup-table
        @author streckel 06/2004 */
    virtual bool Distort(BIAS::HomgPoint2D& point2d) const;

    /** @brief Undistort distorted coordinates using the
        standard distortion parameters and the parameter describing the
        second constant root of the distortion polynomial of the
        radial symmetric distortion.
        In contrast to the the bouguet way of calculating the radial symmetric
        distortion by using the polynomial
        raddist = 1 + kc1 * r^2 + kc2 * r^4
        the "brown" way uses a modyfied polynomial which has a second
        root. the second constant root of the polynomial is implemented by
        subtracting the constant term rc from raddist with
        rc = kc1 * r0^2 + kc2 * r0^4
        r0 is the parameter describing the root of the polynomial.
        (For details see: Luhmann, Nahbereichsphotogrammetrie. page. 121 )
        @author grauel 09/2007 */
    //void UndistortBrown(HomgPoint2D& point2d) const;

    /** @brief Distort undistorted coordinates using the

        @author grauel 09/2007 */
    //void DistortBrown(HomgPoint2D& point2d) const;

    /** @brief distortion of a normalized point
        @author woelk 11/2006 */
    int DistortNormalized(BIAS::HomgPoint2D& point2d) const;

    /** @brief covariant virtual copy constructor for use in Projection */
    virtual ProjectionParametersPerspective* Clone() const {
      return new ProjectionParametersPerspective(*this);
    }

    /** Shifts the principle point so that imagePoint-(halfWidth, halfHeight)
     * is mapped to (0,0). Image size is adapted to halfWidth*2+1 and
     * halfHeight*2+1. Distortion parameters are applied in normalized coords
     * hence they keep their validity.
     * \returns true only if whole cutout lies in image.
     */
    bool GetCutOutParameters(const Vector2<int>& centerPoint,
                             const unsigned int halfWidth,
                             const unsigned int halfHeight,
                             ProjectionParametersPerspective& cutOutParams)
      const;

    /** @brief Get the current camera focal length.
        @author buck */
    inline void GetFocalLength(double& f) const {
      f = focallength_;
    }

    /**
     * @brief  Returns the focal length.
     * @author rwulff
     * @date   07/2010
     */
    inline double GetFocalLength() const {
      return focallength_;
    }

    inline void GetUndistortion(double& kc1, double& kc2,
                                double& kc3, double& kc4) const
    {
      kc1 = kc1_;
      kc2 = kc2_;
      kc3 = kc3_;
      kc4 = kc4_;
    }

    inline void GetUndistortion(double& kc1, double& kc2,
                                double& kc3, double& kc4,
                                double r0) const
    {
      kc1 = kc1_;
      kc2 = kc2_;
      kc3 = kc3_;
      kc4 = kc4_;
      r0 = r0_;
    }

    inline bool IsDistorted() const {
      return ((kc1_!=0.0) || (kc2_!=0.0) || (kc3_!=0.0) || (kc4_!=0.0));
    }

    /** @brief Sets the parameters for a simple perspective camera with
        imagesize 512x512, focal length 512 (fov = 90 deg.),
        princ.point (255.5,255.5) and without distortion.
    */
    inline void SetSimplePerspective(const double FoV = M_PI/2,
                                     const unsigned int width = 512,
                                     const unsigned int height = 512) {
      SetImageSize(width, height);
      focallength_ = double(width)/(2.0*tan(FoV/2.0));
      aspectratio_ = 1.0;
      principalX_ = 0.5*double(width-1);
      principalY_ = 0.5*double(height-1);
      //K_.SetIdentity();
      ////set focal length
      //K_[0][0] = K_[1][1] = 512.0;
      //set princ. point
      // K_[0][2] = K_[1][2] = 255.5;
      //invK_ = K_.Invert();
      //focallength_=512.0;
      skew_=0;
      kc1_=0;
      kc2_=0;
      kc3_=0;
      kc4_=0;
      r0_=0;
      distType_=DISTYPE_DEF;
      minZlocal_ = 0.1;
      ComputeK_();
    }

    /** @brief Set the current camera focal length in pixel and the a
        spect ratio */
    inline void SetFocalLengthAndAspect(double f,
                                        double AspectRatio) {
      focallength_ = f;
      ProjectionParametersBase::SetAspectratio(AspectRatio);
      ComputeK_();
    }

    /** @brief Set the current camera skew factor */
    inline void SetSkew(double skew) {
      skew_ = skew;
      ComputeK_();
    }

    /** @brief Obtain the current camera skew factor */
    inline double GetSkew() const {
      return skew_;
    }

    /** @brief Set principal point in pixels relative to top left corner,
        virtual overload to recalculate K_
        @author streckel */
    virtual void SetPrincipal(const double x, const double y) {
      ProjectionParametersBase::SetPrincipal(x, y);
      ComputeK_();
    }

    /** @brief Set CCD cell-size in meter, virtual overload to
        recalculate K_.
        @author streckel */
    virtual void SetAspectratio(const double AspectRatio) {
      ProjectionParametersBase::SetAspectratio(AspectRatio);
      ComputeK_();
    }

    /** @brief Set type of distortion parameters
        @author grauel */
    inline void SetDistortionType(BIAS_ProjParaPersp_DISTORTION_TYPE distype)
    {
      distType_ = distype;
    }

    /** @brief Get type of distortion parameters
        @author grauel */
    inline BIAS_ProjParaPersp_DISTORTION_TYPE GetDistortionType() const
    {
      return(distType_);
    }


    inline void SetUndistortion(double kc1, double kc2){
      kc1_ = kc1;
      kc2_ = kc2;
      kc3_ = 0;
      kc4_ = 0;
      distType_ = DISTYPE_RADIAL;
    }

    /** @brief Set the lens undistortion parameters*/
    inline void SetUndistortion(double kc1, double kc2,
                                double kc3, double kc4)
    {
      kc1_ = kc1;
      kc2_ = kc2;
      kc3_ = kc3;
      kc4_ = kc4;
      distType_ = DISTYPE_DEF; //reset to default distortion
    }

    /** @brief Set the lens undistortion parameters including the root of
        the polynomial.
        @author grauel 9/2007 */
    inline void SetUndistortionBrown(double kc1, double kc2,
                                     double kc3, double kc4,
                                     double r0)
    {
      kc1_ = kc1;
      kc2_ = kc2;
      kc3_ = kc3;
      kc4_ = kc4;
      r0_  = r0;
      distType_ = DISTYPE_BROWN;
    }

    inline void SetUndistortionInverseRad(double kc1, double kc2,
                                          double kc3, double kc4){
    
      kc1_ = kc1;
      kc2_ = kc2;
      kc3_ = kc3;
      kc4_ = kc4;
      distType_ = DISTYPE_INVERSE_RAD;
    }
    
    inline void SetUndistortionRad3(double kc1, double kc2, double kc3){
      kc1_ = kc1;
      kc2_ = kc2;
      kc3_ = kc3;
      kc4_ = 0;
      distType_ = DISTYPE_RAD3;
    }
    
    inline void SetUndistortionInverseRad3(double kc1, double kc2, double kc3){
      kc1_ = kc1;
      kc2_ = kc2;
      kc3_ = kc3;
      kc4_ = 0;
      distType_ = DISTYPE_INVRAD3;
    }
    
    
    inline void GetUndistortionInverseRad(double& kc1, double& kc2,
                                          double& kc3, double& kc4) const {
      
      if(distType_ != DISTYPE_INVERSE_RAD){
        BIASWARN("Distortion type is not inverse, parameters might be invalid");
        BIASABORT;
      }
      kc1 = kc1_;
      kc2 = kc2_;
      kc3 = kc3_;
      kc4 = kc4_;
    }
    
    /** @brief Get the lens undistortion parameters including the parameter
        describing root of the polynomial
        @author grauel 9/2007 */
    inline void GetUndistortionBrown(double& kc1, double& kc2,
                                     double& kc3, double& kc4,
                                     double& r0) const
    {
      if(distType_ != DISTYPE_BROWN)
      {
        BIASWARN("Distortion type is not DISTYPE_BROWN. the parameters might not be  valid.")
      }
      kc1 = kc1_;
      kc2 = kc2_;
      kc3 = kc3_;
      kc4 = kc4_;
      r0 = r0_;

    }

    inline void GetUndistortionRad3(double& kc1, double& kc2, double& kc3){
      
      if(distType_ != DISTYPE_RAD3){
        BIASWARN("Distortion type is not DistypeRAD3, the parameters might be invalid." << distType_);       
      }
      
      kc1 = kc1_;
      kc2 = kc2_;
      kc3 = kc3_;
    }
    
    inline void GetUndistortionInvRad3(double& kc1, double& kc2, double& kc3){
      
      if(distType_ != DISTYPE_INVRAD3){
        BIASWARN("Distortion type is not DistypeINVRAD3, the parameters might be invalid.");       
      }
      
      kc1 = kc1_;
      kc2 = kc2_;
      kc3 = kc3_;
    }
    
    
    /* @brief get the PMatrix composed from the projection object
     */
    inline virtual BIAS::PMatrix GetP() const {
      return PMatrix(K_, GetR(), GetC());
    }

    /* @brief get cached KMatrix
     */
    inline virtual BIAS::KMatrix GetK() const {
      return K_;
    };

    /* @brief get cached inverted KMatrix
     */
    inline virtual BIAS::KMatrix GetKinv() const {
      return invK_;
    };

    /** @brief sets the internal parameters from a given KMatrix
        and updates the cached K and its inverse */
    virtual void SetK(const KMatrix& K) {
      BIASASSERT(K[0][0]!=0);
      focallength_  = K[0][0];
      skew_ = K[0][1];
      ProjectionParametersBase::SetPrincipal(K[0][2],K[1][2]);
      double aspectratio = K[1][1]/K[0][0];
      ProjectionParametersBase::SetAspectratio(aspectratio);
      K_ = K; invK_ = K.Invert();
    };

    /** @brief set from P  */
    virtual void SetP(const PMatrix& P) {
      PMatrix p(P); // de-const
      SetK(p.GetK());
      SetC(p.GetC());
      SetR(p.GetR());
      ComputeK_();
    };

    /** @brief Adapt internal parameters to resampled image.
        @param ratio 2.0 = downsample by 2, 0.5 = upsample by 2
        @param offset Offset for principal point (applied before down/upsampling!)
        \attention Delivers only correct results if ratio is chosen
        to rescale image size to integral image dimensions
        in both directions. Image size is rounded!
    */
    virtual void Rescale(double ratio, const double offset = 0.0) {
      ProjectionParametersBase::Rescale(ratio, offset);
      focallength_ /= ratio;
      skew_ /= ratio;
      ComputeK_();
      // dont have to update radial/tangential distortion coefficients
      // because we measure in normalize space
    }

    /** @brief Adapt internal parameters to resampled image. Allows to rescale to different aspect ratio.
        @param width new image width
        @param height new image height
    */
    virtual void Rescale(unsigned int width, unsigned int height){
      unsigned int oldwidth = width_;
      unsigned int oldheight = height_;
      ProjectionParametersBase::Rescale(width, height);
      double ratio = (double)oldwidth/(double)width_;
      focallength_ /= ratio;
      skew_ /= ratio;
      aspectratio_ /= ((double)oldheight/(double)height_) / ((double)oldwidth/(double)width_);
      ComputeK_();

    }

    /** @brief compute scalar dissimilarity between two cameras based upon
        rough approximation of field of view overlap for pure rotation and
        penalty term for translation: 0=identical */
    virtual double ViewDifference(const ProjectionParametersBase* pPPB) const;

    void GetIdealK(KMatrix& K) const;
    void SetIdealImageSize(unsigned int width, unsigned int height);
    void GetIdealImageSize(unsigned int& width, unsigned int& height) const;

    /** \brief transforms an image from polar coordinates to cartesian coordinates
     *  \author ischiller
     *  \date 06/2010  */
    int TransformPolarToCartesianCoordinates(const BIAS::Image<float>& polarDepth,
                                                                                         BIAS::Image<float>& cartesianDepth);
    /** \brief transforms a depth value from polar coodinates to cartesian coordinates
     *  \return the transformed depth in cartesian coordinates
     *  \author ischiller
     *  \date 06/2010  */
    float TransformPolarToCartesianCoordinates(const HomgPoint2D& pos, const float depthPolar);
    /** \brief transforms an image from cartesian coordinates to polar coordinates
    *   \author ischiller
    *   \date 06/2010  */
    int TransformCartesianToPolarCoordinates(const BIAS::Image<float>& cartesianDepth,
                                                                                         BIAS::Image<float>& polarDepth);
    /** \brief transforms a depth value from cartesian coordinates to polar coordinates
    *  \return the transformed depth in polar coordinates
    *  \author ischiller
    *  \date 06/2010  */
    float TransformCartesianToPolarCoordinates(const HomgPoint2D& pos, const float depthCartesian);


    /** @brief sets minimum z value of unit-ray in local CCS to be accepted
        as in front of camera (e.g. DoesPointProjectIntoImage) */
    void SetMinZLocal(const double& minz) {
      minZlocal_ = minz;
    }

    /** @get minimum z value of ray in image (if computed already) */
    double GetMinZLocal() const { return minZlocal_; }

    /** @brief run along image border and compute maximum field of view,
        save in minzlocal */
    void UpdateMinZLocal();

    /** Calculates an ModelViewProjection matrix useable in OpenGL.
        \param zNear defines the near z-clipping plane.
        \param zFar defines the far z-clipping plane.
        \param flip reading out the framebuffer into a bias image implicitly flips the image,
        when this can be remedied by (pre)flipping in the projection matrix - however when rendering
        onscreen, the visible image will be flipped then.
    **/
    Matrix4x4<double> GetGLModelviewProjectionMatrix(const double zNear,
                                                  const double zFar,
                                                  const bool flip,
                                                  const bool transpose=false);

    /** Calculates an projection matrix useable in OpenGL.
        \param zNear defines the near z-clipping plane.
        \param zFar defines the far z-clipping plane.
        \param flip reading out the framebuffer into a bias image implicitly flips the image,
        when this can be remedied by (pre)flipping in the projection matrix - however when rendering
        onscreen, the visible image will be flipped then.
    **/
    Matrix4x4<double> GetGLProjectionMatrix(const double zNear,
                                         const double zFar,
                                         const bool flip,
                                         const bool transpose=false);

    /** Calculates an projection matrix useable in OpenGL.
        The viewing frustum encloses the image all optical rays passing through the image region
        specified by the parameters upperLeft, width and height.
        \param zNear defines the near z-clipping plane.
        \param zFar defines the far z-clipping plane.
        \param upperLeft specifies the image positions of the upper left "visible" optical ray.
        \param width specifies the x-component of the image position of the lower right "visible" optical ray.
        \param height specifies the y-component of the image position of the lower right "visible" optical ray.
        \param flip reading out the framebuffer into a bias image implicitly flips the image,
        when this can be remedied by (pre)flipping in the projection matrix - however when rendering
        onscreen, the visible image will be flipped then.
    **/
    Matrix4x4<double> 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=false);

    static std::string GetRadialDistModelString(BIAS_ProjParaPersp_DISTORTION_TYPE model);

#ifdef BIAS_HAVE_XML2
    /** @brief specialization of XML block name function */
    virtual int XMLGetClassName(std::string& TopLevelTag,
                                double& Version) const;

    /** @brief specialization of XML write function */
    virtual int XMLOut(const xmlNodePtr Node, XMLIO& XMLObject) const;

    /** @brief specialization of XML read function */
    virtual int XMLIn(const xmlNodePtr Node, XMLIO& XMLObject);
#endif

    friend std::ostream&
      operator<<(std::ostream &os, const ProjectionParametersPerspective& p);

  protected:
    /** @brief called from constructors to set zeros to most values */
    void InitParams_() {
      K_.SetIdentity();
      invK_.SetIdentity();
      focallength_=1.0;
      skew_=0;
      kc1_=0;
      kc2_=0;
      kc3_=0;
      kc4_=0;
      r0_=0;
      distType_=DISTYPE_DEF;
      idealImageWidth_ = 512; //width_;
      idealImageHeight_ = 512; //height_;
      minZlocal_ = 0.1;
    }

    /** @brief computes the cached KMatrix and its inverse
        call after every parameter change */
    void ComputeK_();

    /// focal length, aspect ratio, principal point in K_, inverse in invK_
    KMatrix K_, invK_;

    /// focal length is stored in pixel for cellSizeX
    double focallength_;
    /// lens undistortion parameters after Bouquet
    /// also used as parameters for distortion after Brown(?)
    double kc1_,kc2_,kc3_,kc4_;

    // additional lens undistortion parameter after Brown(?)
    // describing the constant root of the polynomial
    double r0_;

    // distortion type parameter
    BIAS_ProjParaPersp_DISTORTION_TYPE distType_;

    /// skew calibration parameter
    double skew_;

    /// ideal image size
    unsigned int idealImageWidth_, idealImageHeight_;

    /// rigorously clip unit rays in CCS with z smaller than this values
    double minZlocal_;

    /** Helper function for minimal angle step calculation. **/
    int GetMinimalAngleInCorner_(const HomgPoint2D& corner, double& minAngle);


    inline void Distort_(const double x, const double y,
                         double& dist_x, double& dist_y) const;

    inline bool Undistort_(const double dist_x, const double dist_y,
                           double& x, double& y) const;
  };


  //////////// inline code //////////////////
  inline std::ostream& operator<<(std::ostream &os,
                                  const ProjectionParametersPerspective& p)
  {
    os << "ProjectionParametersPerspective:" << std::endl;
    os << "Pose " << p.GetPose() << std::endl;
    unsigned int cx, cy;
    p.GetImageSize(cx, cy);
    os<<"ImageSize: " << cx << " " << cy << std::endl;
    os<<"Skew: "<<p.skew_<<std::endl;
    os<<"K:"<<p.GetK()<<std::endl;
    std::string dist_model;
    bool brown = false;
    if(p.distType_ == DISTYPE_BROWN){
      dist_model = "Browns distortion model";
      brown = true;
    }
    if(p.distType_ == DISTYPE_DEF || p.distType_ == DISTYPE_RADIAL){
      dist_model = "Default distortion model";
    }
    if(p.distType_ == DISTYPE_INVERSE_RAD){
      dist_model = "Inverse rad distorion model";
    }
    if(p.distType_ == DISTYPE_NONE){
      dist_model = "no distortion model";
    }
    if(p.distType_ == DISTYPE_RAD3){
      dist_model = "radial distortion, 3 coeffs";
    }
    if(p.distType_ == DISTYPE_INVRAD3){
      dist_model = "inverse radial distortion, 3 coeffs";     
    }
    os<<dist_model<<": ";
    os<<  " kc1_: " <<p.kc1_<< " kc2_: "<<p.kc2_<<" kc3_: "
      <<p.kc3_<<" kc4_: "<<p.kc4_;
    if (brown)
      os <<" r0_: "<< p.r0_;
    os << "\nminZlocal: "<<p.minZlocal_<<std::endl;
    os<<std::endl;
    return os;
  }

  void ProjectionParametersPerspective::
  Distort_(const double x, const double y,
           double& dist_x, double& dist_y) const
  {
    double twoxy, xx, yy;
    double rc;
    double tmpx, tmpy;
    double r0squared;
    double rsquared;
    double raddist;
    xx = x*x;
    yy = y*y;
    rsquared = xx + yy;

    switch(distType_){
    case DISTYPE_NONE:
      raddist = 1.0;
      break;
    case DISTYPE_BROWN:
        //todo why is here no iteration of  UNDISTORT_ITERS ??? ischiller
        //todo and the tangential factors are missing too!!

      // r0 is the distance of polynomial root from the distortion center
      // since the x and y are normalized r0 must be normalized too.
      r0squared = (r0_)*(r0_);
      //"constant root of polynomial"
      rc = kc1_ * r0squared + kc2_* r0squared*r0squared;
      // radial distortion:
      raddist = 1 + kc1_*rsquared + kc2_*rsquared*rsquared - rc;
      break;
    case DISTYPE_DEF:
      // radial distortion:
      raddist = 1 + kc1_*rsquared + kc2_*rsquared*rsquared;
      if (kc3_!=0.0 || kc4_!=0.0){
        twoxy = 2.0*x*y;
        // apply radial and tangential distortion:
        dist_x = raddist * x + kc3_*twoxy + kc4_ * (rsquared + 2.0*xx);
        dist_y = raddist * y + kc4_*twoxy + kc3_ * (rsquared + 2.0*yy);
      } else {
        // apply only radial distortion:
        dist_x = raddist * x;
        dist_y = raddist * y;
      }
      break;
    case DISTYPE_RADIAL:
      // radial distortion:
      raddist = 1 + kc1_*rsquared + kc2_*rsquared*rsquared;
      dist_x = raddist * x;
      dist_y = raddist * y;
      break;
    case DISTYPE_INVERSE_RAD:
      BIASWARNONCE("distorting with dist inverse ");
      // inverse radial distortion - the iterative technique is applied here
      double tandistx, tandisty;
      dist_x = x;
      dist_y = y;
      for (unsigned int i = 0; i < UNDISTORT_ITERS; i++){
        twoxy = 2 * dist_x*dist_y;
        rsquared = dist_x*dist_x + dist_y*dist_y;

        // radial distortion:
        raddist = 1.0 + (kc1_ + kc2_*rsquared)*rsquared;
        // tangential distortion:
        tandistx = kc3_*twoxy + kc4_ * (rsquared + 2*dist_x*dist_x);
        tandisty = kc4_*twoxy + kc3_ * (rsquared + 2*dist_y*dist_y);

        // apply distortion:
        tmpx = raddist * dist_x + tandistx;
        tmpy = raddist * dist_y + tandisty;

        // subtract distortion from distorted coordinates
        dist_x = x - (tmpx-dist_x);
        dist_y = y - (tmpy-dist_y);

        // prevent NaN in next iteration step
        if (fabs(dist_x)>1e10 || fabs(dist_y)>1e10) {
          dist_x = -1;
          dist_y = -1;
          return;
        }
      }
      break;
    case DISTYPE_RAD3:
      // radial distortion with 3 coefficients:
      raddist = 1 + kc1_*rsquared + kc2_*rsquared*rsquared + kc3_*rsquared*rsquared*rsquared;
      dist_x = raddist * x;
      dist_y = raddist * y;
      break;
      
    case DISTYPE_INVRAD3:
      // radial distortion with 3 coefficients, but inversed, hence the iteration is applied here
      BIASWARNONCE("distorting with dist inverse ");
      dist_x = x;
      dist_y = y;
      for (unsigned int i = 0; i < UNDISTORT_ITERS; i++){
        twoxy = 2 * dist_x*dist_y;
        rsquared = dist_x*dist_x + dist_y*dist_y;

        // radial distortion:
        raddist = 1.0 + (kc1_ + kc2_*rsquared+kc3_*rsquared*rsquared)*rsquared;

        // apply distortion:
        tmpx = raddist * dist_x;
        tmpy = raddist * dist_y;

        // subtract distortion from distorted coordinates
        dist_x = x - (tmpx-dist_x);
        dist_y = y - (tmpy-dist_y);

        // prevent NaN in next iteration step
        if (fabs(dist_x)>1e10 || fabs(dist_y)>1e10) {
          dist_x = -1;
          dist_y = -1;
          return;
        }
      }
      break;
    default:
      BIASERR("unknown distortion type");
      BIASABORT
      break;
    }

  }

  bool ProjectionParametersPerspective::
  Undistort_(const double dist_x, const double dist_y,
             double& x, double& y) const
  {
    double twoxy;
    double tmpx, tmpy;
    double rsquared;
    double raddist;
    double tandistx, tandisty;
    double distx, disty;
    double rc, r0squared;

    x = dist_x;
    y = dist_y;
    switch (distType_){
    case DISTYPE_NONE:
      x = dist_x;
      y = dist_y;
      return true;
    case DISTYPE_BROWN:
      // r0 is the distance of polynomial root from the distortion center
      r0squared = (r0_)*(r0_);

      //"constant root of polynomial"
      rc = kc1_ * r0squared + kc2_* r0squared* r0squared;

      for (unsigned int i = 0; i < UNDISTORT_ITERS; i++){
        twoxy = 2 * x*y;
        rsquared = x*x + y*y;

        // radial distortion:
        raddist = 1.0 + (kc1_ + kc2_*rsquared)*rsquared - rc;
        // tangential distortion:
        tandistx = kc3_*twoxy + kc4_ * (rsquared + 2*x*x);
        tandisty = kc4_*twoxy + kc3_ * (rsquared + 2*y*y);

        // subtract distortion from distorted coordinates
        x = (dist_x - tandistx)/raddist;
        y = (dist_y - tandisty)/raddist;

        // prevent NaN in next iteration step
        if (fabs(x)>1e10 || fabs(y)>1e10) {
          return false;
        }
      }
      break;
    case DISTYPE_DEF:
      for (unsigned int i = 0; i < UNDISTORT_ITERS; i++){
        twoxy = 2 * x*y;
        rsquared = x*x + y*y;

        // radial distortion:
        raddist = 1.0 + (kc1_ + kc2_*rsquared)*rsquared;
        // tangential distortion:
        tandistx = kc3_*twoxy + kc4_ * (rsquared + 2*x*x);
        tandisty = kc4_*twoxy + kc3_ * (rsquared + 2*y*y);

        // apply distortion:
        distx = raddist * x + tandistx;
        disty = raddist * y + tandisty;

        // subtract distortion from distorted coordinates
        x = dist_x - (distx-x);
        y = dist_y - (disty-y);

        // prevent NaN in next iteration step
        if (fabs(x)>1e10 || fabs(y)>1e10) {
          return false;
        }
      }
      break;
    case DISTYPE_RADIAL:

      for (unsigned int i = 0; i < UNDISTORT_ITERS; i++){
        rsquared = x*x + y*y;

        // radial distortion:
        raddist = 1.0 + (kc1_ + kc2_*rsquared)*rsquared;

        // apply distortion:
        distx = raddist * x;
        disty = raddist * y;

        // subtract distortion from distorted coordinates
        x = dist_x - (distx-x);
        y = dist_y - (disty-y);

        // prevent NaN in next iteration step
        if (fabs(x)>1e10 || fabs(y)>1e10) {
          return false;
        }
      }
      break;
    case DISTYPE_INVERSE_RAD:
      BIASWARNONCE("undistorting with dist inverse ")
      // radial distortion:
      rsquared = dist_x*dist_x + dist_y*dist_y;
      raddist = 1 + kc1_*rsquared + kc2_*rsquared*rsquared;
      if (kc3_!=0.0 || kc4_!=0.0){
        twoxy = 2.0*dist_x*dist_y;
        // apply radial and tangential distortion:
        x = raddist * dist_x + kc3_*twoxy + kc4_ * (rsquared + 2.0*dist_x*dist_x);
        y = raddist * dist_y + kc4_*twoxy + kc3_ * (rsquared + 2.0*dist_y*dist_y);
      } else {
        // apply only radial distortion:
        x = raddist * dist_x;
        y = raddist * dist_y;
      }
      break;
    case DISTYPE_RAD3:
      // radial distortion with 3 coefficients, this is undistort, so iteration needs to be applied
      x = dist_x;
      y = dist_y;
      for (unsigned int i = 0; i < UNDISTORT_ITERS; i++){
        twoxy = 2 * x*y;
        rsquared = x*x + y*y;

        // radial distortion:
        raddist = 1.0 + (kc1_ + kc2_*rsquared+kc3_*rsquared*rsquared)*rsquared;

        // apply distortion:
        tmpx = raddist * x;
        tmpy = raddist * y;

        // subtract distortion from distorted coordinates
        x = dist_x - (tmpx-x);
        y = dist_y - (tmpy-y);

        // prevent NaN in next iteration step
        if (fabs(x)>1e10 || fabs(y)>1e10) {
          x = -1;
          y = -1;
          return false;
        }
      }
      break;
      
    case DISTYPE_INVRAD3:
      // radial distortion with 3 coefficients:
      rsquared = dist_x*dist_x + dist_y*dist_y;
      raddist = 1 + kc1_*rsquared + kc2_*rsquared*rsquared + kc3_*rsquared*rsquared*rsquared;
      x = raddist * dist_x;
      y = raddist * dist_y;
      
      break;
    default:
      BIASERR("unknown distortion type");
      BIASABORT
      break;
    }
    return true;
  }

} // end namespace


#include <Base/Common/BIASpragmaEnd.hh>

#endif