ProjectionParametersProjective.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_ProjectionParametersProjective_hh__
#define __BIAS_ProjectionParametersProjective_hh__
#include "bias_config.h"

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

namespace BIAS {

 /** @class ProjectionParametersProjective
     @brief 12 parametric p for completely projective reconstruction
     scenarios, unknown focal lengths, self calibration, ...

     The handling of the "pose" in this class is special because we cannot
     decompose P to get the center and the rotation.
     The center computation is possible via the nullspace of P, but there is
     not way to retrieve R. In addition, the local project and upproject methods
     cannot be used as well a stuff like SetR, SetC, distort, undistort, etc.
     Therefore a lot of BIASABORTs are return by this class.

     @author koeser 01/2006 update sedlazeck 08/2009
 */
  class BIASGeometry_EXPORT ProjectionParametersProjective:
    public  ProjectionParametersBase {
  public:

    ProjectionParametersProjective(const unsigned int width = 0,
                                   const unsigned int height = 0)
      : ProjectionParametersBase(width, height)
    {
    }

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

    ProjectionParametersProjective &
    operator=(const ProjectionParametersProjective & P){
      ProjectionParametersBase::operator=(P);
      return *this;
    }

    virtual ~ProjectionParametersProjective(){}

    virtual const bool
    DoExtrinsicsDiffer(const ProjectionParametersBase* p) const {
      BIASERR("cannot compare extriniscs for this parameter type")
      BIASABORT
      return false;
    }

    virtual const bool
    DoIntrinsicsDiffer(const ProjectionParametersBase* p) const{
      BIASERR("cannot compare intrinsics for this parameter type")
      BIASABORT
      return false;
    }


    bool DoesPointProjectIntoImageLocal(const Vector3<double>& localX,
                                           HomgPoint2D& x,
                                           bool IgnoreDistortion = false) const;

    bool DoesPointProjectIntoImage(const Vector3<double>& X,
                                       HomgPoint2D& x,
                                       bool IgnoreDistortion) const;

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


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

    inline HomgPoint2D Project(const HomgPoint3D &X,
                                  bool IgnoreDistortion = false) const {
      HomgPoint3D X_G(X);
      HomgPoint3D X_L = Pose_.GlobalToLocal(X_G);
      X_L.Homogenize();
      HomgPoint2D x_l(X_L[0], X_L[1], X_L[2]);
      return x_l;
    }

    inline int Project(const HomgPoint3D &X, HomgPoint2D &p2d,
                          bool IgnoreDistortion = false) const {
      HomgPoint3D X_G(X);
      HomgPoint3D X_L = Pose_.GlobalToLocal(X_G);
      X_L.Homogenize();
      p2d = HomgPoint2D(X_L[0], X_L[1], X_L[2]);
      return 0;
    }


    inline void UnProjectToRay(const HomgPoint2D& pos, Vector3<double>& pointOnRay,
                               Vector3<double>& direction,
                               bool IgnoreDistortion=false) const {

      HomgPoint3D temp3D = HomgPoint3D(pos);
      HomgPoint3D unprojected = Pose_.LocalToGlobal(temp3D);
      unprojected.Homogenize();
      direction = Vector3<double>(unprojected[0], unprojected[1], unprojected[2]);
      pointOnRay = GetC();
    }

    inline void UnProjectToRay(const HomgPoint2D& pos, Vector3<double>& pointOnRay,
                               Vector3<double>& direction,
                               const ProjectionParametersBase &proj,
                               bool IgnoreDistortion=false) const {
      BIASERR("R matrix not available in projective case!!!")
      BIASABORT
    }

    virtual Vector3<double> UnProjectToPointLocal(const HomgPoint2D& pos, const double& depth,
                              bool IgnoreDistortion = false) const {
      BIASERR("Cannot perform local unprojection in projective case!!!")
      BIASABORT
      Vector3<double> dummy;
      return dummy;
    }

    /** @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 {

      BIASERR("No local projection possible in projective case!!!")
      BIASABORT

      HomgPoint2D dummy;
      return dummy;
    }

    /** @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 radius 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
    {

      BIASERR("No local unprojection possible in projective case!!!")
      BIASABORT
//      HomgPoint3D X_L((Vector3<double>) pos);
//      HomgPoint3D X_G = Pose_.LocalToGlobal(X_L);
//      X_G.Homogenize();
//      Vector3<double> x_g(X_G[0], X_G[1], X_G[2]);
    }

    virtual Matrix3x3<double>
    UnProjectCovLocal(const HomgPoint2D& pos,
                   const Matrix3x3<double>& cov,
                   bool IgnoreDistortion = false,
                   bool Normalize = false) const;


    int GetUnProjectionJacobian(const HomgPoint2D& x,
                                            Matrix2x2<double>& Jac,
                                            const bool homogenized=true) const{

      BIASERR("Not possible in projective case!!!")
      BIASABORT

      return 0;
    }


    /** @brief get projection center */
    virtual Vector3<double> GetC() const;

    virtual BIAS::Quaternion<double> GetQ() const {
      BIASERR("PMatrix decomposition not possible in projective case!!!")
      BIASABORT

      BIAS::Quaternion<double> dummy;
      return dummy;
    }



    virtual BIAS::Vector<double> GetCQ() const{
      BIASERR("PMatrix decomposition not possible in projective case!!!")
      BIASABORT

      BIAS::Vector<double> dummy;
      return dummy;
    }

    /** @brief return R (R^T which is in front part of PMatrix ) */
    virtual RMatrix GetR() const {
      BIASERR("PMatrix decomposition not possible in projective case!!!")
      BIASABORT

      BIAS::RMatrix dummy;
      return dummy;
    };

    virtual const BIAS::Pose& GetPose() const{
      BIASERR("PMatrix decomposition not possible in projective case!!!")
      BIASABORT

      return Pose_;
    }


    virtual inline BIAS::PoseParametrization GetPoseParametrization() const{
      BIASERR("PMatrix decomposition not possible in projective case!!!")
      BIASABORT

      BIAS::PoseParametrization dummy;
      return dummy;
    }

    inline void SetPoseParametrization(const BIAS::PoseParametrization& pp){
      BIASERR("PMatrix decomposition not possible in projective case, you can only use SetP!!!")
      BIASABORT

    }

    virtual BIAS::Matrix3x4<double> GetExternals() const{
      BIASERR("PMatrix decomposition not possible in projective case!!!")
      BIASABORT

      BIAS::Matrix3x4<double> dummy;
      return dummy;
    }

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



#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
    /** @brief set 12 entries if projective P using a PMatrix */
    void SetP(const Matrix3x4<double>& P) { Pose_.SetFromMatrix3x4(P);}

    void SetR(const RMatrix& R) {
      BIASERR("dont call this for projective !");
      BIASABORT
    }
    void SetC(const Vector3<double>& C) {
      BIASERR("dont call this for projective !")
      BIASABORT
    }

    void SetQ(const BIAS::Quaternion<double>& Q){
      BIASERR("dont call this for projective !")
      BIASABORT
    }

    virtual void SetQC(const BIAS::Quaternion<double>& Q,
                           const BIAS::Vector3<double>& C){
      BIASERR("dont call this for projective !")
      BIASABORT

    };


    virtual void SetPose(const BIAS::Pose pose) {
      BIASERR("dont call this for projective !")
      BIASABORT
    };


    void ValidatePose(){
      BIASERR("dont call this for projective !")
      BIASABORT
    }

    virtual BIAS::KMatrix GetFakeKMatrix(double& imgsize, int resolution = 0,
                                             const double& maxangle = 1.4) const{
      BIASERR("dont call this for projective !")
      BIASABORT

      BIAS::KMatrix dummy;
      return dummy;

    };

    virtual BIAS::KMatrix GetFakeKMatrix(int resolution = 0,
                                             const double& maxangle = 1.4) const{
      BIASERR("dont call this for projective !")
      BIASABORT

      BIAS::KMatrix dummy;
      return dummy;

    };

    double ViewDifference(const ProjectionParametersBase* ppb) const {

      Vector3<double> C1 = GetC();
      Vector3<double> C2 = ppb->GetC();

      double diff = (C1 - C2).NormL2();

      return diff;
    }


    void NormalizePMatrix(){
      PMatrix P = GetP();
      P.Normalize();
      SetP(P);
    }


    /** @brief return the complete pmatrix (internals+externals / projective)
        used for projection */
    inline virtual PMatrix GetP() const {
      return PMatrix(Pose_.GetMatrix3x4());
    }
    friend std::ostream& operator<<(std::ostream &os,
                                    const ProjectionParametersProjective& p);
  };



  /////////////// inline code ///////////////////////
  inline std::ostream& operator<<(std::ostream &os,
                                  const ProjectionParametersProjective& p)
  {   
    os<<"ProjectionParametersProjective:" << std::endl;
    //todo remove this, is done in base class, output of special projective parameters here
    os << "width " << p.width_ << " height " << p.height_ << std::endl;
    os << p.Pose_ <<std::endl;
    return os;
  }

} // end namespace


#include <Base/Common/BIASpragmaEnd.hh>

#endif