ATANCamera.h
Go to the documentation of this file.
00001 // *-* c++ *-*
00002 // Copyright 2008 Isis Innovation Limited
00003 
00004 // N-th implementation of a camera model
00005 // GK 2007
00006 // Evolved a half dozen times from the CVD-like model I was given by
00007 // TWD in 2000
00008 // 
00009 // This one uses the ``FOV'' distortion model of
00010 // Deverneay and Faugeras, Straight lines have to be straight, 2001
00011 //
00012 // BEWARE: This camera model caches intermediate results in member variables
00013 // Some functions therefore depend on being called in order: i.e.
00014 // GetProjectionDerivs() uses data stored from the last Project() or UnProject()
00015 // THIS MEANS YOU MUST BE CAREFUL WITH MULTIPLE THREADS
00016 // Best bet is to give each thread its own version of the camera!
00017 //
00018 // Camera parameters are stored in a GVar, but changing the gvar has no effect
00019 // until the next call to RefreshParams() or SetImageSize().
00020 //
00021 // Pixel conventions are as follows:
00022 // For Project() and Unproject(),
00023 // round pixel values - i.e. (0.0, 0.0) - refer to pixel centers
00024 // I.e. the top left pixel in the image covers is centered on (0,0)
00025 // and covers the area (-.5, -.5) to (.5, .5)
00026 //
00027 // Be aware that this is not the same as what opengl uses but makes sense
00028 // for acessing pixels using ImageRef, especially ir_rounded.
00029 //
00030 // What is the UFB?
00031 // This is for projecting the visible image area
00032 // to a unit square coordinate system, with the top-left at 0,0,
00033 // and the bottom-right at 1,1
00034 // This is useful for rendering into textures! The top-left pixel is NOT
00035 // centered at 0,0, rather the top-left corner of the top-left pixel is at 
00036 // 0,0!!! This is the way OpenGL thinks of pixel coords.
00037 // There's the Linear and the Distorting version - 
00038 // For the linear version, can use 
00039 // glMatrixMode(GL_PROJECTION); glLoadIdentity();
00040 // glMultMatrix(Camera.MakeUFBLinearFrustumMatrix(near,far));
00041 // To render un-distorted geometry with full frame coverage.
00042 //
00043 
00044 #ifndef __ATAN_CAMERA_H
00045 #define __ATAN_CAMERA_H
00046 
00047 #include <TooN/TooN.h>
00048 #include <cmath>
00049 using namespace TooN;
00050 #include <cvd/vector_image_ref.h>
00051 //#include <gvars3/gvars3.h>
00052 
00053 #include <ros/ros.h>
00054 #include <ros/package.h>
00055 
00056 #define NUMTRACKERCAMPARAMETERS 5
00057 
00058 class CameraCalibrator;
00059 class CalibImage;
00060 
00061 // The parameters are:
00062 // 0 - normalized x focal length
00063 // 1 - normalized y focal length
00064 // 2 - normalized x offset
00065 // 3 - normalized y offset
00066 // 4 - w (distortion parameter)
00067 
00068 class ATANCamera {
00069 public:
00070   ATANCamera(std::string sName);
00071 
00072   // Image size get/set: updates the internal projection params to that target image size.
00073   void SetImageSize(Vector<2> v2ImageSize);
00074   inline void SetImageSize(CVD::ImageRef irImageSize) {SetImageSize(vec(irImageSize));};
00075   inline Vector<2> GetImageSize() {return mvImageSize;};
00076   void RefreshParams();
00077 
00078   // Various projection functions
00079   Vector<2> Project(const Vector<2>& camframe); // Projects from camera z=1 plane to pixel coordinates, with radial distortion
00080   inline Vector<2> Project(CVD::ImageRef ir) { return Project(vec(ir)); }
00081   Vector<2> UnProject(const Vector<2>& imframe); // Inverse operation
00082   inline Vector<2> UnProject(CVD::ImageRef ir)  { return UnProject(vec(ir)); }
00083 
00084   Vector<2> UFBProject(const Vector<2>& camframe);
00085   Vector<2> UFBUnProject(const Vector<2>& camframe);
00086   inline Vector<2> UFBLinearProject(const Vector<2>& camframe);
00087   inline Vector<2> UFBLinearUnProject(const Vector<2>& fbframe);
00088 
00089   Matrix<2,2> GetProjectionDerivs(); // Projection jacobian
00090 
00091   inline bool Invalid() {  return mbInvalid;}
00092   inline double LargestRadiusInImage() {  return mdLargestRadius; }
00093   inline double OnePixelDist() { return mdOnePixelDist; }
00094 
00095   // The z=1 plane bounding box of what the camera can see
00096   inline Vector<2> ImplaneTL(); 
00097   inline Vector<2> ImplaneBR(); 
00098 
00099   // OpenGL helper function
00100   Matrix<4> MakeUFBLinearFrustumMatrix(double near, double far);
00101 
00102   // Feedback for Camera Calibrator
00103   double PixelAspectRatio() { return mvFocal[1] / mvFocal[0];}
00104 
00105 
00106   // Useful for gvar-related reasons (in case some external func tries to read the camera params gvar, and needs some defaults.)
00107   static const Vector<NUMTRACKERCAMPARAMETERS> mvDefaultParams;
00108 
00109   //Weiss{
00110   // GVars3::gvar3<Vector<NUMTRACKERCAMPARAMETERS> > mgvvCameraParams; // The actual camera parameters
00111   Vector<NUMTRACKERCAMPARAMETERS> mgvvCameraParams; // The actual camera parameters
00112   //}
00113 
00114 protected:
00115 
00116 
00117   Matrix<2, NUMTRACKERCAMPARAMETERS> GetCameraParameterDerivs();
00118   void UpdateParams(Vector<NUMTRACKERCAMPARAMETERS> vUpdate);
00119   void DisableRadialDistortion();
00120 
00121   // maximal allowable field of view (for fisheye cams)
00122   double MaxFOV_; // maximal field of view: in radians and half angle
00123 
00124   // Cached from the last project/unproject:
00125   Vector<2> mvLastCam;      // Last z=1 coord
00126   Vector<2> mvLastIm;       // Last image/UFB coord
00127   Vector<2> mvLastDistCam;  // Last distorted z=1 coord
00128   double mdLastR;           // Last z=1 radius
00129   double mdLastDistR;       // Last z=1 distorted radius
00130   double mdLastFactor;      // Last ratio of z=1 radii
00131   bool mbInvalid;           // Was the last projection invalid?
00132 
00133   // Cached from last RefreshParams:
00134   double mdLargestRadius; // Largest R in the image
00135   double mdMaxR;          // Largest R for which we consider projection valid
00136   double mdOnePixelDist;  // z=1 distance covered by a single pixel offset (a rough estimate!)
00137   double md2Tan;          // distortion model coeff
00138   double mdOneOver2Tan;   // distortion model coeff
00139   double mdW;             // distortion model coeff
00140   double mdWinv;          // distortion model coeff
00141   double mdDistortionEnabled; // One or zero depending on if distortion is on or off.
00142   Vector<2> mvCenter;     // Pixel projection center
00143   Vector<2> mvFocal;      // Pixel focal length
00144   Vector<2> mvInvFocal;   // Inverse pixel focal length
00145   Vector<2> mvImageSize;  
00146   Vector<2> mvUFBLinearFocal;
00147   Vector<2> mvUFBLinearInvFocal;
00148   Vector<2> mvUFBLinearCenter;
00149   Vector<2> mvImplaneTL;   
00150   Vector<2> mvImplaneBR;
00151 
00152   // Radial distortion transformation factor: returns ration of distorted / undistorted radius.
00153   inline double rtrans_factor(double r)
00154   {
00155     if(r < 0.001 || mdW == 0.0)
00156       return 1.0;
00157     else 
00158       return (mdWinv* atan(r * md2Tan) / r);
00159   };
00160 
00161   // Inverse radial distortion: returns un-distorted radius from distorted.
00162   inline double invrtrans(double r)
00163   {
00164     if(mdW == 0.0)
00165       return r;
00166         return tan(r * mdW) * mdOneOver2Tan;
00167   };
00168 
00169   std::string msName;
00170 
00171   friend class CameraCalibrator;   // friend declarations allow access to calibration jacobian and camera update function.
00172   friend class CalibImage;
00173 };
00174 
00175 // Some inline projection functions:
00176 inline Vector<2> ATANCamera::UFBLinearProject(const Vector<2>& camframe)
00177 {
00178   Vector<2> v2Res;
00179   v2Res[0] = camframe[0] * mvUFBLinearFocal[0] + mvUFBLinearCenter[0];
00180   v2Res[1] = camframe[1] * mvUFBLinearFocal[1] + mvUFBLinearCenter[1];
00181   return v2Res;
00182 }
00183 
00184 inline Vector<2> ATANCamera::UFBLinearUnProject(const Vector<2>& fbframe)
00185 {
00186   Vector<2> v2Res;
00187   v2Res[0] = (fbframe[0] - mvUFBLinearCenter[0]) * mvUFBLinearInvFocal[0];
00188   v2Res[1] = (fbframe[1] - mvUFBLinearCenter[1]) * mvUFBLinearInvFocal[1];
00189   return v2Res;
00190 }
00191 
00192 
00193 #endif
00194 


ptam
Author(s): Markus Achtelik , Stephan Weiss , Simon Lynen
autogenerated on Sun Oct 5 2014 23:52:33