HomographyInit.cc
Go to the documentation of this file.
00001 // Copyright 2008 Isis Innovation Limited
00002 #include "HomographyInit.h"
00003 #include "SmallMatrixOpts.h"
00004 #include <utility>
00005 #include <TooN/se3.h>
00006 #include <TooN/SVD.h>
00007 #include <TooN/SymEigen.h>
00008 #include <TooN/wls.h>
00009 #include "MEstimator.h"
00010 
00011 using namespace std;
00012 bool HomographyInit::IsHomographyInlier(Matrix<3> m3Homography, HomographyMatch match)
00013 {
00014   Vector<2> v2Projected = project(m3Homography * unproject(match.v2CamPlaneFirst));
00015   Vector<2> v2Error = match.v2CamPlaneSecond - v2Projected;
00016   Vector<2> v2PixelError = match.m2PixelProjectionJac * v2Error;
00017   double dSquaredError = v2PixelError * v2PixelError;
00018   return (dSquaredError < mdMaxPixelErrorSquared);
00019 }
00020 
00021 double HomographyInit::MLESACScore(Matrix<3> m3Homography, HomographyMatch match)
00022 {
00023   Vector<2> v2Projected = project(m3Homography * unproject(match.v2CamPlaneFirst));
00024   Vector<2> v2Error = match.v2CamPlaneSecond - v2Projected;
00025   Vector<2> v2PixelError = match.m2PixelProjectionJac * v2Error;
00026   double dSquaredError = v2PixelError * v2PixelError;
00027   if(dSquaredError > mdMaxPixelErrorSquared)
00028     return mdMaxPixelErrorSquared;
00029   else 
00030     return dSquaredError;
00031 }
00032 
00033 bool HomographyInit::Compute(vector<HomographyMatch> vMatches, double dMaxPixelError, SE3<> &se3SecondFromFirst)
00034 {
00035   mdMaxPixelErrorSquared = dMaxPixelError * dMaxPixelError;
00036   mvMatches = vMatches;
00037   
00038   // Find best homography from minimal sets of image matches
00039   BestHomographyFromMatches_MLESAC();
00040   
00041   // Generate the inlier set, and refine the best estimate using this
00042   mvHomographyInliers.clear();
00043   for(unsigned int i=0; i<mvMatches.size(); i++)
00044     if(IsHomographyInlier(mm3BestHomography, mvMatches[i]))
00045       mvHomographyInliers.push_back(mvMatches[i]);
00046   for(int iteration = 0; iteration < 5; iteration++)
00047     RefineHomographyWithInliers();
00048   
00049   // Decompose the best homography into a set of possible decompositions
00050   DecomposeHomography();
00051 
00052   // At this stage should have eight decomposition options, if all went according to plan
00053   if(mvDecompositions.size() != 8)
00054     return false;
00055   
00056   // And choose the best one based on visibility constraints
00057   ChooseBestDecomposition();
00058   
00059   se3SecondFromFirst = mvDecompositions[0].se3SecondFromFirst;
00060   return true;
00061 }
00062 
00063 Matrix<3> HomographyInit::HomographyFromMatches(vector<HomographyMatch> vMatches)
00064 {
00065   unsigned int nPoints = vMatches.size();
00066   assert(nPoints >= 4);
00067   int nRows = 2*nPoints;
00068   if(nRows < 9)
00069     nRows = 9;
00070   Matrix<> m2Nx9(nRows, 9);
00071   for(unsigned int n=0; n<nPoints; n++)
00072     {
00073       double u = vMatches[n].v2CamPlaneSecond[0];
00074       double v = vMatches[n].v2CamPlaneSecond[1];
00075       
00076       double x = vMatches[n].v2CamPlaneFirst[0];
00077       double y = vMatches[n].v2CamPlaneFirst[1];
00078       
00079       // [u v]T = H [x y]T
00080       m2Nx9[n*2+0][0] = x;
00081       m2Nx9[n*2+0][1] = y;
00082       m2Nx9[n*2+0][2] = 1;
00083       m2Nx9[n*2+0][3] = 0;
00084       m2Nx9[n*2+0][4] = 0;
00085       m2Nx9[n*2+0][5] = 0;
00086       m2Nx9[n*2+0][6] = -x*u;
00087       m2Nx9[n*2+0][7] = -y*u;
00088       m2Nx9[n*2+0][8] = -u;
00089 
00090       m2Nx9[n*2+1][0] = 0;
00091       m2Nx9[n*2+1][1] = 0;
00092       m2Nx9[n*2+1][2] = 0;
00093       m2Nx9[n*2+1][3] = x;
00094       m2Nx9[n*2+1][4] = y;
00095       m2Nx9[n*2+1][5] = 1;
00096       m2Nx9[n*2+1][6] = -x*v;
00097       m2Nx9[n*2+1][7] = -y*v;
00098       m2Nx9[n*2+1][8] = -v;
00099     }
00100 
00101   if(nRows == 9)  
00102    for(int i=0; i<9; i++)  // Zero the last row of the matrix, 
00103      m2Nx9[8][i] = 0.0;  // TooN SVD leaves out the null-space otherwise
00104   
00105   // The right null-space of the matrix gives the homography...
00106   SVD<> svdHomography(m2Nx9);
00107   Vector<9> vH = svdHomography.get_VT()[8];
00108   Matrix<3> m3Homography;
00109   m3Homography[0] = vH.slice<0,3>();
00110   m3Homography[1] = vH.slice<3,3>();
00111   m3Homography[2] = vH.slice<6,3>();
00112   return m3Homography;
00113 };
00114 
00115 // Throughout the whole thing,
00116 // SecondView = Homography * FirstView
00117 
00118 void HomographyInit::RefineHomographyWithInliers()
00119 {
00120   WLS<9> wls;
00121   wls.add_prior(1.0);
00122   
00123   vector<Matrix<2,9> > vmJacobians;
00124   vector<Vector<2> > vvErrors;
00125   vector<double> vdErrorSquared;
00126   
00127   for(unsigned int i=0; i<mvHomographyInliers.size(); i++)
00128     {
00129       // First, find error.
00130       Vector<2> v2First = mvHomographyInliers[i].v2CamPlaneFirst;
00131       Vector<3> v3Second = mm3BestHomography * unproject(v2First);
00132       Vector<2> v2Second = project(v3Second);
00133       Vector<2> v2Second_real = mvHomographyInliers[i].v2CamPlaneSecond;
00134       Vector<2> v2Error = mvHomographyInliers[i].m2PixelProjectionJac * (v2Second_real - v2Second);
00135       
00136       vdErrorSquared.push_back((double)(v2Error* v2Error));
00137       vvErrors.push_back(v2Error);
00138       
00139       Matrix<2,9> m29Jacobian;
00140       double dDenominator = v3Second[2];
00141       
00142       // Jacobians wrt to the elements of the homography:
00143       // For x:
00144       m29Jacobian[0].slice<0,3>() = unproject(v2First) / dDenominator;
00145       m29Jacobian[0].slice<3,3>() = Zeros;
00146       double dNumerator = v3Second[0];
00147       m29Jacobian[0].slice<6,3>() = -unproject(v2First) * dNumerator / (dDenominator * dDenominator);
00148       // For y:
00149       m29Jacobian[1].slice<0,3>() = Zeros;
00150       m29Jacobian[1].slice<3,3>()  = unproject(v2First) / dDenominator;;
00151       dNumerator = v3Second[1];
00152       m29Jacobian[1].slice<6,3>() = -unproject(v2First) * dNumerator / (dDenominator * dDenominator);
00153       
00154       vmJacobians.push_back(mvHomographyInliers[i].m2PixelProjectionJac * m29Jacobian);
00155     }
00156   
00157   // Calculate robust sigma:
00158   vector<double> vdd = vdErrorSquared;
00159   double dSigmaSquared = Tukey::FindSigmaSquared(vdd);
00160   
00161   // Add re-weighted measurements to WLS:
00162   for(unsigned int i=0; i<mvHomographyInliers.size(); i++)
00163     {
00164       double dWeight = Tukey::Weight(vdErrorSquared[i], dSigmaSquared);
00165       wls.add_mJ(vvErrors[i][0], vmJacobians[i][0], dWeight);
00166       wls.add_mJ(vvErrors[i][1], vmJacobians[i][1], dWeight);
00167     }
00168   wls.compute();
00169   Vector<9> v9Update = wls.get_mu();
00170   Matrix<3> m3Update;
00171   m3Update[0] = v9Update.slice<0,3>();
00172   m3Update[1] = v9Update.slice<3,3>();
00173   m3Update[2] = v9Update.slice<6,3>();
00174   mm3BestHomography += m3Update;
00175 }
00176 
00177 void HomographyInit::BestHomographyFromMatches_MLESAC()
00178 {
00179   // Not many matches? Don't do ransac, throw them all in a pot and see what comes out.
00180   if(mvMatches.size() < 10)
00181     {
00182       mm3BestHomography = HomographyFromMatches(mvMatches);
00183       return;
00184     }
00185   
00186   // Enough matches? Run MLESAC.
00187   int anIndices[4];
00188   
00189   mm3BestHomography = Identity;
00190   double dBestError = 999999999999999999.9;
00191   
00192   // Do 300 MLESAC trials.
00193   for(int nR = 0; nR < 300 ; nR++)
00194     { 
00195       // Find set of four unique matches
00196       for(int i=0; i<4; i++)
00197         {
00198           bool isUnique = false;
00199           int n;
00200           while(!isUnique)
00201             {
00202               n = rand() % mvMatches.size();
00203               isUnique =true;
00204               for(int j=0; j<i && isUnique; j++)
00205                 if(anIndices[j] == n)
00206                   isUnique = false;
00207             };
00208           anIndices[i] = n;
00209         }
00210       vector<HomographyMatch> vMinimalMatches;
00211       for(int i=0; i<4; i++)
00212         vMinimalMatches.push_back(mvMatches[anIndices[i]]);
00213       
00214       // Find a homography from the minimal set..
00215       Matrix<3> m3Homography = HomographyFromMatches(vMinimalMatches);
00216       
00217       //..and sum resulting MLESAC score
00218       double dError = 0.0;
00219       for(unsigned int i=0; i<mvMatches.size(); i++)
00220         dError += MLESACScore(m3Homography, mvMatches[i]);
00221       
00222       if(dError < dBestError)
00223         {
00224           mm3BestHomography = m3Homography;
00225           dBestError = dError;
00226         }
00227     };
00228 }
00229 
00230 void HomographyInit::DecomposeHomography()
00231 {
00232   mvDecompositions.clear();
00233   SVD<3> svd(mm3BestHomography);
00234   Vector<3> v3Diag = svd.get_diagonal();
00235   double d1 = fabs(v3Diag[0]); // The paper suggests the square of these (e.g. the evalues of AAT)
00236   double d2 = fabs(v3Diag[1]); // should be used, but this is wrong. c.f. Faugeras' book.
00237   double d3 = fabs(v3Diag[2]);
00238   
00239   Matrix<3> U = svd.get_U();
00240   Matrix<3> V = svd.get_VT().T();
00241   
00242   double s = M3Det(U) * M3Det(V);
00243   
00244   double dPrime_PM = d2;
00245   
00246   int nCase;
00247   if(d1 != d2 && d2 != d3)
00248     nCase = 1;
00249   else if( d1 == d2 && d2 == d3)
00250     nCase = 3;
00251   else
00252     nCase = 2;
00253   
00254   if(nCase != 1)
00255     {
00256       cout << "  Homographyinit: This motion case is not implemented or is degenerate. Try again. " << endl;
00257       return;
00258     }
00259   
00260   double x1_PM;
00261   double x2;
00262   double x3_PM;
00263 
00264   // All below deals with the case = 1 case.
00265   // Case 1 implies (d1 != d3) 
00266   { // Eq. 12
00267     x1_PM = sqrt((d1*d1 - d2*d2) / (d1*d1 - d3*d3));
00268     x2    = 0;
00269     x3_PM = sqrt((d2*d2 - d3*d3) / (d1*d1 - d3*d3));
00270   };
00271   
00272   double e1[4] = {1.0,-1.0,1.0,-1.0};
00273   double e3[4] = {1.0, 1.0, -1.0,-1.0};
00274     
00275   Vector<3> v3np;
00276   HomographyDecomposition decomposition;
00277 
00278   // Case 1, d' > 0:
00279   decomposition.d = s * dPrime_PM;
00280   for(int signs=0; signs<4; signs++)
00281     {
00282       // Eq 13
00283       decomposition.m3Rp = Identity;
00284       double dSinTheta = (d1 - d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2;
00285       double dCosTheta = (d1 * x3_PM * x3_PM + d3 * x1_PM * x1_PM) / d2;
00286       decomposition.m3Rp[0][0] = dCosTheta;                
00287       decomposition.m3Rp[0][2] = -dSinTheta;
00288       decomposition.m3Rp[2][0] = dSinTheta;                
00289       decomposition.m3Rp[2][2] = dCosTheta;
00290       
00291       // Eq 14
00292       decomposition.v3Tp[0] = (d1 - d3) * x1_PM * e1[signs];
00293       decomposition.v3Tp[1] = 0.0;
00294       decomposition.v3Tp[2] = (d1 - d3) * -x3_PM * e3[signs];
00295   
00296       v3np[0] = x1_PM * e1[signs];
00297       v3np[1] = x2;
00298       v3np[2] = x3_PM * e3[signs];
00299       decomposition.v3n = V * v3np;
00300       
00301       mvDecompositions.push_back(decomposition);
00302     }
00303   // Case 1, d' < 0:
00304   decomposition.d = s * -dPrime_PM;
00305   for(int signs=0; signs<4; signs++)
00306     { 
00307       // Eq 15
00308       decomposition.m3Rp = -1 * Identity;
00309       double dSinPhi = (d1 + d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2;
00310       double dCosPhi = (d3 * x1_PM * x1_PM - d1 * x3_PM * x3_PM) / d2;
00311       decomposition.m3Rp[0][0] = dCosPhi;                
00312       decomposition.m3Rp[0][2] = dSinPhi;
00313       decomposition.m3Rp[2][0] = dSinPhi;                
00314       decomposition.m3Rp[2][2] = -dCosPhi;
00315       
00316       // Eq 16
00317       decomposition.v3Tp[0] = (d1 + d3) * x1_PM * e1[signs];
00318       decomposition.v3Tp[1] = 0.0;
00319       decomposition.v3Tp[2] = (d1 + d3) * x3_PM * e3[signs];
00320 
00321       v3np[0] = x1_PM * e1[signs];
00322       v3np[1] = x2;
00323       v3np[2] = x3_PM * e3[signs];
00324       decomposition.v3n = V * v3np;
00325       
00326       mvDecompositions.push_back(decomposition);
00327     }
00328   
00329   // While we have the SVD results calculated here, store the decomposition R and t results as well..
00330   for(unsigned int i=0; i<mvDecompositions.size(); i++)
00331     {
00332       mvDecompositions[i].se3SecondFromFirst.get_rotation() = 
00333         s * U * mvDecompositions[i].m3Rp * V.T();
00334       mvDecompositions[i].se3SecondFromFirst.get_translation() = 
00335         U * mvDecompositions[i].v3Tp;
00336     }
00337 }
00338 
00339 bool operator<(const HomographyDecomposition lhs, const HomographyDecomposition rhs)
00340 {
00341   return lhs.nScore < rhs.nScore;
00342 }
00343 
00344 static double SampsonusError(Vector<2> &v2Dash, const Matrix<3> &m3Essential, Vector<2> &v2)
00345 {
00346   Vector<3> v3Dash = unproject(v2Dash);
00347   Vector<3> v3 = unproject(v2);  
00348   
00349   double dError = v3Dash * m3Essential * v3;
00350   
00351   Vector<3> fv3 = m3Essential * v3;
00352   Vector<3> fTv3Dash = m3Essential.T() * v3Dash;
00353   
00354   Vector<2> fv3Slice = fv3.slice<0,2>();
00355   Vector<2> fTv3DashSlice = fTv3Dash.slice<0,2>();
00356   
00357   return (dError * dError / (fv3Slice * fv3Slice + fTv3DashSlice * fTv3DashSlice));
00358 }
00359 
00360 
00361 void HomographyInit::ChooseBestDecomposition()
00362 {
00363   assert(mvDecompositions.size() == 8);
00364   for(unsigned int i=0; i<mvDecompositions.size(); i++)
00365     {
00366       HomographyDecomposition &decom = mvDecompositions[i];
00367       int nPositive = 0;
00368       for(unsigned int m=0; m<mvHomographyInliers.size(); m++)
00369         {
00370           Vector<2> &v2 = mvHomographyInliers[m].v2CamPlaneFirst;
00371           double dVisibilityTest = (mm3BestHomography[2][0] * v2[0] + mm3BestHomography[2][1] * v2[1] + mm3BestHomography[2][2]) / decom.d;
00372           if(dVisibilityTest > 0.0)
00373             nPositive++;
00374         };
00375       decom.nScore = -nPositive;
00376     }
00377   
00378   sort(mvDecompositions.begin(), mvDecompositions.end());
00379   mvDecompositions.resize(4);
00380   
00381   for(unsigned int i=0; i<mvDecompositions.size(); i++)
00382     {
00383       HomographyDecomposition &decom = mvDecompositions[i];
00384       int nPositive = 0;
00385       for(unsigned int m=0; m<mvHomographyInliers.size(); m++)
00386         {
00387           Vector<3> v3 = unproject(mvHomographyInliers[m].v2CamPlaneFirst);
00388           double dVisibilityTest = v3 * decom.v3n / decom.d;
00389           if(dVisibilityTest > 0.0)
00390             nPositive++;
00391         };
00392       decom.nScore = -nPositive;
00393     }
00394   
00395   sort(mvDecompositions.begin(), mvDecompositions.end());
00396   mvDecompositions.resize(2);
00397   
00398   // According to Faugeras and Lustman, ambiguity exists if the two scores are equal
00399   // but in practive, better to look at the ratio!
00400   double dRatio = (double) mvDecompositions[1].nScore / (double) mvDecompositions[0].nScore;
00401 
00402   if(dRatio < 0.9) // no ambiguity!
00403     mvDecompositions.erase(mvDecompositions.begin() + 1);
00404   else             // two-way ambiguity. Resolve by sampsonus score of all points.
00405     {
00406       double dErrorSquaredLimit  = mdMaxPixelErrorSquared * 4;
00407       double adSampsonusScores[2];
00408       for(int i=0; i<2; i++)
00409         {
00410           SE3<> se3 = mvDecompositions[i].se3SecondFromFirst;
00411           Matrix<3> m3Essential;
00412           for(int j=0; j<3; j++)
00413             m3Essential.T()[j] = se3.get_translation() ^ se3.get_rotation().get_matrix().T()[j];
00414           
00415           double dSumError = 0;
00416           for(unsigned int m=0; m < mvMatches.size(); m++ )
00417             {
00418               double d = SampsonusError(mvMatches[m].v2CamPlaneSecond, m3Essential, mvMatches[m].v2CamPlaneFirst);
00419               if(d > dErrorSquaredLimit)
00420                 d = dErrorSquaredLimit;
00421               dSumError += d;
00422             }
00423           
00424           adSampsonusScores[i] = dSumError;
00425         }
00426 
00427       if(adSampsonusScores[0] <= adSampsonusScores[1])
00428         mvDecompositions.erase(mvDecompositions.begin() + 1);
00429       else
00430         mvDecompositions.erase(mvDecompositions.begin());
00431     }
00432   
00433 }
00434 
00435 


uga_tum_ardrone
Author(s):
autogenerated on Sat Jun 8 2019 20:30:11