ProjectionParametersCylindric.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_ProjectionParametersCylindric_hh__
#define __BIAS_ProjectionParametersCylindric_hh__

#include <bias_config.h>

#include <math.h>

#include <Base/Common/BIASpragmaStart.hh>

#include <Geometry/ProjectionParametersBase.hh>


namespace BIAS {

  /** @class ProjectionParametersCylindric
   *  @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. This projection can be thought of as a projection onto an
   image located at the hull of a virtual cylinder around the camera
   center. The following image shows the optical axis (z,blue) of the
   camera being orthogonal to the cylinder axis.
   \image html "cylinder.png"
   The axis of the virtual cylinder can be set parallel or
   orthogonal to the camera viewing direction modeling different
   types of cylinder cameras.
   Optionally, the virtual cylinder axis can be rotated arbitrarily,
   and an offset of the cylinder origin with respect to the local
   camera coordinate system can be given. Projection becomes computationally
   expensive in this case since the intersection of viewing
   rays with the cylinder hull must be computed explicitly here.
   Note that the radius of the virtual cylinder is fixed as 1.
   @note  Here, x/phi are the horizontal/vertical coordinates in the cylinder
   hull image. This x is NOT directly related to the x-axis of the
   camera coordinate system!

   @author bartczak/esquivel
  */
  class BIASGeometry_EXPORT ProjectionParametersCylindric:

    public  ProjectionParametersBase {

  public:

    /** @brief Enumeration of typical directions of the cylinder axis, i.e.
        orthogonal (standard) or parallel to camera's viewing direction. */
    enum ProjectionCylinderAxisEnum { AXIS_ORTHOGONAL = 0, AXIS_PARALLEL };

    /**
     * Note: Angles are given in radians
     */
    ProjectionParametersCylindric(const double xMin, const double xMax,
                                  const double phiMin, const double phiMax,
                                  const unsigned int width,
                                  const unsigned int height)
      : ProjectionParametersBase(width, height)
      {
        phiMin_ = phiMin;
        phiMax_ = phiMax;
        xMin_ = xMin;
        xMax_ = xMax;
        //SetQ(Quaternion<double>(0, 0, 0, 1));
        //SetC(Vector3<double>(0.0, 0.0, 0.0));
        SetCylinderType(AXIS_ORTHOGONAL);
        UseAxisOffset_ = false;
        SetCylinderOffset(Vector3<double>(0.0));
        UpdateQuasiK_();
      };

   

    ProjectionParametersCylindric(const unsigned int width = 0,
                                  const unsigned int height = 0)
      : ProjectionParametersBase(width, height)
      {
        phiMin_ = 0.0;
        phiMax_ = 2.0*M_PI;
        xMin_ = 0.0;
        xMax_ = 1.0;
        focallengthPhi_ = 1.0;
        focallengthX_ = 1.0;
        //SetQ(Quaternion<double>(0, 0, 0, 1));
        //SetC(Vector3<double>(0.0, 0.0, 0.0));
        SetCylinderType(AXIS_ORTHOGONAL);
        UseAxisOffset_ = false;
        SetCylinderOffset(Vector3<double>(0.0));
      };

    void SetParameters(const double xMin, const double xMax,
                       const double phiMin, const double phiMax,
                       const unsigned int width,
                       const unsigned int height){
      phiMin_ = phiMin;
      phiMax_ = phiMax;
      xMin_ = xMin;
      xMax_ = xMax;
      width_ = width;
      height_ = height;    
      UpdateQuasiK_();    
    }

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

    ProjectionParametersCylindric& operator=(const ProjectionParametersCylindric &P) {
      ProjectionParametersBase::operator=(P);
      phiMin_ = P.phiMin_;
      phiMax_ = P.phiMax_;
      xMin_   = P.xMin_;
      xMax_   = P.xMax_;
      focallengthPhi_ = P.focallengthPhi_;
      focallengthX_   = P.focallengthX_;
      AxisRotation_   = P.AxisRotation_;
      AxisOffset_     = P.AxisOffset_;
      AxisRotationInv_= P.AxisRotationInv_;
      AxisOffsetInv_  = P.AxisOffsetInv_;
      UseAxisOffset_  = P.UseAxisOffset_;
      return *this;
    };

    virtual ~ProjectionParametersCylindric() {};

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

    bool DoesPointProjectIntoImageLocal(const BIAS::Vector3<double> &localP,
                                        HomgPoint2D &p,
                                        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

        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 cylindric image plane with radius of one from the
     * image center.
     */
    HomgPoint3D UnProjectToImagePlane(const HomgPoint2D& pos,
                                      const double& depth = 1.0,
                                      bool IgnoreDistortion = false) const;


    virtual void UnProjectLocal(const HomgPoint2D &pos, Vector3<double>& pointOnRay,
                                Vector3<double>& direction,
                                bool IgnoreDistortion = false) const;

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

    //virtual double ViewDifference(const ProjectionParametersBase* pPPB) 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 have
        have 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 Calculates the 3D point on the cylinder hull local to the camera which
        belongs to the image position pos. We assume that the image points
        describe the surface of a cylinder stretched around the camera center.
        @author esquivel  */
    virtual Vector3<double> UnProjectToCylinderPoint(const HomgPoint2D &pos,
                                                     bool IgnoreDistortion = false) const;

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

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


    /** @brief Calculates difference of cylinder centers weighted with
        orientation angle up to now.
        @author esquivel */
    virtual double ViewDifference(const ProjectionParametersBase* pPPB) 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

    inline double GetFocallengthX() { return focallengthX_; };

    inline double GetFocallengthPhi() { return focallengthPhi_; };

    void SetCylinderRotation(const Vector3<double> &rotAxis, double rotAngle,
                             const ProjectionCylinderAxisEnum axisType = AXIS_ORTHOGONAL);

    void SetCylinderRotation(const Quaternion<double> &rotation,
                             const ProjectionCylinderAxisEnum axisType = AXIS_ORTHOGONAL);

    void SetCylinderType(const ProjectionCylinderAxisEnum axisType);

    void SetCylinderOffset(const Vector3<double> &offset);

    virtual void Rescale(double ratio, const double offset = 0.0){
      // make sure *this is consistent
      BIASASSERT(Equal(focallengthX_, double(width_)/(xMax_-xMin_)));
      BIASASSERT(Equal(principalX_, (-1.0)*xMin_*focallengthX_));
      BIASASSERT(Equal(focallengthPhi_, double(height_)/(phiMax_-phiMin_)));
      BIASASSERT(Equal(principalY_, (-1.0)*phiMin_*focallengthPhi_));
      ProjectionParametersBase::Rescale(ratio, offset);
      UpdateQuasiK_();
    }

    virtual void Rescale(unsigned int width, unsigned int height){
       // make sure *this is consistent
       BIASASSERT(Equal(focallengthX_, double(width_)/(xMax_-xMin_)));
       BIASASSERT(Equal(principalX_, (-1.0)*xMin_*focallengthX_));
       BIASASSERT(Equal(focallengthPhi_, double(height_)/(phiMax_-phiMin_)));
       BIASASSERT(Equal(principalY_, (-1.0)*phiMin_*focallengthPhi_));
       ProjectionParametersBase::Rescale(width, height);
       UpdateQuasiK_();
     }

   


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

  protected:

    inline void ProjectToNormalizedCoords_(const Vector3<double> &point,
                                           Vector2<double> &xPhi) const {
      xPhi[0] = point[0];
      xPhi[1] = atan2(point[1], point[2]);
    };

    /**
     * Description of coordinate range in normalized frame.
     *
     *@{
     */
    double phiMin_, phiMax_;
    double xMin_, xMax_;

    /** @brief uses internal params to calculate normalized coordinate ranges.
     */
    void UpdateFromQuasiK_();

    /** @brief Uses normalized coordinate ranges and image dimensions to
        calculate internals.
    */
    void UpdateQuasiK_();   

    /** @brief quasi K (principle point already contained in base class)
     */
    /* ratio between zylinder angle range and samples within this range:
       focallengthPhi_ = samples/angle range;
    */
    double focallengthPhi_;

    /* ratio between zylinder axis range and samples within this range:
       focallengthPhi_ = samples/angle range;
    */
    double focallengthX_;

    /** @brief Rotation of cylinder axis */
    Quaternion<double> AxisRotation_, AxisRotationInv_;

    /** @brief Offset for cylinder axis origin (optional) */
    Vector3<double> AxisOffset_, AxisOffsetInv_;
    bool UseAxisOffset_;

  };


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

  inline std::ostream& operator<<(std::ostream &os,
                                  const ProjectionParametersCylindric& p)
  {
    os << "ProjectionParametersCylindric:" << std::endl;
    os << "- focallenghtX_ = " << p.focallengthX_ << std::endl;
    os << "- focallenghtPhi_ = " << p.focallengthPhi_ << std::endl;
    os << "- principalX_ = " << p.principalX_ << std::endl;
    os << "- principalY_ = " << p.principalY_ << std::endl;
    os << "- xMin_ = " << p.xMin_ << std::endl;
    os << "- xMax_ = " << p.xMax_ << std::endl;
    os << "- phiMin_ = " << p.phiMin_ << std::endl;
    os << "- phiMax_ = " << p.phiMax_ << std::endl;
    os << "- width_ = " << p.width_ << std::endl;
    os << "- height_ = " << p.height_ << std::endl << std::endl;
    double angle;
    Vector3<double> axis;
    p.AxisRotation_.GetAxisAngle(axis, angle);
    os << "- cylinder axis adaption rotation axis is "
       << axis[0] << "," << axis[1] << "," << axis[2]
       << ", rotation angle is " << 180.0*angle/M_PI << ", offset is "
       << p.AxisOffset_[0] << "," << p.AxisOffset_[1] << "," << p.AxisOffset_[2]
       << "." << std::endl;
    return os;
  }


} // end namespace

#include <Base/Common/BIASpragmaEnd.hh>

#endif