camera_model.h

Go to the documentation of this file.

//part of the source code of master thesis, source code produced by Jiri Divis
#ifndef VSLAM_CAMERA_MODEL_H_INCLUDED
#define VSLAM_CAMERA_MODEL_H_INCLUDED
#include "utils.h"





namespace vslam {
/*------------------------------------------------------------------------------------------*/
/*------------------------------------------------------------------------------------------*/


  inline Vector2d two3dLinesAngle(const Vector3d vec1, const Vector3d& vec2){
    assert(false); //not implemented
  }

  struct SphericalCoord{
    double azimuth;
    double elevation;
  };

  template<class ImageCoordT, class RayCoordT> class CameraModel{
    public:
      bool canProject(const RayCoordT& ray) const { return true; }
      
      ImageCoordT project(const RayCoordT& ray) const { assert(false); }

      bool canBackProject(const ImageCoordT& im_coord) const { 
        return canProject(BackProject(im_coord));
      }

      RayCoordT backProject(const ImageCoordT& im_coord) const { assert(false); }
  };

  template<class ImageCoordT> class ConcentricCameraModel : 
        public CameraModel<ImageCoordT, Vector3d>{
  };



  class FieldOfViewCCM{
      bool canProject(const Vector3d& ray) const { return true; }
  };


  class SphericalCM : ConcentricCameraModel<Vector3d>{
    protected:
      double hfov;
      double vfov;

    public:
      static const double IGNORE = -1;

      SphericalCM(double _hfov = IGNORE, double _vfov = IGNORE) :
          hfov(_hfov), vfov(_vfov) {}
     

      bool canProject(const Vector3d& ray_coord) const{
        Vector2d hv_angles = two3dLinesAngle(ray_coord, Vector3d::Zero());
        return (hfov < 0 || hv_angles(0) <= hfov) && (vfov < 0 || hv_angles(1) <= vfov);
      }
      
      Vector3d project(const Vector3d& ray_coord) const{
        return ray_coord.normalized();
      }

      bool canBackProject(const Vector3d& im_coord) const{
        return canProject(im_coord);
      }

      Vector3d backProject(const Vector3d& im_coord) const{
        return im_coord;
      }

  };


  class CylindricalCCMSphRays : CameraModel<Vector2d, SphericalCoord>{
    protected:
        int pan_width;
        int pan_height;
    public:
        CylindricalCCMSphRays(int _pan_width, int _pan_height) :
                pan_width(_pan_width), pan_height(_pan_height) {}

        SphericalCoord backProject(Eigen::Vector2d pixel) const{
            SphericalCoord sc;
            sc.azimuth = linspace_idx(
                    (double)0,
                    (double)(pan_width - 1) / (double)pan_width* (-2.0)*M_PI, 
                    pan_width,
                    pixel(0) - (double)(((int)pan_width) / 2)); 
            sc.elevation = -linspace_idx(-M_PI / 2.0, M_PI / 2.0, pan_height, pixel(1));
            return sc;
        }

        Vector2d project(SphericalCoord sc) const{
            Eigen::Vector2d t;
            t(0) = inv_linspace_idx(
                    (double)0, 
                    (double)(pan_width - 1) / (double)pan_width * (-2.0)*M_PI,
                    pan_width, 
                    sc.azimuth) + (double)(((int)pan_width) / 2);
            t(1) = inv_linspace_idx(-M_PI / 2.0, M_PI / 2.0, pan_height, -sc.elevation);
            return t;
        }
  };


  class CylindricalCCMCartRays : CameraModel<Vector2d, Vector3d>{
    private:
      int pan_width;
      int pan_height;
      CylindricalCCMSphRays sccm;

    public:
      CylindricalCCMCartRays(int _w, int _h) : 
        pan_width(_w), pan_height(_h), sccm(pan_width, pan_height) {}


      Vector2d project(const Vector3d& dir_vec) const{
          SphericalCoord sc;

          Vector3d dir_vec2 = dir_vec.normalized();
          double x = dir_vec2(0);
          double y = dir_vec2(1);
          double z = dir_vec2(2);

          double r_xz = std::sqrt(x*x + z*z);
          sc.azimuth = std::atan2(-x, z);
          sc.elevation = std::atan2(y, r_xz);
          return sccm.project(sc);
      }

      Vector3d backProject(Vector2d& im_coord) const{
          SphericalCoord sc = sccm.backProject(im_coord);
          Eigen::Vector3d direction;
          direction(0) = -std::sin( sc.azimuth ) * std::cos( sc.elevation );
          direction(1) = std::sin( sc.elevation );
          direction(2) = std::cos( sc.azimuth ) * std::cos( sc.elevation );
          return direction;
      }
  };


  template<class CameraModelRayIntermediate, class CameraModelIntermediateImage>
  projectionCoordinateAdaptorCM : public CameraModel<NewImageCoordT, CameraModelT::RayCoordT>{
      CameraModelT cm_ray_to_intermed;
      CameraModelT cm_intermed_to_image;
      project(const CameraModelT::RayCoordT ray_coord){
          return cm_intermed_to_image(cm_ray_to_intermed(ray_coord));
      }

      backProject(const CameraModelT::

  }


  template<class T> struct MCRCoord{
      int id;
      T coord;

      MCRCoord<T>(int _id, T _coord) : id(_id), coord(_coord) {}
  };


  
  template<class CCMT> class MultiCameraRigFromCCM : 
      CameraModel<MCRCoord< typename CCMT::ImageCoordT>, 
                  MCRCoord< typename CCMT::RayCoordT> >{
    private:
      vector<CCMT> cameras;
      vector<SE3> camera_transforms;

    public:

      bool canProject(const Vector3d& point){
        for(size_t i=0; i<cameras.size(); i++){
          if(cameras[i].canProject(point)){
            return true;
          }
        }
        return false;
      }

      MCRCoord<typename CCMT::ImageCoordT> project(Vector3d point){
        for(size_t i=0; i<cameras.size(); i++){
          if(cameras[i].canProject(point)){
            return MCRCoord<typename CCMT::ImageCoordT>(
              i, makepair(cameras[i].getProjectionCenter(), cameras[i].project(point)));
          }
        }
        assert(false);
      }
  };

  template<class CCMT> class MultiCameraRigFromCCM : 
      CameraModel<MCRCoord< typename CCMT::ImageCoordT>, 
      MCRCoord< typename CCMT::RayCoordT> >{
          private:
              vector<CCMT> cameras;
              vector<SE3> camera_transforms;
              sph_approx_backprojection = false;

          public:
              bool canProject(const Vector3d& point){
                  MCRCoord<typename CCMT::ImageCoordT> coord = project(point);
                  return coord != INVALID_PROJECTION;
              }

              MCRCoord<typename CCMT::ImageCoordT> project(Vector3d point){
                  for(size_t i=0; i<cameras.size(); i++){
                      if(cameras[i].canProject(point)){
                          Vector3d bproji = vector3dAngle(cameras[i].backProjectionFromPoint(point));
                          if(camIdWithMinOpticalAxisAngleToVector3d(bproji)==i){
                              return cameras[i].project(point);
                          }
                      }
                  }
                  return INVALID_PROJECTION;
              }

              bool canBackProject(){
                  MCRCoord<typename CCMT::ImageCoordT> coord = backProject(point);
                  return coord != MCRCoord<typename CCMT::ImageCoordT>::INVALID_COORD_IDX;
              }

              backProject(){ //XXX i uz znam!!!
                  for(size_t i=0; i<cameras.size(); i++){
                      if(cameras[i].canBackProject(point)){
                          Vector3d bproji = vector3dAngle(cameras[i].backProject(point));
                          if(camIdWithMinOpticalAxisAngleToVector3d(bproji)==i){
                              if(sph_approx_backprojection){
                                  return bproji;
                              } else{
                                  return MCRCoord<typename CCMT::ImageCoordT>(
                                          i, makepair(cameras[i].getProjectionCenter(), bproji));
                              }
                          }
                      }
                  }
                  return MCRCoord<typename CCMT::ImageCoordT>(
                          MCRCoord<typename CCMT::ImageCoordT>::INVALID_COORD_IDX);
              }


      };



  //XXX sph approximation in projection function
  //XXX sph approximation in backprojection function
  //XXX change of imaging surface
  //XXX change of ray space representation


}


#endif