ATANCamera.cc
Go to the documentation of this file.
00001 // Copyright 2008 Isis Innovation Limited
00002 #include "ptam/ATANCamera.h"
00003 #include <TooN/helpers.h>
00004 #include <cvd/vector_image_ref.h>
00005 #include <iostream>
00006 #include <ptam/Params.h>
00007 //#include <gvars3/instances.h>
00008 using namespace std;
00009 using namespace CVD;
00010 //using namespace GVars3;
00011 
00012 ATANCamera::ATANCamera(string sName)
00013 {
00014   // The camera name is used to find the camera's parameters in a GVar.
00015   msName = sName;
00016   //Weiss{
00017   //mgvvCameraParams = mvDefaultParams;
00018   const FixParams& pPars = PtamParameters::fixparams();
00019   mgvvCameraParams = makeVector(pPars.Cam_fx, pPars.Cam_fy, pPars.Cam_cx, pPars.Cam_cy, pPars.Cam_s);
00020   MaxFOV_ = (pPars.MaxFoV/2)/180*M_PI;
00021   //GV2.Register(mgvvCameraParams, sName+".Parameters", mvDefaultParams, HIDDEN | FATAL_IF_NOT_DEFINED);
00022   //mvImageSize[0] = 640.0;
00023   //mvImageSize[1] = 480.0;
00024   mvImageSize[0] =pPars.ImageSizeX;
00025   mvImageSize[1] = pPars.ImageSizeY;
00026   //}
00027   RefreshParams();
00028 }
00029 
00030 void ATANCamera::SetImageSize(Vector<2> vImageSize)
00031 {
00032   mvImageSize = vImageSize;
00033   RefreshParams();
00034 };
00035 
00036 void ATANCamera::RefreshParams() 
00037 {
00038   // This updates internal member variables according to the current camera parameters,
00039   // and the currently selected target image size.
00040   //
00041 
00042   // First: Focal length and image center in pixel coordinates
00043   mvFocal[0] = mvImageSize[0] * mgvvCameraParams[0];
00044   mvFocal[1] = mvImageSize[1] * mgvvCameraParams[1];
00045   mvCenter[0] = mvImageSize[0] * mgvvCameraParams[2] - 0.5;
00046   mvCenter[1] = mvImageSize[1] * mgvvCameraParams[3] - 0.5;
00047 
00048   // One over focal length
00049   mvInvFocal[0] = 1.0 / mvFocal[0];
00050   mvInvFocal[1] = 1.0 / mvFocal[1];
00051 
00052   // Some radial distortion parameters..
00053   mdW =  mgvvCameraParams[4];
00054   if(mdW != 0.0)
00055   {
00056     md2Tan = 2.0 * tan(mdW / 2.0);
00057     mdOneOver2Tan = 1.0 / md2Tan;
00058     mdWinv = 1.0 / mdW;
00059     mdDistortionEnabled = 1.0;
00060   }
00061   else
00062   {
00063     mdWinv = 0.0;
00064     md2Tan = 0.0;
00065     mdDistortionEnabled = 0.0;
00066   }
00067 
00068   // work out biggest radius in image
00069   Vector<2> v2;
00070   v2[0]= max(mgvvCameraParams[2], 1.0 - mgvvCameraParams[2]) / mgvvCameraParams[0];
00071   v2[1]= max(mgvvCameraParams[3], 1.0 - mgvvCameraParams[3]) / mgvvCameraParams[1];
00072   
00073   double rbuff=sqrt(v2*v2);
00074   if((rbuff*mdW)>MaxFOV_)
00075           mdLargestRadius =  tan(MaxFOV_) * mdOneOver2Tan;
00076   else
00077           mdLargestRadius= tan(rbuff * mdW) * mdOneOver2Tan;
00078 
00079   // At what stage does the model become invalid?
00080   mdMaxR = 1.5 * mdLargestRadius; // (pretty arbitrary)
00081 
00082   // work out world radius of one pixel
00083   // (This only really makes sense for square-ish pixels)
00084   {
00085     Vector<2> v2Center = UnProject(mvImageSize / 2);
00086     Vector<2> v2RootTwoAway = UnProject(mvImageSize / 2 + vec(ImageRef(1,1)));
00087     Vector<2> v2Diff = v2Center - v2RootTwoAway;
00088     mdOnePixelDist = sqrt(v2Diff * v2Diff) / sqrt(2.0);
00089   }
00090 
00091   // Work out the linear projection values for the UFB
00092   {
00093     // First: Find out how big the linear bounding rectangle must be
00094     vector<Vector<2> > vv2Verts;
00095     vv2Verts.push_back(UnProject(makeVector( -0.5, -0.5)));
00096     vv2Verts.push_back(UnProject(makeVector( mvImageSize[0]-0.5, -0.5)));
00097     vv2Verts.push_back(UnProject(makeVector( mvImageSize[0]-0.5, mvImageSize[1]-0.5)));
00098     vv2Verts.push_back(UnProject(makeVector( -0.5, mvImageSize[1]-0.5)));
00099     Vector<2> v2Min = vv2Verts[0];
00100     Vector<2> v2Max = vv2Verts[0];
00101     for(int i=0; i<4; i++)
00102       for(int j=0; j<2; j++)
00103       {
00104         if(vv2Verts[i][j] < v2Min[j]) v2Min[j] = vv2Verts[i][j];
00105         if(vv2Verts[i][j] > v2Max[j]) v2Max[j] = vv2Verts[i][j];
00106       }
00107     mvImplaneTL = v2Min;
00108     mvImplaneBR = v2Max;
00109 
00110     // Store projection parameters to fill this bounding box
00111     Vector<2> v2Range = v2Max - v2Min;
00112     mvUFBLinearInvFocal = v2Range;
00113     mvUFBLinearFocal[0] = 1.0 / mvUFBLinearInvFocal[0];
00114     mvUFBLinearFocal[1] = 1.0 / mvUFBLinearInvFocal[1];
00115     mvUFBLinearCenter[0] = -1.0 * v2Min[0] * mvUFBLinearFocal[0];
00116     mvUFBLinearCenter[1] = -1.0 * v2Min[1] * mvUFBLinearFocal[1];
00117   }
00118 
00119 }
00120 
00121 // Project from the camera z=1 plane to image pixels,
00122 // while storing intermediate calculation results in member variables
00123 Vector<2> ATANCamera::Project(const Vector<2>& vCam){
00124   mvLastCam = vCam;
00125   mdLastR = sqrt(vCam * vCam);
00126   mbInvalid = (mdLastR > mdMaxR);
00127   mdLastFactor = rtrans_factor(mdLastR);
00128   mdLastDistR = mdLastFactor * mdLastR;
00129   mvLastDistCam = mdLastFactor * mvLastCam;
00130 
00131   mvLastIm[0] = mvCenter[0] + mvFocal[0] * mvLastDistCam[0];
00132   mvLastIm[1] = mvCenter[1] + mvFocal[1] * mvLastDistCam[1];
00133 
00134   return mvLastIm;
00135 }
00136 
00137 // Un-project from image pixel coords to the camera z=1 plane
00138 // while storing intermediate calculation results in member variables
00139 Vector<2> ATANCamera::UnProject(const Vector<2>& v2Im)
00140 {
00141   mvLastIm = v2Im;
00142   mvLastDistCam[0] = (mvLastIm[0] - mvCenter[0]) * mvInvFocal[0];
00143   mvLastDistCam[1] = (mvLastIm[1] - mvCenter[1]) * mvInvFocal[1];
00144   mdLastDistR = sqrt(mvLastDistCam * mvLastDistCam);
00145   mdLastR = invrtrans(mdLastDistR);
00146   double dFactor;
00147   if(mdLastDistR > 0.01)
00148     dFactor =  mdLastR / mdLastDistR;
00149   else
00150     dFactor = 1.0;
00151   mdLastFactor = 1.0 / dFactor;
00152   mvLastCam = dFactor * mvLastDistCam;
00153   return mvLastCam;
00154 }
00155 
00156 // Utility function for easy drawing with OpenGL
00157 // C.f. comment in top of ATANCamera.h
00158 Matrix<4> ATANCamera::MakeUFBLinearFrustumMatrix(double near, double far)
00159 {
00160   Matrix<4> m4 = Zeros;
00161 
00162 
00163   double left = mvImplaneTL[0] * near;
00164   double right = mvImplaneBR[0] * near;
00165   double top = mvImplaneTL[1] * near;
00166   double bottom = mvImplaneBR[1] * near;
00167 
00168   // The openGhelL frustum manpage is A PACK OF LIES!!
00169   // Two of the elements are NOT what the manpage says they should be.
00170   // Anyway, below code makes a frustum projection matrix
00171   // Which projects a RHS-coord frame with +z in front of the camera
00172   // Which is what I usually want, instead of glFrustum's LHS, -z idea.
00173   m4[0][0] = (2 * near) / (right - left);
00174   m4[1][1] = (2 * near) / (top - bottom);
00175 
00176   m4[0][2] = (right + left) / (left - right);
00177   m4[1][2] = (top + bottom) / (bottom - top);
00178   m4[2][2] = (far + near) / (far - near);
00179   m4[3][2] = 1;
00180 
00181   m4[2][3] = 2*near*far / (near - far);
00182 
00183   return m4;
00184 };
00185 
00186 Matrix<2,2> ATANCamera::GetProjectionDerivs()
00187 {
00188   // get the derivative of image frame wrt camera z=1 frame at the last computed projection
00189   // in the form (d im1/d cam1, d im1/d cam2)
00190   //             (d im2/d cam1, d im2/d cam2)
00191 
00192   double dFracBydx;
00193   double dFracBydy;
00194 
00195   double &k = md2Tan;
00196   double &x = mvLastCam[0];
00197   double &y = mvLastCam[1];
00198   double r = mdLastR * mdDistortionEnabled;
00199 
00200   if(r < 0.01)
00201   {
00202     dFracBydx = 0.0;
00203     dFracBydy = 0.0;
00204   }
00205   else
00206   {
00207     dFracBydx =
00208         mdWinv * (k * x) / (r*r*(1 + k*k*r*r)) - x * mdLastFactor / (r*r);
00209     dFracBydy =
00210         mdWinv * (k * y) / (r*r*(1 + k*k*r*r)) - y * mdLastFactor / (r*r);
00211   }
00212 
00213   Matrix<2> m2Derivs;
00214 
00215   m2Derivs[0][0] = mvFocal[0] * (dFracBydx * x + mdLastFactor);  
00216   m2Derivs[1][0] = mvFocal[1] * (dFracBydx * y);  
00217   m2Derivs[0][1] = mvFocal[0] * (dFracBydy * x);  
00218   m2Derivs[1][1] = mvFocal[1] * (dFracBydy * y + mdLastFactor);  
00219   return m2Derivs;
00220 }
00221 
00222 Matrix<2,NUMTRACKERCAMPARAMETERS> ATANCamera::GetCameraParameterDerivs()
00223 {
00224   // Differentials wrt to the camera parameters
00225   // Use these to calibrate the camera
00226   // No need for this to be quick, so do them numerically
00227 
00228   Matrix<2, NUMTRACKERCAMPARAMETERS> m2NNumDerivs;
00229   Vector<NUMTRACKERCAMPARAMETERS> vNNormal = mgvvCameraParams;
00230   Vector<2> v2Cam = mvLastCam;
00231   Vector<2> v2Out = Project(v2Cam);
00232   for(int i=0; i<NUMTRACKERCAMPARAMETERS; i++)
00233   {
00234     if(i == NUMTRACKERCAMPARAMETERS-1 && mdW == 0.0)
00235       continue;
00236     Vector<NUMTRACKERCAMPARAMETERS> vNUpdate;
00237     vNUpdate = Zeros;
00238     vNUpdate[i] += 0.001;
00239     UpdateParams(vNUpdate);
00240     Vector<2> v2Out_B = Project(v2Cam);
00241     m2NNumDerivs.T()[i] = (v2Out_B - v2Out) / 0.001;
00242     mgvvCameraParams = vNNormal;
00243     RefreshParams();
00244   }
00245   if(mdW == 0.0)
00246     m2NNumDerivs.T()[NUMTRACKERCAMPARAMETERS-1] = Zeros;
00247   return m2NNumDerivs;
00248 }
00249 
00250 void ATANCamera::UpdateParams(Vector<5> vUpdate)
00251 {
00252   // Update the camera parameters; use this as part of camera calibration.
00253   mgvvCameraParams = mgvvCameraParams + vUpdate;
00254   RefreshParams();
00255 }
00256 
00257 void ATANCamera::DisableRadialDistortion()
00258 {
00259   // Set the radial distortion parameter to zero
00260   // This disables radial distortion and also disables its differentials
00261   mgvvCameraParams[NUMTRACKERCAMPARAMETERS-1] = 0.0;
00262   RefreshParams();
00263 }
00264 
00265 Vector<2> ATANCamera::UFBProject(const Vector<2>& vCam)
00266 {
00267   // Project from camera z=1 plane to UFB, storing intermediate calc results.
00268   mvLastCam = vCam;
00269   mdLastR = sqrt(vCam * vCam);
00270   mbInvalid = (mdLastR > mdMaxR);
00271   mdLastFactor = rtrans_factor(mdLastR);
00272   mdLastDistR = mdLastFactor * mdLastR;
00273   mvLastDistCam = mdLastFactor * mvLastCam;
00274 
00275   mvLastIm[0] = mgvvCameraParams[2]  + mgvvCameraParams[0] * mvLastDistCam[0];
00276   mvLastIm[1] = mgvvCameraParams[3]  + mgvvCameraParams[1] * mvLastDistCam[1];
00277   return mvLastIm;
00278 }
00279 
00280 Vector<2> ATANCamera::UFBUnProject(const Vector<2>& v2Im)
00281 {
00282   mvLastIm = v2Im;
00283   mvLastDistCam[0] = (mvLastIm[0] - mgvvCameraParams[2]) / mgvvCameraParams[0];
00284   mvLastDistCam[1] = (mvLastIm[1] - mgvvCameraParams[3]) / mgvvCameraParams[1];
00285   mdLastDistR = sqrt(mvLastDistCam * mvLastDistCam);
00286   mdLastR = invrtrans(mdLastDistR);
00287   double dFactor;
00288   if(mdLastDistR > 0.01)
00289     dFactor =  mdLastR / mdLastDistR;
00290   else
00291     dFactor = 1.0;
00292   mdLastFactor = 1.0 / dFactor;
00293   mvLastCam = dFactor * mvLastDistCam;
00294   return mvLastCam;
00295 }
00296 //default 150 firefly
00297 const Vector<NUMTRACKERCAMPARAMETERS> ATANCamera::mvDefaultParams = makeVector(0.392, 0.613, 0.484, 0.476, 0.967);
00298 


ptam
Author(s): Stephan Weiss, Markus Achtelik, Simon Lynen
autogenerated on Tue Jan 7 2014 11:12:22