cameraHandler.hpp

/* $Id$ */

#ifndef _CAMERA_HANDLER_HPP
#define _CAMERA_HANDLER_HPP

#include "kernel/jafarException.hpp"
#include "jmath/jblas.hpp"
#include "camera/cameraException.hpp"
#include "camera/cameraPinhole.hpp"
#include "camera/cameraBarreto.hpp"

namespace jafar {
  namespace camera {
    class AbstractCameraHandler {
      
      public:
      AbstractCameraHandler(){};
      virtual ~AbstractCameraHandler(){};
      virtual void setParameters(const jblas::vec& params) = 0;
      virtual void applyScale(double scale) = 0;
      virtual bool project(const jblas::vec3& v, 
                           jblas::vec2& pix) = 0;
      virtual void projectJac(const jblas::vec3& v, 
                              jblas::mat& J) = 0;
      virtual void projectJac(const jblas::vec3& v, 
                              jblas::vec2& pix,
                              jblas::mat& J) = 0;
      virtual void imageToCameraFrame(const jblas::vec2& pixel, 
                                      jblas::vec3& position) = 0;
      virtual void imageToCameraFrameDirection(const jblas::vec2& pixel, 
                                               jblas::vec3& direction) = 0;
      virtual void imageToCameraFrameDirectionJac(const jblas::vec2& pixel,
                                                  jblas::mat& J) = 0;
      virtual void imageToCameraFrameDirectionJac(const jblas::vec2& pixel,
                                                  jblas::vec3& direction,
                                                  jblas::mat& J) = 0;
      virtual void load(const std::string& configFile) = 0;
      virtual void intrinsics(jblas::mat33& intrinsic) = 0;
    }; // class AbstractCameraHandler
    
    template <class CAMERA = camera::CameraPinhole>
      class CameraHandler : public AbstractCameraHandler {
      
      protected:
      CAMERA m_camera;

      public:
      CameraHandler() : AbstractCameraHandler() {};
      CameraHandler(const CAMERA& _camera){
        m_camera = _camera;
      };

      virtual ~CameraHandler(){};

      virtual void setParameters(const jblas::vec& params) {};

      void applyScale(double scale){
        m_camera.applyScale(scale);
      }

      virtual bool project(const jblas::vec3& v, 
                           jblas::vec2& pix) {return false;};

      virtual void projectJac(const jblas::vec3& v, 
                              jblas::mat& J) {};

      virtual void projectJac(const jblas::vec3& v, 
                              jblas::vec2& pix,
                              jblas::mat& J) {};

      virtual void imageToCameraFrame(const jblas::vec2& pixel, 
                                      jblas::vec3& position) {};

      virtual void imageToCameraFrameDirection(const jblas::vec2& pixel, 
                                               jblas::vec3& direction) {};

      virtual void imageToCameraFrameDirectionJac(const jblas::vec2& pixel,
                                          jblas::mat& J) {};

      virtual void imageToCameraFrameDirectionJac(const jblas::vec2& pixel,
                                          jblas::vec3& direction,
                                          jblas::mat& J) {};

      void load(const std::string& configFile){
        m_camera.load(configFile);
      };

      virtual void intrinsics(jblas::mat33& intrinsic) {};

    }; // class CameraHandler

    class PinholeCameraHandler : public CameraHandler<camera::CameraPinhole> {
      public:

      PinholeCameraHandler();

      PinholeCameraHandler(const camera::CameraPinhole& _camera);

      ~PinholeCameraHandler();

      void setParameters(const jblas::vec& params);

      void imageToCameraFrame(const jblas::vec2& pixel, 
                              jblas::vec3& position);

      void imageToCameraFrameJac(const jblas::vec2& pixel,
                                 jblas::mat& J);

      void imageToCameraFrameJac(const jblas::vec2& pixel,
                                 jblas::vec3& position,
                                 jblas::mat& J);

      bool project(const jblas::vec3& v, 
                   jblas::vec2& pix);

      void projectJac(const jblas::vec3& v, 
                      jblas::mat& J);

      void projectJac(const jblas::vec3& v, 
                      jblas::vec2& pix,
                      jblas::mat& J);

      void imageToCameraFrameDirection(const jblas::vec2& pixel,
                                       jblas::vec3& direction);

      void imageToCameraFrameDirectionJac(const jblas::vec2& pixel,
                                          jblas::mat& J);

      void imageToCameraFrameDirectionJac(const jblas::vec2& pixel,
                                          jblas::vec3& direction,
                                          jblas::mat& J);

      void intrinsics(jblas::mat33& intrinsic);
    }; // class PinholeCameraHandler

    class StereoBenchHandler : public CameraHandler<camera::StereoBench> {
      public:
      StereoBenchHandler();

      StereoBenchHandler(const camera::StereoBench& _camera);

      ~StereoBenchHandler();

      void setParameters(const jblas::vec& params);

      void imageToCameraFrame(const jblas::vec2& pixel,
                              const double& disparity,
                              jblas::vec3& position);

      void imageToCameraFrameJac(const jblas::vec2& pixel,
                                 const double& disparity,
                                 jblas::mat& J);

      void imageToCameraFrameJac(const jblas::vec2& pixel,
                                 const double& disparity,
                                 jblas::vec3& position,
                                 jblas::mat& J);

      bool project(const jblas::vec3& v,
                   jblas::vec2& pix);      

      void projectJac(const jblas::vec3& v, 
                      jblas::mat& J);      

      void projectJac(const jblas::vec3& v, 
                      jblas::vec2& pix,
                      jblas::mat& J);

      void imageToCameraFrameDirection(const jblas::vec2& pixel, 
                                       jblas::vec3& direction);

      void imageToCameraFrameDirectionJac(const jblas::vec2& pixel,
                                          jblas::mat& J);

      void imageToCameraFrameDirectionJac(const jblas::vec2& pixel,
                                          jblas::vec3& direction,
                                          jblas::mat& J);

      void intrinsics(jblas::mat33& intrinsic);
    }; //class StereoBenchHandler

    class ParabolicBarretoCameraHandler : public CameraHandler<camera::CameraParabolicBarreto> {
      public:
      ParabolicBarretoCameraHandler();

      ParabolicBarretoCameraHandler(const camera::CameraParabolicBarreto& _camera);

      ~ParabolicBarretoCameraHandler();

      void setParameters(const jblas::vec& params);

      void imageToCameraFrame(const jblas::vec2& pixel, 
                              jblas::vec3& position);
      bool project(const jblas::vec3& v, 
                   jblas::vec2& pix);

      void projectJac(const jblas::vec3& v, 
                      jblas::mat& J);

      void projectJac(const jblas::vec3& v, 
                      jblas::vec2& pix,
                      jblas::mat& J);

      void imageToCameraFrameDirection(const jblas::vec2& pixel, 
                                       jblas::vec3& direction);
      
      void imageToCameraFrameDirectionJac(const jblas::vec2& pixel,
                                          jblas::mat& J);

      void imageToCameraFrameDirectionJac(const jblas::vec2& pixel,
                                          jblas::vec3& direction,
                                          jblas::mat& J);

      void intrinsics(jblas::mat33& intrinsic);
    }; // class ParabolicBarretoCameraHandler

  } // camera

} // jafar

#endif