ProjectionParametersDynamicLoad.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_ProjectionParametersDynamicLoad_hh__
#define __BIAS_ProjectionParametersDynamicLoad_hh__
#include "bias_config.h"

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

#ifndef WIN32

namespace BIAS {

// typedef to make it easier to set up our factory
typedef ProjectionParametersBase* (*maker_t)();


  /** @class ProjectionParametersDynamicLoad
   *  @ingroup g_geometry
   *  @brief can load exernally programmed child classes of ProjectionParametersBase
   *  @author sedlazeck */
  class BIASGeometry_EXPORT ProjectionParametersDynamicLoad
    : public  ProjectionParametersBase {
  public:
    
    ProjectionParametersDynamicLoad(const unsigned int width = 0,
                                    const unsigned int height = 0)
      : ProjectionParametersBase(width, height)
    {
      hndl_ = NULL;
      dynptr_ = NULL;
      className_ = "";
      //InitFactory_(); // needs to be called after setting string specifiying, which projection should be loaded
    }
    
    ProjectionParametersDynamicLoad(const std::string& className, const unsigned int width = 0,
                                    const unsigned int height = 0)
      : ProjectionParametersBase(width, height)
    {
      hndl_ = NULL;
      dynptr_ = NULL;
      className_ = className;
      InitFactory_();
    }


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

    ProjectionParametersDynamicLoad&
      operator=(const ProjectionParametersDynamicLoad & P){
      ProjectionParametersBase::operator=(P);
      if(P.dynptr_!=NULL){
        dynptr_ = P.dynptr_->Clone();
      } else {
        dynptr_ = P.dynptr_;
      }
      hndl_ = P.hndl_;
      className_ = P.className_;
      return *this;
    }


    virtual ~ProjectionParametersDynamicLoad();

    /** @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.
     *  @author koeser 09/2007 */
    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.
     *  @param it must be initialized by GetFirstBorderPixel
     *  @return false if the returned position closes the loop around the image
     *  @author koeser 09/2007 */
    virtual bool GetNextBorderPixel(PixelIterator& it);

    /** @brief call this to start a run at the outer boundary of an image.
     *
     *  You get the first edge position i.e. (-0.5, -0.5) for a perspective
     *  image. Feed this into GetNextBorderPixel to get the next position.
     *  @author bartczak 06/2009 */
    virtual void GetFirstEdgePosition(PixelIterator& it);

    /** @brief call this iteratively to run at the outer edge of an image.
     *
     *  Like the GetNextBorderPixel() method, however runs along the
     *  outer rim of an image, i.e. the coordinate range (-0.5, -0.5) and (W-0.5, H-0.5).
     *  The positions returned are the upper left corner of the pixels.
     *  These are at the same time the upper right corners of the neighboring pixel.
     *
     *  @param it must be initialized by GetFirstBorderPixel
     *  @return false if the returned position closes the loop around the image
     *  @author  bartczak 06/2009 */

    virtual bool GetNextEdgePosition(PixelIterator& it);

    /** Determines the maximal and minimal viewing range in means of
     *  spherical coordinates. The spherical coordinates are calculated
     *  relatively to the passed coordinate frame.
     *  The method is using the class SphericalCoordinates, see also the
     *  documentation there in order to understand the meaning of the
     *  sphericalReferenceFrame.
     *
     * \attention in order to make sense the sphericalReferenceFrame
     *  must have the same origin as the ProjectionParameters.
     *
     * \param sphericalReferenceFrame reference frame for the calculation of
     * spherical coordinates belonging to optical rays, must be defined in the
     * same coordinate frame like the local frame of the ProjectionParameters.
     *
     * \returns minPhi phi angle from (-M_PI, M_PI] with smallest value
     *          within viewing range.
     * \returns maxPhi phi angle from (-M_PI, M_PI] with largest value
     *          within viewing range.
     * \returns centerPhi phi angle from (-M_PI, M_PI] belonging to the
     *          optical ray passing through the image center. This angle
     *          allows to determine which direction the camera was facing
     *          in the sphericalReferenceFrame.
     * \returns minTheta theta angle from [0, M_PI] with smallest value
     *          within viewing range.
     * \returns maxTheta theta angle from [0, M_PI] with largest value
     *          within viewing range.
     *
     * \author bartczak 10/2007
     **/
    virtual int
      GetSphericalViewingRange(const CoordinateTransform3D& sphericalReferenceFrame,
                               double& minPhi, double& maxPhi, double& centerPhi,
                               double& minTheta, double& maxTheta);

    /** Delivers the assumed minimal angular distance between two optical rays
     * belonging to integer image coordinates within the FoV.
     * \author bartczak 10/2007
     **/
    virtual int GetMinimalAngularSamplingStep(double& minAngleStep)
    {
      return dynptr_->GetMinimalAngularSamplingStep(minAngleStep);
    }

    virtual const bool
        DoExtrinsicsDiffer(const ProjectionParametersBase* p) const{
      return dynptr_->DoExtrinsicsDiffer(p);
    }

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

    /** Checks if 3D point projects into specified image and returns
     *  belonging 2D image point.
     *  @param X assumes homogenized point!
     *  @author bartczak
     */
    virtual bool
      DoesPointProjectIntoImage(const HomgPoint3D& X, HomgPoint2D& x,
                                bool IgnoreDistortion = false) const{
      return dynptr_->DoesPointProjectIntoImage(X, x, IgnoreDistortion);
    }
    
    /** Checks if 3D point projects into specified image and returns
     *  belonging 2D image point.
     *  @param localX assumes homogenized point!
     *  @author bartczak
     */
    virtual bool
      DoesPointProjectIntoImageLocal(const Vector3<double>& localX,
                                     HomgPoint2D& x,
                                     bool IgnoreDistortion = false) const;


    /** @brief calculates the projection of a point in the world coordinate
        system to a pixel in the image plane of the camera
        !warning! the function my return an invalid point (0,0,0) in certain
        cases (3d point) behind camera or 3d point to close to camera center
        @author woelk 05/2006 */
    virtual inline HomgPoint2D Project(const HomgPoint3D &X,
                                       bool IgnoreDistortion = false) const {
      return dynptr_->Project(X, IgnoreDistortion);
    }

    /** @brief calculates the projection of a point in the world 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 valid 2d point is
        computed, *particularily* when the 3d point is behind the camera
        or when the resulting 2d point is at infinity.
        returns negative, when 3d point and camera center coincide
        @author woelk 05/2006 */
    virtual inline int Project(const HomgPoint3D &X, HomgPoint2D &p2d,
                               bool IgnoreDistortion = false) const {

      return dynptr_->Project(X, p2d, IgnoreDistortion);
    }

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

        !warning! function return an invalid 2d point (0,0,0) in certain cases

        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;

    /** @brief unprojects the covariance matrices to K=Identity camera

        This uses the unscented transform to propagate covariance. Use a faster
        or better specialization in derived class if you want.
        @author koeser 06/2006 */
    virtual Matrix3x3<double>
      UnProjectCovLocal(const HomgPoint2D& pos, const Matrix3x3<double>& cov2D,
                        bool IgnoreDistortion = false, bool Normalize = false);

    /** @brief calculates the viewing ray in the global coordinate frame
        from the camera center which belongs to the given image
        position. The resulting vector3 may have any length.

        @author woelk 05/2006 */
    virtual inline void UnProjectToRay(
      const HomgPoint2D& pos, Vector3<double>& pointOnRay,
      Vector3<double>& direction, bool IgnoreDistortion=false) const
    {
      dynptr_->UnProjectToRay(pos, pointOnRay, direction, IgnoreDistortion);
    }

    virtual inline void UnProjectToRay(
      const HomgPoint2D& pos, Vector3<double>& pointOnRay,
      Vector3<double>& direction, const ProjectionParametersBase &proj,
      bool IgnoreDistortion=false) const
    {
      dynptr_->UnProjectToRay(pos, pointOnRay, direction, proj, IgnoreDistortion);
    }

    /** @brief calculates a 3D point in
        the global (not the rig) coordinate system, which
        belongs to the image position pos with distance depth to the
        camera center.
        @return: (0,0,0,0) is returned on failure */
    virtual HomgPoint3D UnProjectToPoint(const HomgPoint2D& pos, double depth,
                                         bool IgnoreDistortion = false) const{
      return dynptr_->UnProjectToPoint(pos, depth, IgnoreDistortion);
    }

    virtual HomgPoint3D UnProjectToPoint(const HomgPoint2D& pos, const double depth,
                                         const ProjectionParametersBase &proj,
                                         bool IgnoreDistortion = false) const{
    
      return dynptr_->UnProjectToPoint(pos, depth, proj, IgnoreDistortion);
    }

    /** @brief calculates a 3D point in
        the local camera coordinate system, which
        belongs to the image position pos in cam with distance depth to the
        camera center cam.
        @author koeser  */
    virtual Vector3<double>
      UnProjectToPointLocal(const HomgPoint2D& pos, const double& depth,
                            bool IgnoreDistortion = false) const{
    
      return dynptr_->UnProjectToPointLocal(pos, depth, IgnoreDistortion);
    }

    /** Interface defintion for lens distortion function, implemented by
        derived classes */
    virtual bool Distort(BIAS::HomgPoint2D& point2d) const;

    /** Interface defintion for lens undistortion function, implemented by
        derived classes */
    virtual bool Undistort(BIAS::HomgPoint2D& point2d) const;


    /** @brief Get warp (local magnification/shear/...) when going from the
     *  real image to ideal image with focal length = 1.
     *
     *  When an image is undistorted to an ideal perspective camera with
     *  K=id, the region around each pixel must be warped. For perspective
     *  cameras without radial distortion this is the undoing (inverse) of
     *  the upper 2x2 part of the KMatrix (simply a rescale of coordinates).
     *  If radial distortion is present, the local warp depends on the image
     *  position. Same for most other camera models. This matrix is closely
     *  related to the absolute local image resolution.
     *
     *  Instead of going to the w=1 (homogenized) ideal image, we can as well
     *  go to a tangent image. This can be steered by the parameter
     *  homogenized (if false, we use normalized mode).
     *
     *  @author koeser 06/2008
     */
    virtual int GetUnProjectionJacobian(const HomgPoint2D& x,
                                        Matrix2x2<double>& Jac,
                                        const bool homogenized = true) const{
      return dynptr_->GetUnProjectionJacobian(x, Jac, homogenized);
    }

    // ---- Getter methods  -------------------------------------------

    /** @brief Get projection center. */
    virtual BIAS::Vector3<double> GetC() const 
    { 
      return dynptr_->GetC();
    }

    /** @brief Get orientation as unit quaternion. */
    virtual BIAS::Quaternion<double> GetQ() const
    {
      return dynptr_->GetQ();
    }

    /** @brief Return pose as 7 vector (first projection center C, then
        unit quaternion Q representing orientation). */
    virtual BIAS::Vector<double> GetCQ() const
    {
      return dynptr_->GetCQ();
    }

    /** @brief Get orientation as rotation matrix R. */
    virtual BIAS::RMatrix GetR() const{
      return dynptr_->GetR();
    }

    /** @brief Return pose covariance as 7x7 matrix with respect to C, Q. */
    virtual BIAS::Matrix<POSE_TYPE> GetCov() const {
      return dynptr_->GetCov();
    }

    /** @brief Return complete pose object. */
    virtual const BIAS::Pose& GetPose() const
    {
      return dynptr_->GetPose();
    }

    /** @brief Return copy of pose parametrization object. */
    virtual inline BIAS::PoseParametrization GetPoseParametrization() const 
    { 
      return dynptr_->GetPoseParametrization();
    }

    /** @brief Set pose from pose parametrization. */
    virtual inline void SetPoseParametrization(const BIAS::PoseParametrization& pp)
    { dynptr_->SetPoseParametrization(pp); }

    /** @brief Return cached 3x4 representation of external matrix [R|C] */
    virtual BIAS::Matrix3x4<double> GetExternals() const 
    { 
      return dynptr_->GetExternals();
    }

    /** @brief Obtain image dimensions. */
    virtual inline int GetImageSize(unsigned int& Width, unsigned int& Height) const {
      return dynptr_->GetImageSize(Width, Height);
    }

    /**
     * @brief Returns the image width.
     *
     * @return the image width
     */
    virtual inline
    const unsigned int& GetImageWidth() const
    {
      return dynptr_->GetImageWidth();
    }

    /**
     * @brief Returns the image height.
     *
     * @return the image height
     */
    virtual inline
    const unsigned int& GetImageHeight() const
    {
      return dynptr_->GetImageHeight();
    }

    virtual inline std::string GetIdentifier() const {
      return dynptr_->GetIdentifier();
    }

    virtual inline std::string GetVideoSourceType() const {
      return dynptr_->GetVideoSourceType();
    }

    /** @brief Return aspectratio (i.e. cellsizeX / cellsizeY). */
    virtual inline double GetAspectratio() const {
      return dynptr_->GetAspectratio();
    }

    /** @brief Get principal point (in pixels relative to top left corner). */
    virtual inline int GetPrincipal(double& PrincipalX, double& PrincipalY) const {
      return dynptr_->GetPrincipal(PrincipalX, PrincipalY);
    }

    // ---- Setter methods  ------------------------------------------------

    /** @brief Set CCD aspect ratio (i.e. cellsizeX / cellsizeY).
        @author streckel */
    virtual inline void SetAspectratio(const double AspectRatio) {
      dynptr_->SetAspectratio(AspectRatio);
    }

    /** @brief Set principal point (in pixels relative to top left corner).
        @author streckel */
    virtual inline void SetPrincipal(const double x, const double y) {
      dynptr_->SetPrincipal(x, y);
    }

    /** @brief Set image dimensions (in pixels). */
    virtual inline void SetImageSize(const unsigned int w,
                                     const unsigned int h) {
      dynptr_->SetImageSize(w, h);
    }

    /** @brief Set the identification string */
    virtual inline void SetIdentifier(std::string name ){
      dynptr_->SetIdentifier(name);
    }

    /** @brief Set the camera source type */
    virtual void SetVideoSourceType(const std::string &name){
      dynptr_->SetVideoSourceType(name);
    }

    /** @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){
        dynptr_->Rescale(ratio, offset);
    }

    /**
     * @brief adapt internal parameters to new image size
     * @param width and height are the new images sizes
     *  \attention Delivers only correct results if ratio is chosen
     *   to rescale image size to integral image dimensions in both directions.
     */
    virtual void Rescale(unsigned int width, unsigned int height){
      dynptr_->Rescale(width, height);
    }

    /** @brief Covariant virtual copy constructor used in #BIAS::Projection. */
    virtual ProjectionParametersBase* Clone() const {
      return new ProjectionParametersDynamicLoad(*this);
    }

    /** @brief Set orientation from unit quaternion Q. */
    virtual void SetQ(const BIAS::Quaternion<double>& Q)
    { dynptr_->SetQ(Q); }

    /** @brief Set projection center. */
    virtual void SetC(const BIAS::Vector3<double>& C)
    { dynptr_->SetC(C); }

    /** @brief Set pose from unit quaternion Q and projection center C. */
    virtual void SetQC(const BIAS::Quaternion<double>& Q,
                       const BIAS::Vector3<double>& C)
    { dynptr_->SetQC(Q, C); }

    /** @brief Set pose from pose object. */
    virtual void SetPose(const BIAS::Pose pose)
    { dynptr_->SetPose(pose); }

    /** @brief Set orientation from rotation matrix R. */
    virtual void SetR(const BIAS::RMatrix& R) {
      dynptr_->SetR(R);
    }

    /** @brief Set pose covariance matrix with respect to C, Q. */
    virtual void SetCov(const Matrix<POSE_TYPE>& Cov)
    { dynptr_->SetCov(Cov); }

    /** @brief Check of current projection center is valid. */
    virtual inline bool CValid() const { return dynptr_->CValid(); }

    /** @brief Check if current orientation is valid. */
    virtual inline bool QValid() const { return dynptr_->QValid(); }

    /** @brief Check if current pose is valid. */
    virtual inline bool PoseValid() const
    { return dynptr_->PoseValid(); }

    /** @brief Invalidate currently set pose. */
    virtual inline void InvalidatePose()
    { dynptr_->InvalidatePose(); }

    /** @brief Validate currently set pose. */
    virtual void ValidatePose() {
      dynptr_->ValidatePose();
    }

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

        @attention Stolen from PPSpherical, since this is a general issue.
        THERE MAY BE PROBLEMS FOR OTHER THAN FISHEYE CAMERAS (UNTESTED)
        If this implementation is not correct for your derived projection
        parameters you have to reimplement it in the derived class.

        The FoV of the KMatrix is specified by maxangle (PI*80/180 for 160deg)
        or the max 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 this image
        1 - The perspective image has the same resolution as this
        image in the center (highest resolution)
        2 - The perspective image has the same resolution as this
        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 (moved here by koeser)
    */
    virtual BIAS::KMatrix GetFakeKMatrix(double& imgsize, int resolution = 0,
                                         const double& maxangle = 1.4) const{
      return dynptr_->GetFakeKMatrix(imgsize, resolution, maxangle);
    }
    virtual BIAS::KMatrix GetFakeKMatrix(int resolution = 0,
                                         const double& maxangle = 1.4) const{
      return dynptr_->GetFakeKMatrix(resolution, maxangle);
                             
    }

    /** @brief Returns difference between two projections like |C1-C2|.
        This function can be used to calculate inter-camera distance e.g.
        for ordering purposes.
        @author koeser 03/2006
    */
    virtual double ViewDifference(const ProjectionParametersBase *pPPB) const{
      return dynptr_->ViewDifference(pPPB);
    }

#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
    
    
    /** Worker function for Unscented Transform.
        @author koeser
    */
    virtual int Transform_(const Vector<double>& src,
                           Vector<double>& dst) const{
      return dynptr_->Transform_(src, dst);
    }

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

    /// get base pointer to dynamically loaded object in order to handle it directly
    ProjectionParametersBase* GetDynPtr(){return dynptr_;}
    
    void SetClassName(std::string& name){
      className_ = name;
    }
    
  protected:
    void InitFactory_();
    
    void *hndl_;
    std::map<std::string, maker_t, std::less<std::string> > factory_;
    ProjectionParametersBase* dynptr_;
    std::string className_;
    std::vector<std::string> libPaths_;
  };



  //////////// inline code //////////////////
  inline std::ostream& operator<<(std::ostream &os,
                                  const ProjectionParametersDynamicLoad& p)
  {
    os << "ProjectionParametersDynamicLoad:" << std::endl;
    os << *(p.dynptr_) << std::endl;
    return os;
  }


} // end namespace


#include <Base/Common/BIASpragmaEnd.hh>

#endif //WIN32

#endif