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


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