Camera.h

Go to the documentation of this file.

/********************************************************************
**                Image Component Library (ICL)                    **
**                                                                 **
** Copyright (C) 2006-2013 CITEC, University of Bielefeld          **
**                         Neuroinformatics Group                  **
** Website: www.iclcv.org and                                      **
**          http://opensource.cit-ec.de/projects/icl               **
**                                                                 **
** File   : ICLGeom/src/ICLGeom/Camera.h                           **
** Module : ICLGeom                                                **
** Authors: Erik Weitnauer, Christof Elbrechter                    **
**                                                                 **
**                                                                 **
** GNU LESSER GENERAL PUBLIC LICENSE                               **
** This file may be used under the terms of the GNU Lesser General **
** Public License version 3.0 as published by the                  **
**                                                                 **
** Free Software Foundation and appearing in the file LICENSE.LGPL **
** included in the packaging of this file.  Please review the      **
** following information to ensure the license requirements will   **
** be met: http://www.gnu.org/licenses/lgpl-3.0.txt                **
**                                                                 **
** The development of this software was supported by the           **
** Excellence Cluster EXC 277 Cognitive Interaction Technology.    **
** The Excellence Cluster EXC 277 is a grant of the Deutsche       **
** Forschungsgemeinschaft (DFG) in the context of the German       **
** Excellence Initiative.                                          **
**                                                                 **
********************************************************************/

#pragma once

#include <ICLUtils/CompatMacros.h>
#include <ICLUtils/Size.h>
#include <ICLUtils/Point32f.h>
#include <ICLUtils/Rect.h>
#include <ICLUtils/Rect32f.h>
#include <ICLUtils/Exception.h>
#include <ICLUtils/Array2D.h>
#include <ICLGeom/PlaneEquation.h>
#include <ICLGeom/ViewRay.h>


// the icl namespace
namespace icl {
  namespace geom{

    class ICLGeom_API Camera {
      public:

      struct ICLGeom_API RenderParams {
        utils::Size chipSize;    
        float clipZNear;  
        float clipZFar;   
        utils::Rect viewport;    
        float viewportZMin, viewportZMax; 

        RenderParams(const utils::Size &chipSize=utils::Size::VGA, float clipZNear=1.0, float clipZFar=10000.,
          const utils::Rect &viewport=utils::Rect(0,0,640,480), float viewportZMin=0.,
          float viewportZMax=1.):
          chipSize(chipSize), clipZNear(clipZNear), clipZFar(clipZFar), viewport(viewport),
          viewportZMin(viewportZMin), viewportZMax(viewportZMax) {}
      };

      struct NotEnoughDataPointsException  : public utils::ICLException{
        NotEnoughDataPointsException():utils::ICLException(__FUNCTION__){}
      };

      typedef math::FixedMatrix<icl32f,3,3> Mat3x3;

      Camera(const Vec &pos=Vec(0,0,10,1),
               const Vec &norm=Vec(0,0,-1,1),
               const Vec &up=Vec(1,0,0,1),
               float f=3,
               const utils::Point32f &principalPointOffset=utils::Point32f(320,240),
               float sampling_res_x = 200,
               float sampling_res_y = 200,
               float skew = 0,
               const RenderParams &renderParams = RenderParams()): m_pos(pos), m_norm(norm), m_up(up), m_f(f),
             m_px(principalPointOffset.x), m_py(principalPointOffset.y),
             m_mx(sampling_res_x), m_my(sampling_res_y), m_skew(skew), m_renderParams(renderParams) {}


      Camera(const std::string &filename, const std::string &prefix="config.")  ;

      Camera(std::istream &configDataStream, const std::string &prefix="config.")  ;


      static Camera createFromProjectionMatrix(const math::FixedMatrix<icl32f,4,3> &Q, float focalLength=1);


      static Camera calibrate(std::vector<Vec> Xws, std::vector<utils::Point32f> xis, float focalLength=1,
                              bool performLMAOptimization=true)
        ;


      static Camera calibrate_pinv(std::vector<Vec> Xws, std::vector<utils::Point32f> xis, float focalLength=1,
                                   bool performLMAOptimization=true)
        ;


      static Camera calibrate_extrinsic(const std::vector<Vec> &Xws, const std::vector<utils::Point32f> &xis,
                                        const Camera &intrinsicCamValue, const RenderParams &renderParams=RenderParams(),
                                        bool performLMAOptimization=true)
      ;


      static Camera calibrate_extrinsic(const std::vector<Vec> &Xws, const std::vector<utils::Point32f> &xis,
                                        const Mat &camIntrinsicProjectionMatrix, const RenderParams &renderParams=RenderParams(),
                                        bool performLMAOptimization=true)
      ;


      static Camera calibrate_extrinsic(std::vector<Vec> Xws, std::vector<utils::Point32f> xis,
                                        float fx, float fy, float s, float px ,float py,
                                        const RenderParams &renderParams=RenderParams(),
                                        bool performLMAOptimization=true)
      ;

      static Camera optimize_camera_calibration_lma(const std::vector<Vec> &Xws,
                                                    const std::vector<utils::Point32f> xis,
                                                    const Camera &init);

      // projections normal
      utils::Point32f project(const Vec &Xw) const;
      void project(const std::vector<Vec> &Xws, std::vector<utils::Point32f> &dst) const;
      const std::vector<utils::Point32f> project(const std::vector<Vec> &Xws) const;



      // projections OpenGL
      Vec projectGL(const Vec &Xw) const;
      void projectGL(const std::vector<Vec> &Xws, std::vector<Vec> &dst) const;
      const std::vector<Vec> projectGL(const std::vector<Vec> &Xws) const;


      // projection magic
      ViewRay getViewRay(const utils::Point32f &pixel) const;


      std::vector<ViewRay> getViewRays(const std::vector<utils::Point32f> &pixels) const;


      utils::Array2D<ViewRay> getAllViewRays() const;

      ViewRay getViewRay(const Vec &Xw) const;

      Vec estimate3DPosition(const utils::Point32f &pixel, const PlaneEquation &plane) const ;

      static Vec getIntersection(const ViewRay &v,
                                 const PlaneEquation &plane) ;

      // setters and getters
      void setRotation(const Mat3x3 &rot);
      void setRotation(const Vec &rot);


      void setTransformation(const Mat &m);


      void setWorldTransformation(const Mat &m);



      void setWorldFrame(const Mat &m);

      Mat getCSTransformationMatrix() const;

      Mat getInvCSTransformationMatrix() const;

      Mat getCSTransformationMatrixGL() const;
      Mat getProjectionMatrix() const;
      Mat getProjectionMatrixGL() const;
      Mat getViewportMatrixGL() const;

      math::FixedMatrix<icl32f,4,3> getQMatrix() const;

      math::FixedMatrix<icl32f,3,4> getInvQMatrix() const;

      inline void translate(const Vec &d) { m_pos += d; }

      std::string toString() const;

      const std::string &getName() const { return m_name; }
      const Vec &getPosition() const { return m_pos; }
      const Vec &getNorm() const { return m_norm; }
      const Vec &getUp() const { return m_up; }
      Vec getHoriz() const { return cross(m_up, m_norm); }
      float getFocalLength() const { return m_f; }
      utils::Point32f getPrincipalPointOffset() const { return utils::Point32f(m_px, m_py); }
      float getPrincipalPointOffsetX() const { return m_px; }
      float getPrincipalPointOffsetY() const { return m_py; }
      float getSamplingResolutionX() const { return m_mx; }
      float getSamplingResolutionY() const { return m_my; }
      float getSkew() const { return m_skew; }
      const RenderParams &getRenderParams() const { return m_renderParams; }
      RenderParams &getRenderParams() { return m_renderParams; }

      void setName(const std::string &name) { m_name = name; }
      void setPosition(const Vec &pos) { m_pos = pos; }
      void setNorm(const Vec &norm, bool autoOrthogonalizeRotationMatrix=false) {
        m_norm = norm; m_norm[3] = 0; m_norm.normalize(); m_norm[3] = 1;
        if(autoOrthogonalizeRotationMatrix) orthogonalizeRotationMatrix();
      }
      void setUp(const Vec &up, bool autoOrthogonalizeRotationMatrix=false) {
        m_up = up; m_up[3] = 0; m_up.normalize(); m_up[3] = 1;
        if(autoOrthogonalizeRotationMatrix) orthogonalizeRotationMatrix();
      }
      void orthogonalizeRotationMatrix();

      void setFocalLength(float value) { m_f = value; }
      void setPrincipalPointOffset(float px, float py) { m_px = px; m_py = py; }
      void setPrincipalPointOffset(const utils::Point32f &p) { m_px = p.x; m_py = p.y; }
      void setSamplingResolutionX(float value) { m_mx = value; }
      void setSamplingResolutionY(float value) { m_my = value; }
      void setSamplingResolution(float x, float y) { m_mx = x; m_my = y; }
      void setSkew(float value) { m_skew = value; }
      void setRenderParams(const RenderParams &rp) { m_renderParams = rp; }



      void setResolution(const utils::Size &newScreenSize);


      void setResolution(const utils::Size &newScreenSize, const utils::Point &newPrincipalPointOffset);

      inline const utils::Size &getResolution() const { return m_renderParams.chipSize; }

      static Vec estimate_3D(const std::vector<Camera*> cams,
                             const std::vector<utils::Point32f> &UVs,
                             bool removeInvalidPoints=false) ;

      Mat estimatePose(const std::vector<Vec> &objectCoords,
                       const std::vector<utils::Point32f> &imageCoords,
                       bool performLMAOptimization=true);


      static Vec estimate_3D_svd(const std::vector<Camera*> cams,
                                 const std::vector<utils::Point32f> &UVs);


      static Camera create_camera_from_calibration_or_udist_file(const std::string &filename) ;

      protected:
      static Mat createTransformationMatrix(const Vec &norm, const Vec &up, const Vec &pos);

      private:
      // General Parameters
      std::string m_name;   

      // External Parameters
      Vec m_pos;        
      Vec m_norm;       
      Vec m_up;         

      // Internal Parameters
      float m_f;        
      float m_px, m_py; 
      float m_mx, m_my; 
      float m_skew;     

      // Rendering Parameters
      RenderParams m_renderParams;


      static void checkAndFixPoints(std::vector<Vec> &worldPoints, std::vector<utils::Point32f> &imagePoints)
        ;
      static void load_camera_from_stream(std::istream &is,
                                          const std::string &prefix,
                                          Camera &cam);
    };

    ICLGeom_API std::ostream &operator<<(std::ostream &os, const Camera &cam);

    ICLGeom_API std::istream &operator>>(std::istream &is, Camera &cam) ;

  } // namespace geom
} // namespace icl