CalibImage.cc
Go to the documentation of this file.
00001 // Copyright 2008 Isis Innovation Limited
00002 #include "ptam/OpenGL.h"
00003 #include "ptam/CalibImage.h"
00004 #include <stdlib.h>
00005 //#include <gvars3/instances.h>
00006 #include <cvd/utility.h>
00007 #include <cvd/convolution.h>
00008 #include <cvd/fast_corner.h>
00009 #include <cvd/vector_image_ref.h>
00010 #include <cvd/image_interpolate.h>
00011 
00012 #include <TooN/se3.h>
00013 #include <TooN/SVD.h>
00014 #include <TooN/wls.h>
00015 
00016 #include <ptam/Params.h>
00017 
00018 using namespace std;
00019 using namespace CVD;
00020 //using namespace GVars3;
00021 
00022 inline bool IsCorner(Image<byte> &im, ImageRef ir, int nGate)
00023 { // Does a quick check to see if a point in an image could be a grid corner.
00024   // Does this by going around a 16-pixel ring, and checking that there's four
00025   // transitions (black - white- black - white - )
00026   // Also checks that the central pixel is blurred.
00027 
00028   // Find the mean intensity of the pixel ring...
00029   int nSum = 0;
00030   static byte abPixels[16];
00031   for(int i=0; i<16; i++)
00032   {
00033     abPixels[i] = im[ir + fast_pixel_ring[i]];
00034     nSum += abPixels[i];
00035   };
00036   int nMean = nSum / 16;
00037   int nHiThresh = nMean + nGate;
00038   int nLoThresh = nMean - nGate;
00039 
00040   // If the center pixel is roughly the same as the mean, this isn't a corner.
00041   int nCenter = im[ir];
00042   if(nCenter <= nLoThresh || nCenter >= nHiThresh)
00043     return false;
00044 
00045   // Count transitions around the ring... there should be four!
00046   bool bState = (abPixels[15] > nMean);
00047   int nSwaps = 0;
00048   for(int i=0; i<16; i++)
00049   {
00050     byte bValNow = abPixels[i];
00051     if(bState)
00052     {
00053       if(bValNow < nLoThresh)
00054       {
00055         bState = false;
00056         nSwaps++;
00057       }
00058     }
00059     else
00060       if(bValNow > nHiThresh)
00061       {
00062         bState = true;
00063         nSwaps++;
00064       };
00065   }
00066   return (nSwaps == 4);
00067 };
00068 
00069 Vector<2> GuessInitialAngles(Image<byte> &im, ImageRef irCenter)
00070     {
00071   // The iterative patch-finder works better if the initial guess
00072   // is roughly aligned! Find one of the line-axes by searching round 
00073   // the circle for the strongest gradient, and use that and +90deg as the
00074   // initial guesses for patch angle.
00075   //
00076   // Yes, this is a very poor estimate, but it's generally (hopefully?) 
00077   // enough for the iterative finder to converge.
00078 
00079   image_interpolate<Interpolate::Bilinear, byte> imInterp(im);
00080   double dBestAngle = 0;
00081   double dBestGradMag = 0;
00082   double dGradAtBest = 0;
00083   for(double dAngle = 0.0; dAngle < M_PI; dAngle += 0.1)
00084   {
00085     Vector<2> v2Dirn;
00086     v2Dirn[0] = cos(dAngle);      v2Dirn[1] = sin(dAngle);
00087     Vector<2> v2Perp;
00088     v2Perp[1] = -v2Dirn[0];      v2Perp[0] = v2Dirn[1];
00089 
00090     double dG =     imInterp[vec(irCenter) + v2Dirn * 3.0 + v2Perp * 0.1] -
00091         imInterp[vec(irCenter) + v2Dirn * 3.0 - v2Perp * 0.1]
00092                  +      imInterp[vec(irCenter) - v2Dirn * 3.0 - v2Perp * 0.1] -
00093                  imInterp[vec(irCenter) - v2Dirn * 3.0 + v2Perp * 0.1];
00094     if(fabs(dG) > dBestGradMag)
00095     {
00096       dBestGradMag = fabs(dG);
00097       dGradAtBest = dG;
00098       dBestAngle = dAngle;
00099     };
00100   }
00101 
00102   Vector<2> v2Ret;
00103   if(dGradAtBest < 0)
00104   {   v2Ret[0] = dBestAngle; v2Ret[1] = dBestAngle + M_PI / 2.0;    }
00105   else
00106   {   v2Ret[1] = dBestAngle; v2Ret[0] = dBestAngle - M_PI / 2.0;    }
00107   return v2Ret;
00108     }
00109 
00110 bool CalibImage::MakeFromImage(Image<byte> &im)
00111 {
00112   //Weiss{
00113   //static int gvnCornerPatchSize = 20;
00114   const FixParams& pPars = PtamParameters::fixparams();
00115   static int gvnCornerPatchSize = pPars.CameraCalibrator_CornerPatchSize;
00116   //static gvar3<int> gvnCornerPatchSize("CameraCalibrator.CornerPatchPixelSize", 20, SILENT);
00117   //}
00118   mvCorners.clear();
00119   mvGridCorners.clear();
00120 
00121   mim = im;
00122   mim.make_unique();
00123 
00124   // Find potential corners..
00125   // This works better on a blurred image, so make a blurred copy
00126   // and run the corner finding on that.
00127   {
00128     Image<byte> imBlurred = mim;
00129     imBlurred.make_unique();
00130 
00131     double blursigma = pPars.Calibrator_BlurSigma;
00132 
00133     convolveGaussian(imBlurred, blursigma);
00134     ImageRef irTopLeft(5,5);
00135     ImageRef irBotRight = mim.size() - irTopLeft;
00136     ImageRef ir = irTopLeft;
00137     glPointSize(1);
00138     glColor3f(1,0,1);
00139     glBegin(GL_POINTS);
00140     int nGate = pPars.Calibrator_MeanGate;
00141     do
00142       if(IsCorner(imBlurred, ir, nGate))
00143       {
00144         mvCorners.push_back(ir);
00145         glVertex(ir);
00146       }
00147     while(ir.next(irTopLeft, irBotRight));
00148     glEnd();
00149   }
00150 
00151   // If there's not enough corners, i.e. camera pointing somewhere random, abort.
00152   // int MinCornersForGrabbedImage=20;
00153   int MinCornersForGrabbedImage=pPars.Calibrator_MinCornersForGrabbedImage;
00154   if((int) mvCorners.size() < MinCornersForGrabbedImage)
00155     return false;
00156 
00157   // Pick a central corner point...
00158   ImageRef irCenterOfImage = mim.size()  / 2;
00159   ImageRef irBestCenterPos;
00160   unsigned int nBestDistSquared = 99999999;
00161   for(unsigned int i=0; i<mvCorners.size(); i++)
00162   {
00163     unsigned int nDist = (mvCorners[i] - irCenterOfImage).mag_squared();
00164     if(nDist < nBestDistSquared)
00165     {
00166       nBestDistSquared = nDist;
00167       irBestCenterPos = mvCorners[i];
00168     }
00169   }
00170 
00171   // ... and try to fit a corner-patch to that.
00172   CalibCornerPatch Patch(gvnCornerPatchSize);
00173   CalibCornerPatch::Params Params;
00174   Params.v2Pos = vec(irBestCenterPos);
00175   Params.v2Angles = GuessInitialAngles(mim, irBestCenterPos); 
00176   Params.dGain = 80.0;
00177   Params.dMean = 120.0;
00178 
00179   if(!Patch.IterateOnImageWithDrawing(Params, mim))
00180     return false;
00181 
00182   // The first found corner patch becomes the origin of the detected grid.
00183   CalibGridCorner cFirst;
00184   cFirst.Params = Params;
00185   mvGridCorners.push_back(cFirst);
00186   cFirst.Draw();
00187 
00188   // Next, go in two compass directions from the origin patch, and see if 
00189   // neighbors can be found.
00190   if(!(ExpandByAngle(0,0) || ExpandByAngle(0,2)))
00191     return false;
00192   if(!(ExpandByAngle(0,1) || ExpandByAngle(0,3)))
00193     return false;
00194 
00195   mvGridCorners[1].mInheritedSteps = mvGridCorners[2].mInheritedSteps = mvGridCorners[0].GetSteps(mvGridCorners);
00196 
00197   // The three initial grid elements are enough to find the rest of the grid.
00198   int nNext;
00199   int nSanityCounter = 0; // Stop it getting stuck in an infinite loop...
00200   const int nSanityCounterLimit = 500;
00201   while((nNext = NextToExpand()) >= 0 && nSanityCounter < nSanityCounterLimit )
00202   {
00203     ExpandByStep(nNext);
00204     nSanityCounter++;
00205   }
00206   if(nSanityCounter == nSanityCounterLimit)
00207     return false;
00208 
00209   DrawImageGrid();
00210   return true;
00211 }
00212 
00213 bool CalibImage::ExpandByAngle(int nSrc, int nDirn)
00214 {
00215   //Weiss{
00216   //static int gvnCornerPatchSize = 20;
00217   const FixParams& pPars = PtamParameters::fixparams();
00218   static int gvnCornerPatchSize = pPars.CameraCalibrator_CornerPatchSize;
00219   //static gvar3<int> gvnCornerPatchSize("CameraCalibrator.CornerPatchPixelSize", 20, SILENT);
00220   //}
00221   CalibGridCorner &gSrc = mvGridCorners[nSrc];
00222 
00223   ImageRef irBest;
00224   double dBestDist = 99999;
00225   Vector<2> v2TargetDirn = gSrc.Params.m2Warp().T()[nDirn%2];
00226   if(nDirn >= 2)
00227     v2TargetDirn *= -1;
00228   for(unsigned int i=0; i<mvCorners.size(); i++)
00229   {
00230     Vector<2> v2Diff = vec(mvCorners[i]) - gSrc.Params.v2Pos;
00231     if(v2Diff * v2Diff < 100)
00232       continue;
00233     if(v2Diff * v2Diff > dBestDist * dBestDist)
00234       continue;
00235     Vector<2> v2Dirn = v2Diff;
00236     normalize(v2Dirn);
00237     if(v2Dirn * v2TargetDirn < cos(M_PI / 18.0))
00238       continue;
00239     dBestDist = sqrt(v2Diff * v2Diff);
00240     irBest = mvCorners[i];
00241   }
00242 
00243   CalibGridCorner gTarget;
00244   gTarget.Params = gSrc.Params;
00245   gTarget.Params.v2Pos = vec(irBest);
00246   gTarget.Params.dGain *= -1;
00247 
00248   CalibCornerPatch Patch(gvnCornerPatchSize);
00249   if(!Patch.IterateOnImageWithDrawing(gTarget.Params, mim))
00250   {
00251     gSrc.aNeighborStates[nDirn].val = N_FAILED;
00252     return false;
00253   }
00254 
00255   gTarget.irGridPos = gSrc.irGridPos;
00256   if(nDirn < 2)
00257     gTarget.irGridPos[nDirn]++;
00258   else gTarget.irGridPos[nDirn%2]--;
00259   // Update connection states:
00260   mvGridCorners.push_back(gTarget); // n.b. This invalidates gSrc!
00261   mvGridCorners.back().aNeighborStates[(nDirn + 2) % 4].val = nSrc;
00262   mvGridCorners[nSrc].aNeighborStates[nDirn].val = mvGridCorners.size() - 1;
00263 
00264   mvGridCorners.back().Draw();
00265   return true;
00266 }
00267 
00268 
00269 void CalibGridCorner::Draw()
00270 {
00271   glColor3f(0,1,0);
00272   glEnable(GL_LINE_SMOOTH);
00273   glEnable(GL_BLEND);
00274   glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
00275   glBegin(GL_LINES);
00276   glVertex(Params.v2Pos + Params.m2Warp() * vec(ImageRef( 10,0)));
00277   glVertex(Params.v2Pos + Params.m2Warp() * vec(ImageRef(-10,0)));
00278   glVertex(Params.v2Pos + Params.m2Warp() * vec(ImageRef( 0, 10)));
00279   glVertex(Params.v2Pos + Params.m2Warp() * vec(ImageRef( 0,-10)));
00280   glEnd();
00281 }
00282 
00283 
00284 double CalibGridCorner::ExpansionPotential()
00285 {
00286   // Scoring function. How good would this grid corner be at finding a neighbor?
00287   // The best case is if it's already surrounded by three neighbors and only needs
00288   // to find the last one (because it'll have the most accurate guess for where
00289   // the last one should be) and so on.
00290   int nMissing = 0;
00291   for(int i=0; i<4; i++)
00292     if(aNeighborStates[i].val == N_NOT_TRIED)
00293       nMissing++;
00294 
00295   if(nMissing == 0)
00296     return 0.0;
00297 
00298   if(nMissing == 1)
00299     return 100.0;
00300 
00301   if(nMissing == 3)
00302     return 1.0;
00303 
00304   if(nMissing == 2)
00305   {
00306     int nFirst = 0;
00307     while(aNeighborStates[nFirst].val != N_NOT_TRIED)
00308       nFirst++;
00309     if(aNeighborStates[(nFirst + 2) % 4].val == N_NOT_TRIED)
00310       return 10.0;
00311     else
00312       return 20.0;
00313   }
00314   assert(0); // should never get here
00315   return 0.0;
00316 };
00317 
00318 
00319 Matrix<2> CalibGridCorner::GetSteps(vector<CalibGridCorner> &vgc)
00320 {
00321   Matrix<2> m2Steps;
00322   for(int dirn=0; dirn<2; dirn++)
00323   {
00324     Vector<2> v2Dirn;
00325     int nFound = 0;
00326     v2Dirn = Zeros;
00327     if(aNeighborStates[dirn].val >=0)
00328     {
00329       v2Dirn += vgc[aNeighborStates[dirn].val].Params.v2Pos - Params.v2Pos;
00330       nFound++;
00331     }
00332     if(aNeighborStates[dirn+2].val >=0)
00333     {
00334       v2Dirn -= vgc[aNeighborStates[dirn+2].val].Params.v2Pos - Params.v2Pos;
00335       nFound++;
00336     }
00337     if(nFound == 0)
00338       m2Steps[dirn] = mInheritedSteps[dirn];
00339     else
00340       m2Steps[dirn] = v2Dirn / nFound;
00341   }
00342   return m2Steps;
00343 };
00344 
00345 int CalibImage::NextToExpand()
00346 {
00347   int nBest = -1;
00348   double dBest = 0.0;
00349 
00350   for(unsigned int i=0; i<mvGridCorners.size(); i++)
00351   {
00352     double d = mvGridCorners[i].ExpansionPotential();
00353     if(d > dBest)
00354     {
00355       nBest = i;
00356       dBest = d;
00357     }
00358   }
00359   return nBest;
00360 }
00361 
00362 void CalibImage::ExpandByStep(int n)
00363 {
00364   //Weiss{
00365   // static double gvdMaxStepDistFraction = 0.3;
00366   // static int gvnCornerPatchSize = 20;
00367   const FixParams& pPars = PtamParameters::fixparams();
00368   static double gvdMaxStepDistFraction = pPars.CameraCalibrator_MaxStepDistFraction;
00369   static int gvnCornerPatchSize = pPars.CameraCalibrator_CornerPatchSize;
00370   //static gvar3<double> gvdMaxStepDistFraction("CameraCalibrator.ExpandByStepMaxDistFrac", 0.3, SILENT);
00371   //static gvar3<int> gvnCornerPatchSize("CameraCalibrator.CornerPatchPixelSize", 20, SILENT);
00372   //}
00373 
00374   CalibGridCorner &gSrc = mvGridCorners[n];
00375 
00376   // First, choose which direction to expand in...
00377   // Ideally, choose a dirn for which the Step calc is good!
00378   int nDirn = -10;
00379   for(int i=0; nDirn == -10 && i<4; i++)
00380   {
00381     if(gSrc.aNeighborStates[i].val == N_NOT_TRIED &&
00382         gSrc.aNeighborStates[(i+2) % 4].val >= 0)
00383       nDirn = i;
00384   }
00385   if(nDirn == -10)
00386     for(int i=0; nDirn == -10 && i<4; i++)
00387     {
00388       if(gSrc.aNeighborStates[i].val == N_NOT_TRIED)
00389         nDirn = i;
00390     }
00391   assert(nDirn != -10);
00392 
00393   Vector<2> v2Step;
00394   ImageRef irGridStep = IR_from_dirn(nDirn);
00395 
00396   v2Step = gSrc.GetSteps(mvGridCorners).T() * vec(irGridStep);
00397 
00398   Vector<2> v2SearchPos = gSrc.Params.v2Pos + v2Step;
00399 
00400   // Before the search: pre-fill the failure result for easy returns.
00401   gSrc.aNeighborStates[nDirn].val = N_FAILED;
00402 
00403   ImageRef irBest;
00404   double dBestDist = 99999;
00405   for(unsigned int i=0; i<mvCorners.size(); i++)
00406   {
00407     Vector<2> v2Diff = vec(mvCorners[i]) - v2SearchPos;
00408     if(v2Diff * v2Diff > dBestDist * dBestDist)
00409       continue;
00410     dBestDist = sqrt(v2Diff * v2Diff);
00411     irBest = mvCorners[i];
00412   }
00413 
00414   double dStepDist= sqrt(v2Step * v2Step);
00415   if(dBestDist > gvdMaxStepDistFraction * dStepDist)
00416     return;
00417 
00418   CalibGridCorner gTarget;
00419   gTarget.Params = gSrc.Params;
00420   gTarget.Params.v2Pos = vec(irBest);
00421   gTarget.Params.dGain *= -1;
00422   gTarget.irGridPos = gSrc.irGridPos + irGridStep;
00423   gTarget.mInheritedSteps = gSrc.GetSteps(mvGridCorners);
00424   CalibCornerPatch Patch(gvnCornerPatchSize);
00425   if(!Patch.IterateOnImageWithDrawing(gTarget.Params, mim))
00426     return;
00427 
00428   // Update connection states:
00429   int nTargetNum = mvGridCorners.size();
00430   for(int dirn = 0; dirn<4; dirn++)
00431   {
00432     ImageRef irSearch = gTarget.irGridPos + IR_from_dirn(dirn);
00433     for(unsigned int i=0; i<mvGridCorners.size(); i++)
00434       if(mvGridCorners[i].irGridPos == irSearch)
00435       {
00436         gTarget.aNeighborStates[dirn].val = i;
00437         mvGridCorners[i].aNeighborStates[(dirn + 2) % 4].val = nTargetNum;
00438       }
00439   }
00440   mvGridCorners.push_back(gTarget);
00441   mvGridCorners.back().Draw();
00442 }
00443 
00444 void CalibImage::DrawImageGrid()
00445 {
00446   glLineWidth(2);
00447   glColor3f(0,0,1);
00448   glEnable(GL_LINE_SMOOTH);
00449   glEnable(GL_BLEND);
00450   glBegin(GL_LINES);
00451 
00452   for(int i=0; i< (int) mvGridCorners.size(); i++)
00453   {
00454     for(int dirn=0; dirn<4; dirn++)
00455       if(mvGridCorners[i].aNeighborStates[dirn].val > i)
00456       {
00457         glVertex(mvGridCorners[i].Params.v2Pos);
00458         glVertex(mvGridCorners[mvGridCorners[i].aNeighborStates[dirn].val].Params.v2Pos);
00459       }
00460   }
00461   glEnd();
00462 
00463   glPointSize(5);
00464   glEnable(GL_POINT_SMOOTH);
00465   glColor3f(1,1,0);
00466   glBegin(GL_POINTS);
00467   for(unsigned int i=0; i<mvGridCorners.size(); i++)
00468     glVertex(mvGridCorners[i].Params.v2Pos);
00469   glEnd();
00470 };
00471 
00472 void CalibImage::Draw3DGrid(ATANCamera &Camera, bool bDrawErrors)
00473 {
00474   glLineWidth(2);
00475   glColor3f(0,0,1);
00476   glEnable(GL_LINE_SMOOTH);
00477   glEnable(GL_BLEND);
00478   glBegin(GL_LINES);
00479 
00480   for(int i=0; i< (int) mvGridCorners.size(); i++)
00481   {
00482     for(int dirn=0; dirn<4; dirn++)
00483       if(mvGridCorners[i].aNeighborStates[dirn].val > i)
00484       {
00485         Vector<3> v3; v3[2] = 0.0;
00486         v3.slice<0,2>() = vec(mvGridCorners[i].irGridPos);
00487         glVertex(Camera.Project(project(mse3CamFromWorld * v3)));
00488         v3.slice<0,2>() = vec(mvGridCorners[mvGridCorners[i].aNeighborStates[dirn].val].irGridPos);
00489         glVertex(Camera.Project(project(mse3CamFromWorld * v3)));
00490       }
00491   }
00492   glEnd();
00493 
00494   if(bDrawErrors)
00495   {
00496     glColor3f(1,0,0);
00497     glLineWidth(1);
00498     glBegin(GL_LINES);
00499     for(int i=0; i< (int) mvGridCorners.size(); i++)
00500     {
00501       Vector<3> v3; v3[2] = 0.0;
00502       v3.slice<0,2>() = vec(mvGridCorners[i].irGridPos);
00503       Vector<2> v2Pixels_Projected = Camera.Project(project(mse3CamFromWorld * v3));
00504       Vector<2> v2Error = mvGridCorners[i].Params.v2Pos - v2Pixels_Projected;
00505       glVertex(v2Pixels_Projected);
00506       glVertex(v2Pixels_Projected + 10.0 * v2Error);
00507     }
00508     glEnd();
00509   }
00510 };
00511 
00512 ImageRef CalibImage::IR_from_dirn(int nDirn)
00513 {
00514   ImageRef ir;
00515   ir[nDirn%2] = (nDirn < 2) ? 1: -1;
00516   return ir;
00517 }
00518 
00519 
00520 void CalibImage::GuessInitialPose(ATANCamera &Camera)
00521 {
00522   // First, find a homography which maps the grid to the unprojected image coords
00523   // Use the standard null-space-of-SVD-thing to find 9 homography parms
00524   // (c.f. appendix of thesis)
00525 
00526   int nPoints = mvGridCorners.size();
00527   Matrix<> m2Nx9(2*nPoints, 9);
00528   for(int n=0; n<nPoints; n++)
00529   {
00530     // First, un-project the points to the image plane
00531     Vector<2> v2UnProj = Camera.UnProject(mvGridCorners[n].Params.v2Pos);
00532     double u = v2UnProj[0];
00533     double v = v2UnProj[1];
00534     // Then fill in the matrix..
00535     double x = mvGridCorners[n].irGridPos.x;
00536     double y = mvGridCorners[n].irGridPos.y;
00537 
00538     m2Nx9[n*2+0][0] = x;
00539     m2Nx9[n*2+0][1] = y;
00540     m2Nx9[n*2+0][2] = 1;
00541     m2Nx9[n*2+0][3] = 0;
00542     m2Nx9[n*2+0][4] = 0;
00543     m2Nx9[n*2+0][5] = 0;
00544     m2Nx9[n*2+0][6] = -x*u;
00545     m2Nx9[n*2+0][7] = -y*u;
00546     m2Nx9[n*2+0][8] = -u;
00547 
00548     m2Nx9[n*2+1][0] = 0;
00549     m2Nx9[n*2+1][1] = 0;
00550     m2Nx9[n*2+1][2] = 0;
00551     m2Nx9[n*2+1][3] = x;
00552     m2Nx9[n*2+1][4] = y;
00553     m2Nx9[n*2+1][5] = 1;
00554     m2Nx9[n*2+1][6] = -x*v;
00555     m2Nx9[n*2+1][7] = -y*v;
00556     m2Nx9[n*2+1][8] = -v;
00557   }
00558 
00559   // The right null-space (should only be one) of the matrix gives the homography...
00560   SVD<> svdHomography(m2Nx9);
00561   Vector<9> vH = svdHomography.get_VT()[8];
00562   Matrix<3> m3Homography;
00563   m3Homography[0] = vH.slice<0,3>();
00564   m3Homography[1] = vH.slice<3,3>();
00565   m3Homography[2] = vH.slice<6,3>();
00566 
00567 
00568   // Fix up possibly poorly conditioned bits of the homography
00569   {
00570     SVD<2> svdTopLeftBit(m3Homography.slice<0,0,2,2>());
00571     Vector<2> v2Diagonal = svdTopLeftBit.get_diagonal();
00572     m3Homography = m3Homography / v2Diagonal[0];
00573     v2Diagonal = v2Diagonal / v2Diagonal[0];
00574     double dLambda2 = v2Diagonal[1];
00575 
00576     Vector<2> v2b;   // This is one hypothesis for v2b ; the other is the negative.
00577     v2b[0] = 0.0;
00578     v2b[1] = sqrt( 1.0 - (dLambda2 * dLambda2)); 
00579 
00580     Vector<2> v2aprime = v2b * svdTopLeftBit.get_VT();
00581 
00582     Vector<2> v2a = m3Homography[2].slice<0,2>();
00583     double dDotProd = v2a * v2aprime;
00584 
00585     if(dDotProd>0) 
00586       m3Homography[2].slice<0,2>() = v2aprime;
00587     else
00588       m3Homography[2].slice<0,2>() = -v2aprime;
00589   }
00590 
00591 
00592   // OK, now turn homography into something 3D ...simple gram-schmidt ortho-norm
00593   // Take 3x3 matrix H with column: abt
00594   // And add a new 3rd column: abct
00595   Matrix<3> mRotation;
00596   Vector<3> vTranslation;
00597   double dMag1 = sqrt(m3Homography.T()[0] * m3Homography.T()[0]);
00598   m3Homography = m3Homography / dMag1;
00599 
00600   mRotation.T()[0] = m3Homography.T()[0];
00601 
00602   // ( all components of the first vector are removed from the second...
00603 
00604   mRotation.T()[1] = m3Homography.T()[1] - m3Homography.T()[0]*(m3Homography.T()[0]*m3Homography.T()[1]); 
00605   mRotation.T()[1] /= sqrt(mRotation.T()[1] * mRotation.T()[1]);
00606   mRotation.T()[2] = mRotation.T()[0]^mRotation.T()[1];
00607   vTranslation = m3Homography.T()[2];
00608 
00609   // Store result
00610   mse3CamFromWorld.get_rotation()=mRotation;
00611   mse3CamFromWorld.get_translation() = vTranslation;
00612 };
00613 
00614 vector<CalibImage::ErrorAndJacobians> CalibImage::Project(ATANCamera &Camera)
00615 {
00616   vector<ErrorAndJacobians> vResult;
00617   for(unsigned int n=0; n<mvGridCorners.size(); n++)
00618   {
00619     ErrorAndJacobians EAJ;
00620 
00621     // First, project into image...
00622     Vector<3> v3World;
00623     v3World[2] = 0.0;
00624     v3World.slice<0,2>() = vec(mvGridCorners[n].irGridPos);
00625 
00626     Vector<3> v3Cam = mse3CamFromWorld * v3World;
00627     if(v3Cam[2] <= 0.001)
00628       continue;
00629 
00630     Vector<2> v2Image = Camera.Project(project(v3Cam));
00631     if(Camera.Invalid())
00632       continue;
00633 
00634     EAJ.v2Error = mvGridCorners[n].Params.v2Pos - v2Image;
00635 
00636     // Now find motion jacobian..
00637     double dOneOverCameraZ = 1.0 / v3Cam[2];
00638     Matrix<2> m2CamDerivs = Camera.GetProjectionDerivs();
00639 
00640     for(int dof=0; dof<6; dof++)
00641     {
00642       const Vector<4> v4Motion = SE3<>::generator_field(dof, unproject(v3Cam));
00643       Vector<2> v2CamFrameMotion;
00644       v2CamFrameMotion[0] = (v4Motion[0] - v3Cam[0] * v4Motion[2] * dOneOverCameraZ) * dOneOverCameraZ;
00645       v2CamFrameMotion[1] = (v4Motion[1] - v3Cam[1] * v4Motion[2] * dOneOverCameraZ) * dOneOverCameraZ;
00646       EAJ.m26PoseJac.T()[dof] = m2CamDerivs * v2CamFrameMotion;
00647     };
00648 
00649     // Finally, the camera provids its own jacobian
00650     EAJ.m2NCameraJac = Camera.GetCameraParameterDerivs();
00651     vResult.push_back(EAJ);
00652   }
00653   return vResult;
00654 };
00655 
00656 
00657 
00658 
00659 
00660 
00661 


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