ProjectionParametersSphericalSimple.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_ProjectionParametersSphericalSimple_hh__
#define __BIAS_ProjectionParametersSphericalSimple_hh__

#include <bias_config.h>

//including BIASpragmaEnd at end of file implies including BIASpragmaStart at first
#include <Base/Common/BIASpragmaStart.hh>

#include <Geometry/ProjectionParametersBase.hh>


namespace BIAS {

  /** @class ProjectionParametersSphericalSimple
   *  @test tested with TestProjectionParametersSphericalSimple.cpp
      @brief projection parameters
      camera parameters which define the mapping between rays in the
      camera coordinate system and pixels in the image as well as external pose.
      the parameters phiOffset and thetaOffset are considered to be intrinsic
      parameters describing the rotation of the principal point
      the parameters dPhi and dTheta describe the resolution of a pixel in
      radian per pixel.
      Distortion is ignored.
      This class is intended to describe spherical rectification results. */
  class BIASGeometry_EXPORT ProjectionParametersSphericalSimple
    : public ProjectionParametersBase
  {
  public:

    ProjectionParametersSphericalSimple();

    ProjectionParametersSphericalSimple(const double phiOffs,
                                        const double thetaOffs,
                                        const double dPhi,
                                        const double dTheta,
                                        const unsigned int width,
                                        const unsigned int height);

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

    virtual ~ProjectionParametersSphericalSimple() {};

    /** @brief position of image on sphere in phi direction
        can also considered to be rotation of the principal point. */
    virtual double GetPhiOffset() const { return phiOffset_;};

    /** @brief position of image on sphere in theta direction  */
    virtual double GetThetaOffset() const { return thetaOffset_;};

    /** @brief set resolution in radian/pixel for phi direction*/
    virtual double GetDPhi() const { return dPhi_;};

    /** @brief set resolution in radian/pixel for theta direction*/
    virtual double GetDTheta() const { return dTheta_;};

    /** @brief calculates the projection of a point
        in the local camera coordinate system
        to a pixel in the image plane of the camera

        In the simplest case perspective pinhole projection x = K * point
        where point is transformed of X using  point = (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 calculates the unit size viewing ray from the camera center
        (in the camera coordinate system) which belongs to the image position
        pos e.g. pos=principal point results in (0,0,1)=optical axis */
    virtual void UnProjectLocal(const HomgPoint2D& pos, Vector3<double>& pointOnRay,
                                Vector3<double>& direction,
                                bool IgnoreDistortion = false) const;

    /** @brief map points from image onto unit diameter image plane in 3D.*/
    virtual HomgPoint3D UnProjectToImagePlane(const HomgPoint2D& pos,
                                              const double& depth = 1.0,
                                              bool IgnoreDistortion = false)
      const;

    virtual bool Distort(BIAS::HomgPoint2D& point2d) const { return false; };

    virtual bool Undistort(BIAS::HomgPoint2D& point2d) const { return false; };

    virtual ProjectionParametersBase* Clone() const
    { return new ProjectionParametersSphericalSimple(*this); };

    void Rescale(double ratio, const double offset);

    void Rescale(unsigned int width, unsigned int height);

    /** Checks if 3D point projects into specified image and returns
     *  belonging 2D image point.
     *  @param localX assumes homogenized point! */
    virtual bool
      DoesPointProjectIntoImageLocal(const Vector3<double>& localX,
                                     HomgPoint2D& x,bool IgnoreDistort) const;

    ////////////////////////////////////////////////////////////////////
    // convenience functions

    /** Transform from a semilocal reference frame to pixel coo.
        The semilocal reference  frame is defined to have its origin at
        the camera center and to have the same orientation as the full
        global reference frame */
    inline void GlobalOrientation2Pixel(const Vector3<double>& p2d,
                                        Vector2<double>& pixel) const;

    /** Transform from pixel coo to semilocal reference frame.
        The semilocal reference  frame is defined to have its origin at
        the camera center and to have the same orientation as the full
        global reference frame */
    inline void Pixel2GlobalOrientation(const Vector2<double>& pixel,
                                        Vector3<double>& p2d) const;


    inline void GlobalOrientation2Angles(const Vector3<double>& p2d,
                                        double &phi, double &theta) const;


    inline void Angles2GlobalOrientation(const double &phi,
                                         const double &theta,
                                         Vector3<double>& p2d) const;

    /** Transform from local camera centric reference system to pixel coo */
    inline void Local2Pixel(const Vector3<double>& p2d,
                            Vector2<double>& pixel) const;

    /** Transform from pixel coo to local reference frame */
    inline void Pixel2Local(const Vector2<double>& pixel,
                            Vector3<double>& p2d) const;

    /** Transform sphere angels to local reference frame */
    inline void Local2Angles(const Vector3<double>& local,
                             double &phi, double &theta) const;

    /** Transform local camera centric coos to sphere angles */
    inline void Angles2Local(const double &phi, const double &theta,
                             Vector3<double>& local) const;

    /** compute the angles belonging to pixel coos */
    inline void Pixel2Angles(const Vector2<double>& pixel, double &phi,
                             double &theta) const;

    /** compute the angles belonging to pixel coos */
    inline void Angles2Pixel(const double &phi, const double &theta,
                             Vector2<double>& pixel) const;


#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

    ///////////////////
    // ensure that R_ stays consistent with Pose_
    /////////////////

    virtual void SetPoseParametrization(const PoseParametrization& pp);

    virtual void SetQ(const BIAS::Quaternion<double>& Q);

    virtual void SetQC(const BIAS::Quaternion<double>& Q,
                       const BIAS::Vector3<double>& C);

    virtual void SetPose(const BIAS::Pose pose);

    virtual void SetR(const BIAS::RMatrix& R);

    virtual void ValidatePose();

    RMatrixBase GetRCache() const
    { return R_; }

    /// friend functions
    friend std::ostream& operator<<(std::ostream &os,
                                    const ProjectionParametersSphericalSimple& p);
  protected:
    double phiOffset_;
    double thetaOffset_;
    double dPhi_;
    double dTheta_;
    /// a cache for the orientation of the camera
    BIAS::RMatrixBase R_;
  };

  /////////////////////////////////////////
  // inline code
  /////////////////////////////////////////


  inline std::ostream& operator<<(std::ostream &os,
                                  const ProjectionParametersSphericalSimple& p)
  {
    os << "ProjectionParametersSphericalSimple:" << std::endl;    
    os << "- phiOffset = " << p.phiOffset_ << std::endl;
    os << "- thetaOffset = " << p.thetaOffset_ << std::endl;
    os << "- dPhi = " << p.dPhi_ << std::endl;
    os << "- dTheta = " << p.dTheta_ << std::endl;
    os << "- R = " << p.R_ << std::endl;
    return os;
  }

  void ProjectionParametersSphericalSimple::
  GlobalOrientation2Pixel(const Vector3<double>& p2d,
                          Vector2<double>& pixel) const
  {
    BIASASSERT(QValid_);
    BIAS::Vector3<double> local;
    R_.TransposedMult(p2d, local);
    Local2Pixel(local, pixel);
  }


  void ProjectionParametersSphericalSimple::
  Pixel2GlobalOrientation(const Vector2<double>& pixel,
                          Vector3<double>& p2d) const
  {
    BIASASSERT(QValid_);
    Vector3<double> local;
    Pixel2Local(pixel, local);
    R_.Mult(local, p2d);
  }

  void ProjectionParametersSphericalSimple::
  GlobalOrientation2Angles(const Vector3<double>& p2d,
                           double &phi, double &theta) const
  {
    BIASASSERT(QValid_);
    BIAS::Vector3<double> local;
    R_.TransposedMult(p2d, local);
    Local2Angles(local, phi, theta);
  }


  void ProjectionParametersSphericalSimple::
  Angles2GlobalOrientation(const double &phi, const double &theta,
                           Vector3<double>& p2d) const
  {
    BIASASSERT(QValid_);
    BIAS::Vector3<double> local;
    Angles2Local(phi, theta, local);
    R_.Mult(local, p2d);
  }


  void ProjectionParametersSphericalSimple::
  Local2Pixel(const Vector3<double>& p2d, Vector2<double>& pixel) const
  {
    double phi, theta;
    Local2Angles(p2d, phi, theta);
    Angles2Pixel(phi, theta, pixel);
  }


  void ProjectionParametersSphericalSimple::
  Pixel2Local(const Vector2<double>& pixel, Vector3<double>& p2d) const
  {
    double phi, theta;
    Pixel2Angles(pixel, phi, theta);
    Angles2Local(phi, theta, p2d);
  }


  void ProjectionParametersSphericalSimple::
  Local2Angles(const Vector3<double>& local, double &phi, double &theta) const
  {
    double r = local.NormL2();
    if (Equal(r, 0.0)) { BEXCEPTION("cannot transform vector of zero length"); }
    double x = local[0] / r;
    double y = local[1] / r;
    double z = local[2] / r;
    theta = acos(x);
    double sin_theta = sin(theta);
    if (Equal(sin_theta, 0.)){
      phi = 0.;
    } else {
      phi = atan2(y/sin_theta, z/sin_theta);
    }
  }


  void ProjectionParametersSphericalSimple::
  Angles2Local(const double &phi, const double &theta,
               Vector3<double>& local) const
  {
    double x = cos(theta);
    double y = sin(phi)*sin(theta);
    double z = cos(phi)*sin(theta);
    local.Set(x, y, z);
  }


  void ProjectionParametersSphericalSimple::
  Pixel2Angles(const Vector2<double>& pixel, double &phi, double &theta) const
  {
    theta = pixel[0] * dTheta_ + thetaOffset_;
    phi =   pixel[1] * dPhi_   + phiOffset_;
  }


  void ProjectionParametersSphericalSimple::
  Angles2Pixel(const double &phi, const double &theta,
               Vector2<double>& pixel) const
  {
    // deal with periodicy of phi and theta
    // phi must be in range between -pi and +pi
    // theta must be in range [0, pi]
    const double pi = M_PI;
    const double two_pi = 2. * M_PI;
    double mphi(phi), mtheta(theta);
    while (mphi<-pi) { mphi += two_pi; }
    while (mphi>pi) { mphi -= two_pi; }
    while (mtheta<0.) { mtheta += two_pi; }
    while (mtheta>two_pi) { mtheta -= two_pi; }
    if (mtheta>pi){
      BEXCEPTION("invalid theta: "<<mtheta<<"\n must be in [0, PI]");
    }
    pixel[0] = (mtheta - thetaOffset_) / dTheta_;
    pixel[1] = (mphi - phiOffset_) / dPhi_;

  }
} // end namespace


#include <Base/Common/BIASpragmaEnd.hh>

#endif