ProjectionParametersSphericalFast.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_ProjectionParametersSphericalFast_hh__
#define __BIAS_ProjectionParametersSphericalFast_hh__
#include "bias_config.h"

#include <Base/Common/BIASpragmaStart.hh>
#include <Geometry/ProjectionParametersBase.hh>
#include <Geometry/ProjectionParametersPerspective.hh>
#include <MathAlgo/Interpolator.hh>
#include <Base/ImageUtils/BresenhamCircle.hh>
#include <MathAlgo/PolynomialSolve.hh>

namespace BIAS {

 /** @class ProjectionParametersSphericalFast
  *  @ingroup g_geometry
     @brief spherical camera that uses polynomial only and should therefore be faster
            than other spherical camera
     @author sedlazeck
 */
  class BIASGeometry_EXPORT ProjectionParametersSphericalFast:
    public  ProjectionParametersBase {
  public:

    ProjectionParametersSphericalFast(const unsigned int width = 0,
                                      const unsigned int height = 0)
      : ProjectionParametersBase(width, height) {
      radius_=0;
      maxCamAngle_=0;
      // init polynomials as identity function
      coeffsDist_.resize(5, 0.0);
      coeffsDist_[0] = 1.0;
      coeffsUndist_.resize(5, 0.0);
      coeffsUndist_[0] = 1.0;
    }

    
    ProjectionParametersSphericalFast(const double radius,
                                      const double maxAngle,
                                      const unsigned int width,
                                      const unsigned int height)
      : ProjectionParametersBase(width, height) {
      radius_=radius;
      maxCamAngle_=maxAngle;
      // init polynomials as identity function
      coeffsDist_.resize(5, 0.0);
      coeffsDist_[0] = 1.0;
      coeffsUndist_.resize(5, 0.0);
      coeffsUndist_[0] = 1.0;
    }

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

    ProjectionParametersSphericalFast&
    operator=(const ProjectionParametersSphericalFast & P) {
      ProjectionParametersBase::operator=(P);
      radius_ = P.radius_;
      maxCamAngle_ = P.maxCamAngle_;
      coeffsDist_ = P.coeffsDist_;
      coeffsUndist_ = P.coeffsUndist_;
      return *this;
    }

    virtual ~ProjectionParametersSphericalFast(){}

    /** @brief call this to start a run at the outer boundary of an image.
     *
     *  You get the first boundary position in it, e.g. (0,0) for a perspective
     *  image. Feed this into GetNextBorderPixel to get the next position.
     *  Special implmentation here, because we need to run around the FOV
     *  circle. NOT YET IMPLEMENTED, PLEASE DO !
     */
    virtual void GetFirstBorderPixel(PixelIterator& it);

    /** @brief call this iteratively to run at the outer boundary of an image.
     *
     *  All returned coordinates must have valid local rays in the camera
     *  coordinate system (e.g. in fisheye cams, we run at the fov circle) and
     *  must be in the image. Two subsequent coordinates must not be more
     *  distant than 1 pixel. NOT YET IMPLEMENTED, PLEASE DO !
     *  @param it must be initialized by GetFirstBorderPixel
     *  @return false if the returned position closes the loop around the image
     */
    virtual bool GetNextBorderPixel(PixelIterator& it);


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

    virtual bool Distort(BIAS::HomgPoint2D& point2d) const;

    virtual bool Undistort(BIAS::HomgPoint2D& point2d) const;



    bool DoesPointProjectIntoImageLocal(const BIAS::Vector3<double>& localX,
                                        HomgPoint2D& x,
                                        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 is indicated
        by a return value of (0, 0, 0). 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.*/
    virtual HomgPoint2D ProjectLocal(const Vector3<double>& point,
                                     bool IgnoreDistortion = false) const {
      HomgPoint2D retVal_hom2D;
      ProjectionParametersSphericalFast::ProjectLocal(point, retVal_hom2D, IgnoreDistortion);
      return retVal_hom2D;
    }

    /** @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 is indicated
        by a return value of zero. 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 spherical image plane with radius 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
        e.g. (0,0,1) means optical axis
    */
    virtual void
    UnProjectLocal(const HomgPoint2D& pos, Vector3<double>& origin,
              Vector3<double>& direction, bool IgnoreDistortion = false) const;

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

    /** @brief Return radius of spherical image in pixels. */
    inline double GetRadius() const {
      return radius_;
    }

    /** @brief Set radius of spherical image in pixels. */
    inline void SetRadius(const double r) {
      radius_ = r;
    }

    /** @brief Get maximal camera angle (in rad), i.e. azimuth angle of rays
               corresponding to the outer rim of the image. */
    inline double GetMaxCamAngle() const {
      return maxCamAngle_;
    }

    /** @brief Set maximal camera angle (in rad), i.e. azimuth angle of rays
               corresponding to the outer rim of the image.
        @param[in] maxCamAngle New maximal camera angle to set (in rad)
        @attention If undistortion polynomial is set, it will be recalculated
                   with default sampling. Use RecalculateUndistortion instead
                   if you want to reset undistortion and max. camera angle
                   by yourself.
    */
    void SetMaxCamAngle(const double maxCamAngle);

    /** @brief Recalculate undistortion polynomial for new maximal camera angle.
        @param[in] maxCamAngle New maximal camera angle to set (in rad)
        @attention If new max. camera angle is larger than former, the
                   undistortion polynomial is extrapolated linearly beyond the
                   former range which may result in strange behaviour!
    */
    void RecalculateUndistortion(const double maxCamAngle);

    /** @brief Set undistortion polynomial from corresponding undistorted and
               distorted azimuth angles (in rad) and image radius (in pixels).
        @param[in] undistAngles Vector of undistorted azimuth angles (in rad)
        @param[in] distAngles   Vector of corresponding distorted angles
        @param[in] radius       Radius of spherical image to set (in pixels)
    */
    int SetUndistortion(const std::vector<double>& undistAngles,
                        const std::vector<double>& distAngles,
                        const double radius);


    /** @brief Set undistortion polynomial from from polynomial coefficients
               of undistortion (mapping radius to z coordinate) or distortion
               (mapping theta to radius) function.
        @param[in] maxCamAngle Maximal azimuth angle corresponding to the
                               outer rim of the spherical image
        @param[in] coefficients Vector of polynomial coefficients to
                                estimate the undistortion polynomial from
        @param[in] undistCoeffs Specifies if the given polynomial coefficients
                                 describe the undistortion, or distortion.
    */
    int InitPolyCoeffs(const double maxCamAngle,
                       const std::vector<double>& coefficients,
                       const bool undistCoeffs = true);
    
    /** @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);
      radius_ /= ratio;
    }

    virtual void Rescale(unsigned int width, unsigned int height){
      unsigned int oldwidth = width_;
      ProjectionParametersBase::Rescale(width, height);
      radius_ /= ((double)oldwidth/(double)width_);
    }

    virtual double ViewDifference(const ProjectionParametersBase* pPPB) const;

    /** @brief Returns a fake KMatrix for the fisheye camera.

        Uses MaxAngle_ to limit maxangle, then calls PPB::GetFakeK.
        The FoV of the KMatrix is specified by maxangle (PI*80/180 for 160deg)
        or the max fisheye angle, if this is <160deg.
        @param resolution The resolution parameter influences the
        Resolution of the perspective image that is represented by the camera.
        0 - The perspective image has the same size as the fisheye image
        1 - The perspective image has the same resolution as the fisheye
        image in the center (highest resolution)
        2 - The perspective image has the same resolution as the fisheye
        image near the border (lowest resolution)
        @param imgsize returns the image size
        @param maxangle maximum theta in rad, e.g. (PI*80/180 for 160deg fov)
        @author streckel 05/2006
    */
    virtual BIAS::KMatrix GetFakeKMatrix(double& imgsize, int resolution=0,
                                         const double& maxangle = 1.4) const;

    /** @brief stupid reimplementation since this class does not inherit
        function from base class (gcc) */
    virtual BIAS::KMatrix GetFakeKMatrix(int resolution=0,
                                         const double& maxangle = 1.4) const {
      return ProjectionParametersBase::GetFakeKMatrix(resolution, maxangle);
    }

    /** Calculates perspective camera with specified viewing angle and
     * optical axis aligned with the optical ray corresponding to viewCenter
     * in spherical camera.
     * @param viewCenter pixel position in spherical image that is the
     * center of the perspective cutout.
     * @param perspHalfViewingAngle half field of view of the cutout in Degree
     * @pp returning cutout parameters
     * \returns false if perspective cut out could not be calculated.
     * @author streckel 08/2006
     */
    bool GetPerspectiveCutOutParameters(const BIAS::HomgPoint2D& viewCenter,
                                        const double perspHalfViewingAngleDEG,
                                        ProjectionParametersPerspective& pp) const;

    /** @brief calculates a 3D point in
        a local camera coordinate system specified by camSystem, which
        belongs to the image position pos in cam with distance depth to the
        camera center cam. Overload to habe return value (0,0,0)
        for undefined or out of image radius pixels.
        @author streckel  */
    virtual Vector3<double> UnProjectToPointLocal(const HomgPoint2D& pos,
                                                  const double& depth,
                                                  bool IgnoreDistortion = false) const;


    /** @brief Estimates undistortion polynomial mapping z coordinates to
               radius according to Scaramuzza's Matlab toolbox.

        Scaramuzza's undistortion polynomial is a function z(radius) which
        typically has a local extremum at radius = 0 and therefore can not
        have a linear monomial.
        @author koeser 04/2007
     */
    int EstimateUndistortionPolynomial(std::vector<double>& coefficients,
                                       const unsigned int degree = 5);

    /** @brief Estimates distortion polynomial mapping angle theta to radius
               according to Scaramuzza's Matlab toolbox.

        Scaramuzza's distortion polynomial is a function radius(theta) which
        is usually close to linear for fisheye cameras and SHOULD therefore
        have a linear monomial.
        @author koeser 04/2007
    */
    int EstimateDistortionPolynomial(std::vector<double>& coefficients,
                                     const unsigned int degree = 5);

    /** @brief Returns polynomial coefficients of angle correction polynomial
               (aka undistortion polynomial) mapping distorted to undistorted
               azimuth angles.
        @param[out] coeffs Returns coefficients of undistortion polynomial
    */
    inline void GetPolyCoeffs(std::vector<double> &coeffsdist,
                              std::vector<double> &coeffsundist) const {
      coeffsdist = coeffsDist_;
      coeffsundist = coeffsUndist_;
    }

    void TestPolynomialInversion();
  
    
#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 ProjectionParametersSphericalFast& p);

    inline double EvaluatePolynomial(const double x, 
                                     const std::vector<double>& coeff) const {

      
      register int n = (int)coeff.size() - 1;
      
    //  std::cout << "evalPoly degree " << n << std::endl;
      
      double result = 0.0;
      while (n>0){
        result = (result + coeff[n])*x*x;
        n--;
      }
      result = (result + coeff[0])*x;
      
      return result; 
    }
    
    int FitPolynomial(std::vector<double>& coefficients,
                      std::vector<double>& a,
                      std::vector<double>& b, 
                      unsigned int degree);

    // long names to avoid global symbol clashes
    std::vector<double> myvaluesPolynomial;
    std::vector<double> mylocationsPolynomial;
   
  protected:

    /** @brief Transforms a coordinate pair of image coordinates x, y in the
        uncalibrated spherical image, to a calibrated pair of polar coordinates
        phi, theta.
        @author Birger Streckel */
    void TransfCoordImage2Sphere_ (const HomgPoint2D& Source,
                                 double &theta, double &phi) const;

  

    /** @brief Transforms a calibrated pair of polar coordinates phi, theta
        to a coordinate pair of image coordinates x, y in the uncalibrated
        spherical image.
        @author Birger Streckel */
    void TransfCoordSphere2Image_ (double theta, double phi,
                                 HomgPoint2D &Source) const;

   

    /** @brief theta and phi are periodic in 2*M_PI, if they are out of a
        suitable range, try to find best equivalent angle by adding 2*l*M_PI
        @return 0 on success, <0 indicates that at least one angle is invalid
        @author koeser 10/2004 */
    void EnforceNormalRange_(double& theta, double& phi) const;



    
    
    
    // image radius if image is spherical
    double radius_;

    // Maximum image angle of the fisheye camera = half the fov in rad
    double maxCamAngle_;
    
    // these are the polynom coefficients
    std::vector<double>  coeffsDist_;
    std::vector<double> coeffsUndist_;

    
    
    //bresenham-circle for running allong fov area
    BresenhamCircle fovCircle_;
    
    //projects pixels outside the image onto the image border
    void ProjectOutsidePositions_(int& posX, int& posY);
    

  };

  /////////////// inline code ///////////////////////
  inline std::ostream& operator<<(std::ostream &os,
                                  const ProjectionParametersSphericalFast& p)
  {
    os<<"ProjectionParametersSphericalFast:"<<std::endl;
    os<<"Principal: "<<p.principalX_<< " " <<p.principalY_<<std::endl;
    os<<"Radius: "<<p.radius_<<std::endl;
    os<<"MaxCamAngle: "<<p.maxCamAngle_<<std::endl;
    os<<"Coefficients:"<<std::endl;
    for (unsigned int i=0; i<p.coeffsDist_.size(); i++) {
      os<< "distortion coeff " << i << " " << p.coeffsDist_[i] <<std::endl;
    }
    for (unsigned int i=0; i<p.coeffsUndist_.size(); i++) {
      os<< "undistortion coeff " << i << " " << p.coeffsUndist_[i] <<std::endl;
    }
    
    return os;
  }

} // end namespace


#include <Base/Common/BIASpragmaEnd.hh>

#endif