Projection.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_Projection_hh__
#define __BIAS_Projection_hh__

#include <bias_config.h>

#include <math.h>

#include <Base/Debug/Debug.hh>
#include <Base/Math/Matrix4x4.hh>
#include <Base/Math/Vector4.hh>
#include <Base/Geometry/HomgPoint3D.hh>
#include <Base/Geometry/Quaternion.hh>
#include <Geometry/Pose.hh>
#include <Geometry/PMatrix.hh>
#include <Geometry/ProjectionParametersBase.hh>
#include <Geometry/ProjectionParametersPerspective.hh>
#include <Geometry/ProjectionParametersPerspectiveDepth.hh>
#include <Geometry/ProjectionParametersSpherical.hh>
#include <Geometry/ProjectionParametersSphericalFast.hh>
#include <Geometry/ProjectionParametersSphericalSimple.hh>
#include <Geometry/ProjectionParametersCylindric.hh>
#include <Geometry/ProjectionParametersProjective.hh>
#include <Geometry/ProjectionParametersIO.hh>
#ifndef WIN32
#ifdef BUILD_DYNAMIC_LOAD_PROJECTION
#include <Geometry/ProjectionParametersDynamicLoad.hh>
#endif
#endif
#include <Geometry/ProjectionParametersFactory.hh>

#ifdef BIAS_HAVE_XML2
#  include <Base/Common/XMLBase.hh>
#endif // BIAS_HAVE_XML2


namespace BIAS {

  /** 
   * @class Projection
   * @ingroup g_geometry
   * @brief This class hides the underlying projection model, like projection matrix,
   *  spherical camera, lens distortion. Read and write projections with XMLRead(filename) and XMLWrite(filename)
     * @author MIP
  */
  class BIASGeometry_EXPORT Projection
#ifdef BIAS_HAVE_XML2
    : public XMLBase
#endif // BIAS_HAVE_XML2
  {
  public:
    /** \brief constructor */
    Projection();   
    /** \brief copy constructor */
    Projection(const Projection& p);
    /** \brief constructor for initialization with a ProjectionParametersBase object*/
    explicit Projection(const ProjectionParametersBase& p) ;
    /** \brief destructor for initialization with a ProjectionParametersBase object*/
    virtual ~Projection();
    /** \brief operator=*/
    const Projection& operator=(const Projection& p) {
      // delete old parameters if any
      for (unsigned int i=0; i<vecParameters_.size(); i++) {
        delete vecParameters_[i];      
      }
      vecParameters_.clear();

      // copy new parameters
      vecParameters_.resize(p.vecParameters_.size());
      for (unsigned int i=0; i<p.vecParameters_.size(); i++) 
        vecParameters_[i] = p.vecParameters_[i]->Clone();
      return *this;
    }

    /**
       @brief Create a perspective camera and add to projection               
       @param pose the pose to use
       @param K the KMatrix of the new camera 
       @param width width of the new camera
       @param height height of the new camera
       @param radDistType the radial distortion type (DISTYPE_DEF or DISTYPE_BROWN)
       @param UndistortionCoefficients std::vector containing the distortion parameters     
       @author ischiller
       @date 09/08
    */
    void CreatePerspective(const BIAS::Pose& pose, 
                           const BIAS::KMatrix& K,
                           int width, int height,
                           BIAS_ProjParaPersp_DISTORTION_TYPE radDistType=DISTYPE_DEF,
                           const std::vector<double>&
                           UndistortionCoefficients = 
                           std::vector<double>(4, 0.0));

    /** @brief push_back a new sperical cam to this
        @author Jan Woetzel */
    void CreateSpherical( const Pose& pose, 
                          const unsigned int width, const int height,
                          const double principalX, const double principalY,
                          const double radius,
                          const double aspect=1.0 );

    /**
       @brief Add a camera to the projection,the coordinates of the pose 
       are given in absolute Coordinates, they are transformed to 
       relative coordinates to the first camera internally              
       @param ppB Pointer to ProjectionParametersBase, 
       the Object is copied and stored in vecParameters_
       @author ischiller
       @date 07/07
    */
    void AddAbsoluteCamera(ProjectionParametersBase *ppb);

    /**
       @brief Add a camera to the projection,the coordinates of the pose 
       are given in relative Coordinates, first cam is origin
       @param ppB Pointer to ProjectionParametersBase, 
       the Object is copied and stored in vecParameters_
       @author ischiller
       @date 07/07
    */
    void AddRelativeCamera(ProjectionParametersBase *ppb);


#ifdef BIAS_HAVE_XML2
    /** 
     *  @brief Reads a camera data XML file and constructs the 
     *  ProjectionParametersBase-pointer, using the ProjectionParametersIO
     *  and the CameraParameter file format.
     *  @param filename Name of the XML parameter file */
    int ReadFromCamParamFile(const std::string& filename, const bool silent = false);
  
#endif // BIAS_HAVE_XML2

    /** @brief read BBC's FreeD data from file and constructs a perspective
        projection (may have radial distortion)
        @author koeser
        @date 11/2006 */
    int ReadFromBBC(const std::string& filename, const bool silent = false);

    /** @brief initializes projection from cameradata object
        @author streckel (separated by koeser 11/06) */
    int InitFromCameraData(const std::vector<CameraData>& vcd);

    /** @brief convenience wrapper which tries to read different formats
        @author bartczak, koeser */
    virtual int Load(const std::string& filename);

    /** \brief Determine whether the Projection is Empty
     * \return true if Projection is empty, false otherwise  */
    bool IsEmpty() const { return (vecParameters_.size()==0);}

    /** \brief Determine number of ProjectionParameterBase pointers in Projection
      * \return The number of ProjectionParameterBase and derived objects in Projection */
    inline unsigned int Size() const
    {
      return (unsigned int)vecParameters_.size();
    }

    /** \brief Delete all ProjectionParameterBase objects contained in Projection */
    inline void Clear() 
    {     
      for (unsigned int i=0; i<vecParameters_.size(); i++) {
        delete vecParameters_[i];
      }
      vecParameters_.clear();
    }

    /** @brief const parameter access function
     *  @return NULL on error, pointer to parameters otherwise */
    const ProjectionParametersBase* GetParameters(unsigned int cam = 0) const {
      if(cam >= vecParameters_.size()){
        BIASWARN("No parameters for camera with index:"<<cam);
        return NULL;
      }
      return vecParameters_[cam];
    } 
   
    /** @brief Non-const access function to parameters
     *  @return NULL on error, pointer to parameters otherwise */
    ProjectionParametersBase* GetParameters(unsigned int cam = 0) {
        if(cam >= vecParameters_.size()){
            BIASWARN("No parameters for camera with index:"<<cam);
            return NULL;
        }
      return vecParameters_[cam];
    }

    /** 
     * @brief Allocates memory for parameters base type and returns pointer to it!
     * @attention The responsiblity to free associated memory is passed to 
     *   recipient!
     * @return NULL on error, pointer to parameters otherwise 
     */
    ProjectionParametersBase* GetParameterCloneWithAbsolutePose(unsigned int cam = 0) const {
      if(cam >= vecParameters_.size()){
        BIASWARN("No parameters for camera with index:"<<cam);
        return NULL;
      }
      ProjectionParametersBase* clone = vecParameters_[cam]->Clone();
      if(cam!=0) {
        Pose rigPose = vecParameters_[0]->GetPose();
        Pose absolutePose;
        rigPose.ConcatenateLocalTransform(clone->GetPose(), absolutePose);
        clone->SetPose(absolutePose);
      }
      return clone;

    }

    /** @brief return Center of camera with index cam.
        If cam>0 this is a relative pose in the coordinate system of cam 0 */
    inline Vector3<double> GetC(unsigned int cam = 0) const {
      BIASASSERT(cam<vecParameters_.size() && vecParameters_[cam]);
      return vecParameters_[cam]->GetC();
    }
    
    /** @brief return RMatrix of camera with index cam.
        If cam>0 this is a relative pose in the coordinate system of cam 0 */
    inline RMatrix GetR(unsigned int cam = 0) const {
      BIASASSERT(cam<vecParameters_.size() && vecParameters_[cam]);
      return vecParameters_[cam]->GetR();
    }

    /** @brief return RMatrix of camera with index cam.
        If cam>0 this is an absolute rotation in the global coordinate system */
    inline RMatrix GetAbsoluteR(unsigned int cam = 0) const {
      BIASASSERT(cam<vecParameters_.size());
      BIASASSERT(vecParameters_.size()>cam && vecParameters_[0] && vecParameters_[cam]);
      if(cam>0)
        return vecParameters_[0]->GetR() * vecParameters_[cam]->GetR();
      else
        return vecParameters_[cam]->GetR();
    }
    
    /** @brief Return center of camera with index cam.
      * If cam>0 this is an absolute position in the global coordinate system */
    inline Vector3<double> GetAbsoluteC(unsigned int cam = 0) const {
      BIASASSERT(cam<vecParameters_.size());
      BIASASSERT(vecParameters_.size()>cam  && vecParameters_[0] && vecParameters_[cam]);
      if(cam>0)
        return vecParameters_[0]->GetC() + 
          vecParameters_[0]->GetR() *vecParameters_[cam]->GetC();
      else
        return vecParameters_[0]->GetC();
    }

    /** @brief return rotation quaternion of camera with index cam.
        If cam>0 this is an absolute rotation in the global coordinate system */
    inline Quaternion<double> GetAbsoluteQ(unsigned int cam = 0) const {
      BIASASSERT(cam<vecParameters_.size());
      BIASASSERT(vecParameters_.size()>cam && vecParameters_[0] && vecParameters_[cam]);
      if(cam>0) {
        return vecParameters_[0]->GetQ() * vecParameters_[cam]->GetQ();
      }
      else
        return vecParameters_[0]->GetQ();
    }
    
    /** @brief return rotation quaternion of camera with index cam 
        if cam>0 this is a relative pose in the coordinate system of cam 0 */
    BIAS::Quaternion<double> GetQ(unsigned int cam = 0) const;

    /** @brief Obtain the external camera parameters
     *  @return Const reference to external pose parameters as Matrix3x4<double>
     *  @attention The Externals of cameras with cam >0 may be incorrect
     *  @author koeser
     *  @date 02/2006 */
    inline Matrix3x4<double> GetExternals(unsigned int cam = 0) const {
      BIASASSERT(vecParameters_.size()>cam && vecParameters_[cam]);
      BIASASSERT(cam<vecParameters_.size());
      if (cam==0) { 
        return vecParameters_[cam]->GetExternals();
      } else {
        BIASWARN("Externals of relative camera may be incorrect!");
        return vecParameters_[cam]->GetExternals();
      }
    }

    /** @brief Get the Pose covariance
     *  @attention The covariance of cameras with cam >0 may be incorrect
     *  @return The covariance Matrix  */
    virtual Matrix<double> GetCov(unsigned int cam = 0) const {
      BIASASSERT(cam<vecParameters_.size() && vecParameters_[cam]);
      if (cam==0) return vecParameters_[cam]->GetCov();
      else {
        BIASWARN("Not yet defined behaviour!");
        return vecParameters_[cam]->GetCov();
      }
    }

    /** @brief return complete pose object.
        If cam>0 this is a relative pose in the coordinate system of cam 0 */
    virtual const Pose& 
      GetPose(unsigned int cam = 0) const {
      BIASASSERT(cam<vecParameters_.size() && vecParameters_[cam]);
      return vecParameters_[cam]->GetPose();
    }
  
    /** @brief Set the Pose covariance  */
    virtual void SetCov(const Matrix<double>& Cov, unsigned int cam = 0) {
      BIASASSERT(cam<vecParameters_.size() && vecParameters_[cam]);
      vecParameters_[cam]->SetCov(Cov);
    }

    /** @brief Set center of camera with index cam.
        If cam>0 this is a relative pose in the coordinate system of cam 0 */
    virtual void SetC(const Vector3<double>& C, unsigned int cam = 0) {
      BIASASSERT(vecParameters_.size()>cam && vecParameters_[cam]);
      vecParameters_[cam]->SetC(C);
    }

    /** @brief Set rotation quaternion of camera with index cam.
        If cam>0 this is a relative pose in the coordinate system of cam 0 */
    virtual void SetQ(const Quaternion<double>& Q, unsigned int cam = 0) {
      BIASASSERT(vecParameters_.size()>cam && vecParameters_[cam]);
      vecParameters_[cam]->SetQ(Q);
    } 

    /** @brief Set rotation matrix of camera with index cam.
        If cam>0 this is a relative pose in the coordinate system of cam 0 */
    virtual void SetR(const RMatrix& R, unsigned int cam = 0) {
      BIASASSERT(vecParameters_.size()>cam && vecParameters_[cam]);
      vecParameters_[cam]->SetR(R);
    }

    /** @brief Set the Pose of the camera with index cam.
     *  If cam>0 this is a relative pose in the coordinate system of cam 0 */
    virtual void SetPose(const BIAS::Pose pose, unsigned int cam = 0) {
      BIASASSERT(vecParameters_.size()>cam && vecParameters_[cam]);
      vecParameters_[cam]->SetPose(pose);
    }

    /** @brief Determine whether the pose is valid or not.
     * @return true if pose valid, false otherwise. */
    inline bool PoseValid(unsigned int cam = 0) const {
      if (vecParameters_.size()<=cam || !vecParameters_[cam]) return false;
      return vecParameters_[cam]->PoseValid();
    }

    /** @brief returns the 2d projection of X in camera cam,
     *  where X is given in the global coordinate frame (not the rig)
     *  (needs HomgPoint3D to map points at infinity)
     *  @return A HomgPoint2D with the 2D positio nof the 3D point in the
     *  image of the camera. */
    virtual HomgPoint2D Project(const HomgPoint3D &X, unsigned int cam = 0,
                        bool IgnoreDistortion = false) const;

    /** @brief Checks if 3D point projects into specified image and returns
     *  belonging 2D image point.
     *  @param X assumes homogenized point!
     *  @author bartczak
     *  @return true if the point projects in the image of the camera,
     *  false if point does not project into camera image. */
    virtual bool DoesPointProjectIntoImage(const BIAS::HomgPoint3D& X, 
                                           BIAS::HomgPoint2D& x, 
                                           unsigned int cam = 0,
                                           bool IgnoreDistortion = false) const;

    /**
     * \brief Checks whether extrinsic parameters are different
     * \return false if all params in argument and all params (*this)
     * have same extrinsics.  */
    virtual const bool DoExtrinsicsDiffer(const BIAS::Projection &p) const;
    
    /**
     * \brief Checks whether intrinsic parameters are different
     * \return false if all params in argument and all params in (*this)
     * have same intrinsics */
    virtual const bool DoIntrinsicsDiffer(const BIAS::Projection &p) const;


    /** @brief calculates the viewing ray in the global coordinate frame 
        (not the rig) from the camera center which belongs to the given image 
        position. The resulting vector3 may have any length.
        @return Vector3<double> with ray parameters. */
    void UnProjectToRay(const HomgPoint2D& pos, Vector3<double>& origin,
                                   Vector3<double>& rayDir,
                                   unsigned int cam = 0,
                                   bool IgnoreDistortion = false) const;

    /** @brief calculates the viewing ray in the global coordinate frame
        (not the rig) from the camera center which belongs to the given image
        position. The resulting vector3 may have any length.
        @return Vector3<double> with ray parameters. */
    void UnProjectToRay(const HomgPoint2D& pos,
                               Vector3<double>& rayDir,
                                   unsigned int cam = 0,
                                   bool IgnoreDistortion = false) const;

    /** @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 of cam. */
    Vector3<double> UnProjectToPoint(const HomgPoint2D& pos,
                                     double depth,
                                     unsigned int cam = 0,
                                     bool IgnoreDistortion = false) 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.                
        @author koeser  */
    Vector3<double> UnProjectToPointLocal(const HomgPoint2D& pos,
                                          const double& depth,
                                          unsigned int camSystem = 0,
                                          unsigned int cam = 0,
                                          bool IgnoreDistortion = false) const;


    /** @brief calculate 3D point and associated covariance by unprojection
        @author woelk 03/2006 */
    Matrix3x3<double> UnProjectToCovLocal(const HomgPoint2D& pos,
                                          const Matrix3x3<double>& cov2D, 
                                          const double& depth,
                                          unsigned int camSystem = 0,
                                          unsigned int cam = 0,
                                          bool IgnoreDistortion = false,
                                          bool Normalize = false) const;

    /** @brief calculates a 3D ray in
        a local camera coordinate system specified by camSystem, which 
        belongs to the image position pos in cam. */
    void UnProjectToRayLocal(const HomgPoint2D& pos, Vector3<double>& origin,
                                        Vector3<double>& rayDir,
                                        unsigned int camSystem = 0,
                                        unsigned int cam = 0,
                                        bool IgnoreDistortion = false) const;

#ifdef BIAS_HAVE_XML2
    /**@brief specialization of XML block name function for BIAS::Projection.
     * returns the xml class name Projection    */
    virtual int XMLGetClassName(std::string& TopLevelTag,
                                double& Version) const;

    /** @brief specialization of XML write function for BIAS::Projection.
     *  Called if the function XMLWrite is called.*/
    virtual int XMLOut(const xmlNodePtr Node, XMLIO& XMLObject) const;

    /** @brief specialization of XML read function for BIAS::Projection. 
     *  Called if the function XMLRead is called.*/
    virtual int XMLIn(const xmlNodePtr Node, XMLIO& XMLObject); 
#endif // BIAS_HAVE_XML2
    
    /** @brief adapt internal params to resampled image 
        @param ratio 2.0 = downsample, 0.5 = upsample */
    inline void Rescale(float ratio, unsigned int cam = 0) {
      BIASASSERT(cam < vecParameters_.size());
      BIASASSERT(vecParameters_[cam]!=NULL);
      vecParameters_[cam]->Rescale(ratio);
    }

    /** \brief Set the identifier of the Projection such as a name  */
    inline void SetIdentifier(const std::string& identString) {
      identifier_ = identString;
    }
    
    /** \brief Return the identifier of the Projection such as a name. */
    inline std::string GetIdentifier() const {
      return identifier_;
    }

    /** \brief The outstream operator, used in XMLOut, etc */
    friend std::ostream& operator<<(std::ostream &os, const Projection& p);

  protected:

    /// the intrinsics (like K, ...) of the camera usually only one
    /// only in rig mode there may be many, in that case vecIntrinsics[0]
    /// holds the external pose while the others hold the internal pose 
    /// (pose in the coordinate system of the first camera)
    std::vector<ProjectionParametersBase* > vecParameters_; 

    /** Multifunctional identifier. */
    std::string identifier_;

  };

  inline std::ostream& operator<<(std::ostream &os, const Projection& p) {
    os <<"Identifier: """<<p.GetIdentifier()<<"\n";
    os <<"vecParameters_.size(): "<<p.Size()<<"\n";
    std::vector<ProjectionParametersBase* >::const_iterator it;
    for (it = p.vecParameters_.begin(); it!=p.vecParameters_.end(); it++){
#ifndef WIN32
#ifdef BUILD_DYNAMIC_LOAD_PROJECTION
      // check for dynamic projection first because writing base parameters to the strem
      // does not make any sense
      if ((dynamic_cast<ProjectionParametersDynamicLoad*>(*it))!=NULL){
        os << *(dynamic_cast<ProjectionParametersDynamicLoad*>(*it));
        continue;
      } 
#endif
#endif      
        //ostream of ProjectionParametersBase 
      os<< **it;
      //check which type the current parameters are by dynamic cast and call ostream operator of it.
      //ProjectionParametersPerspectiveDepth has to be checked first, because it is derived from ProjectionParametersPerspective
      if ((dynamic_cast<ProjectionParametersPerspectiveDepth *>(*it))!=NULL)
        os <<*(dynamic_cast<ProjectionParametersPerspectiveDepth *>(*it));
      else if ((dynamic_cast<ProjectionParametersPerspective *>(*it))!=NULL) 
        os <<*(dynamic_cast<ProjectionParametersPerspective *>(*it));
      else if ((dynamic_cast<ProjectionParametersSpherical*>(*it))!=NULL)
        os << *(dynamic_cast<ProjectionParametersSpherical *>(*it));
      else if ((dynamic_cast<ProjectionParametersSphericalFast*>(*it))!=NULL)
        os << *(dynamic_cast<ProjectionParametersSphericalFast *>(*it));
      else if ((dynamic_cast<ProjectionParametersSphericalSimple*>(*it))!=NULL)
        os << *(dynamic_cast<ProjectionParametersSphericalSimple*>(*it));
      else if ((dynamic_cast<ProjectionParametersProjective*>(*it))!=NULL)
        os << *(dynamic_cast<ProjectionParametersProjective*>(*it));
    }
    return os;
  }


} // end namespace 


#endif //__BIAS_Projection_hh__