ProjectionParametersZoom.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_ProjectionParametersZoom_hh__
#define __BIAS_ProjectionParametersZoom_hh__
#include "bias_config.h"

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

namespace BIAS {

#ifndef __BIAS_CPDiscreteParam__
#define __BIAS_CPDiscreteParam__

  // distortion parameters at different zoom levels
  struct BIASImage_EXPORT CPDiscreteParam{
    double zoom;
    double focallength;
    // radial/tangential distortion parameters.
    // 4th order model of brown.
    double kc1,kc2,kc3,kc4;
  };

#endif

 /** @class ProjectionParametersZoom
     @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 streckel
 */
  class BIASGeometry_EXPORT ProjectionParametersZoom:
    public  ProjectionParametersBase {
  public:

    ProjectionParametersZoom(const unsigned int width = 0,
                             const unsigned int height = 0)
      : ProjectionParametersBase(width, height)
    {
      K_.SetIdentity();
      invK_.SetIdentity();
      zoom_=0;
      focallengthZoom_=0;
      kc1Zoom_=0; kc2Zoom_=0; kc3Zoom_=0; kc4Zoom_=0;
    };

    virtual const bool
    DoIntrinsicsDiffer(const ProjectionParametersBase* p) const {
      if (p->DoIntrinsicsDiffer((ProjectionParametersBase*)this))
        return true;
      ProjectionParametersZoom* ppz = (ProjectionParametersZoom*)p;
      if (ppz) {
        if (kc1Zoom_ != ppz->kc1Zoom_ || kc2Zoom_ != ppz->kc2Zoom_ ||
            kc3Zoom_ != ppz->kc3Zoom_ || kc4Zoom_ != ppz->kc4Zoom_ ||
            focallengthZoom_ != ppz->focallengthZoom_ || zoom_ != ppz->zoom_)
          return true;
      }
      return false;
    }

    ProjectionParametersZoom(const ProjectionParametersZoom & P){
      *this = P;
      K_ = P.K_;
      invK_ = P.invK_;
      knownparams_vect_ = P.knownparams_vect_;
      zoom_ = P.zoom_;
      focallengthZoom_ = P.focallengthZoom_;
      kc1Zoom_ = P.kc1Zoom_;
      kc2Zoom_ = P.kc2Zoom_;
      kc3Zoom_ = P.kc3Zoom_;
      kc4Zoom_ = P.kc4Zoom_;
    };

    virtual ~ProjectionParametersZoom(){};

    /** @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 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
        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 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 covariant virtual copy constructor for use in Projection */
    virtual ProjectionParametersZoom* Clone() const {
      return new ProjectionParametersZoom(*this);
    };


    inline void Clear(){
      knownparams_vect_.clear();
    }

    int AddZoomStep(double zoom, double focallength,
                    double kc1, double kc2, double kc3, double kc4);

    /* @brief get cached KMatrix
     */
    inline void GetK(KMatrix& K) const {
      K = K_;
    };

    /* @brief get cached inverted KMatrix
     */
    inline void GetKinv(KMatrix& invK) const {
      invK = invK_;
    };

    /* @brief get the PMatrix composed from the projection object
     */
    inline virtual BIAS::PMatrix GetP() const {
      return BIAS::PMatrix(K_,GetR(),GetC());
    }

    /** @brief Get the current camera zoom.
        @author buck */
    inline int GetZoom(double& zoom) const {
        zoom = zoom_;
        return 0;
    }

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

    /** @brief Get the current camera Undistortion.
        @author buck */
    inline void GetUndistortion(double& kc1, double& kc2,
                                double& kc3, double& kc4) const
    {
      kc1 = kc1Zoom_;
      kc2 = kc2Zoom_;
      kc3 = kc3Zoom_;
      kc4 = kc4Zoom_;
    }


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

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

    /** @brief Set the current camera zoom, the focal length will be
        calculated.
        @author buck */
    inline int SetZoom(const double abszoom) {
      zoom_=abszoom;
      focallengthZoom_ = InterpolateFocalLength_();
      ComputeK_();
      return 0;
    }

    /** @brief Set the current camera focal length, the zoom will be
        calculated.
        @author buck */
    inline int SetFocalLength(const double f) {
      focallengthZoom_=f;
      zoom_=InterpolateAbsZoom_();
      ComputeK_();
      return 0;
    }

    /** @brief Set the zoom and focal length of a zoom camera to the
        default settings.
        @author streckel 06/2004*/
    inline void SetUnzoomed() {
      if (knownparams_vect_.size()>0) {
        SetZoom(knownparams_vect_[0].zoom);
      }
    }


    /** @brief adapt internal params to resampled image
        @param ratio 2.0 = downsample, 0.5 = upsample
    */
    virtual inline void Rescale(double ratio, const double offset) {
      BIASERR("unimplemented");
      BIASABORT;
    }

    virtual inline void Rescale(unsigned int width, unsigned int height){
      BIASERR("unimplemented");
      BIASABORT;
    }

#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 functions
    friend std::ostream& operator<<(std::ostream &os,
                                    const ProjectionParametersZoom& p);
  protected:
    // focal length, aspect ratio, principal point in K_, inverse in invK_
    KMatrix K_, invK_;

   // array of camera calibration constants for zoom cameras
    std::vector<CPDiscreteParam> knownparams_vect_;

    // calibration settings for the current zoom setting of a zoom camera
    // zoom and focallengthZoom are dependend:
    // if zoom is set, focallengthZoom will be calculated and vice versa
    // focal length is stored in pixel
    double zoom_;
    double focallengthZoom_;
    double kc1Zoom_, kc2Zoom_, kc3Zoom_, kc4Zoom_;

    void InterpolateDistortionFromIndex_(unsigned int i,double relnorm);
    double InterpolateFocalLength_(); // w/ given abszoom
    double InterpolateAbsZoom_(); // w/ given focal length

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



  };

  inline std::ostream& operator<<(std::ostream &os,
                                  const ProjectionParametersZoom& p)
  { 
    os << "ProjectionParametersZoom:" << std::endl;
    return os;
  }

} // end namespace


#include <Base/Common/BIASpragmaEnd.hh>

#endif