17 double dSquaredError = v2PixelError * v2PixelError;
18 return (dSquaredError < mdMaxPixelErrorSquared);
26 double dSquaredError = v2PixelError * v2PixelError;
27 if(dSquaredError > mdMaxPixelErrorSquared)
28 return mdMaxPixelErrorSquared;
35 mdMaxPixelErrorSquared = dMaxPixelError * dMaxPixelError;
39 BestHomographyFromMatches_MLESAC();
42 mvHomographyInliers.clear();
43 for(
unsigned int i=0; i<mvMatches.size(); i++)
44 if(IsHomographyInlier(mm3BestHomography, mvMatches[i]))
45 mvHomographyInliers.push_back(mvMatches[i]);
46 for(
int iteration = 0; iteration < 5; iteration++)
47 RefineHomographyWithInliers();
50 DecomposeHomography();
53 if(mvDecompositions.size() != 8)
57 ChooseBestDecomposition();
59 se3SecondFromFirst = mvDecompositions[0].se3SecondFromFirst;
65 unsigned int nPoints = vMatches.size();
67 int nRows = 2*nPoints;
71 for(
unsigned int n=0; n<nPoints; n++)
73 double u = vMatches[n].v2CamPlaneSecond[0];
74 double v = vMatches[n].v2CamPlaneSecond[1];
76 double x = vMatches[n].v2CamPlaneFirst[0];
77 double y = vMatches[n].v2CamPlaneFirst[1];
87 m2Nx9[n*2+0][6] = -x*u;
88 m2Nx9[n*2+0][7] = -y*u;
97 m2Nx9[n*2+1][6] = -x*v;
98 m2Nx9[n*2+1][7] = -y*v;
103 for(
int i=0; i<9; i++)
107 SVD<> svdHomography(m2Nx9);
110 m3Homography[0] = vH.slice<0,3>();
111 m3Homography[1] = vH.slice<3,3>();
112 m3Homography[2] = vH.slice<6,3>();
124 vector<Matrix<2,9> > vmJacobians;
125 vector<Vector<2> > vvErrors;
126 vector<double> vdErrorSquared;
129 for(
unsigned int i=0; i<mvHomographyInliers.size(); i++)
132 Vector<2> v2First = mvHomographyInliers[i].v2CamPlaneFirst;
135 Vector<2> v2Second_real = mvHomographyInliers[i].v2CamPlaneSecond;
136 Vector<2> v2Error = mvHomographyInliers[i].m2PixelProjectionJac * (v2Second_real - v2Second);
138 vdErrorSquared.push_back(v2Error* v2Error);
139 vvErrors.push_back(v2Error);
142 double dDenominator = v3Second[2];
146 m29Jacobian[0].slice<0,3>() =
unproject(v2First) / dDenominator;
147 m29Jacobian[0].slice<3,3>() =
Zeros;
148 double dNumerator = v3Second[0];
149 m29Jacobian[0].slice<6,3>() = -
unproject(v2First) * dNumerator / (dDenominator * dDenominator);
151 m29Jacobian[1].slice<0,3>() =
Zeros;
152 m29Jacobian[1].slice<3,3>() =
unproject(v2First) / dDenominator;;
153 dNumerator = v3Second[1];
154 m29Jacobian[1].slice<6,3>() = -
unproject(v2First) * dNumerator / (dDenominator * dDenominator);
156 vmJacobians.push_back(mvHomographyInliers[i].m2PixelProjectionJac * m29Jacobian);
160 vector<double> vdd = vdErrorSquared;
164 for(
unsigned int i=0; i<mvHomographyInliers.size(); i++)
166 double dWeight =
Tukey::Weight(vdErrorSquared[i], dSigmaSquared);
167 wls.
add_mJ(vvErrors[i][0], vmJacobians[i][0], dWeight);
168 wls.
add_mJ(vvErrors[i][1], vmJacobians[i][1], dWeight);
173 m3Update[0] = v9Update.slice<0,3>();
174 m3Update[1] = v9Update.slice<3,3>();
175 m3Update[2] = v9Update.slice<6,3>();
176 mm3BestHomography += m3Update;
182 if(mvMatches.size() < 10)
184 mm3BestHomography = HomographyFromMatches(mvMatches);
192 double dBestError = 999999999999999999.9;
195 for(
int nR = 0; nR < 300 ; nR++)
198 for(
int i=0; i<4; i++)
200 bool isUnique =
false;
204 n = rand() % mvMatches.size();
206 for(
int j=0; j<i && isUnique; j++)
207 if(anIndices[j] == n)
212 vector<HomographyMatch> vMinimalMatches;
213 for(
int i=0; i<4; i++)
214 vMinimalMatches.push_back(mvMatches[anIndices[i]]);
217 Matrix<3> m3Homography = HomographyFromMatches(vMinimalMatches);
221 for(
unsigned int i=0; i<mvMatches.size(); i++)
222 dError += MLESACScore(m3Homography, mvMatches[i]);
224 if(dError < dBestError)
226 mm3BestHomography = m3Homography;
234 mvDecompositions.clear();
235 SVD<3> svd(mm3BestHomography);
237 double d1 = fabs(v3Diag[0]);
238 double d2 = fabs(v3Diag[1]);
239 double d3 = fabs(v3Diag[2]);
246 double dPrime_PM = d2;
249 if(d1 != d2 && d2 != d3)
251 else if( d1 == d2 && d2 == d3)
258 cout <<
" Homographyinit: This motion case is not implemented or is degenerate. Try again. " << endl;
269 x1_PM = sqrt((d1*d1 - d2*d2) / (d1*d1 - d3*d3));
271 x3_PM = sqrt((d2*d2 - d3*d3) / (d1*d1 - d3*d3));
274 double e1[4] = {1.0,-1.0,1.0,-1.0};
275 double e3[4] = {1.0, 1.0, -1.0,-1.0};
281 decomposition.
d = s * dPrime_PM;
282 for(
int signs=0; signs<4; signs++)
286 double dSinTheta = (d1 - d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2;
287 double dCosTheta = (d1 * x3_PM * x3_PM + d3 * x1_PM * x1_PM) / d2;
288 decomposition.
m3Rp[0][0] = dCosTheta;
289 decomposition.
m3Rp[0][2] = -dSinTheta;
290 decomposition.
m3Rp[2][0] = dSinTheta;
291 decomposition.
m3Rp[2][2] = dCosTheta;
294 decomposition.
v3Tp[0] = (d1 - d3) * x1_PM * e1[signs];
295 decomposition.
v3Tp[1] = 0.0;
296 decomposition.
v3Tp[2] = (d1 - d3) * -x3_PM * e3[signs];
298 v3np[0] = x1_PM * e1[signs];
300 v3np[2] = x3_PM * e3[signs];
301 decomposition.
v3n = V * v3np;
303 mvDecompositions.push_back(decomposition);
306 decomposition.
d = s * -dPrime_PM;
307 for(
int signs=0; signs<4; signs++)
311 double dSinPhi = (d1 + d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2;
312 double dCosPhi = (d3 * x1_PM * x1_PM - d1 * x3_PM * x3_PM) / d2;
313 decomposition.
m3Rp[0][0] = dCosPhi;
314 decomposition.
m3Rp[0][2] = dSinPhi;
315 decomposition.
m3Rp[2][0] = dSinPhi;
316 decomposition.
m3Rp[2][2] = -dCosPhi;
319 decomposition.
v3Tp[0] = (d1 + d3) * x1_PM * e1[signs];
320 decomposition.
v3Tp[1] = 0.0;
321 decomposition.
v3Tp[2] = (d1 + d3) * x3_PM * e3[signs];
323 v3np[0] = x1_PM * e1[signs];
325 v3np[2] = x3_PM * e3[signs];
326 decomposition.
v3n = V * v3np;
328 mvDecompositions.push_back(decomposition);
332 for(
unsigned int i=0; i<mvDecompositions.size(); i++)
334 mvDecompositions[i].se3SecondFromFirst.get_rotation() =
335 s * U * mvDecompositions[i].m3Rp * V.T();
336 mvDecompositions[i].se3SecondFromFirst.get_translation() =
337 U * mvDecompositions[i].v3Tp;
351 double dError = v3Dash * m3Essential * v3;
354 Vector<3> fTv3Dash = m3Essential.T() * v3Dash;
357 Vector<2> fTv3DashSlice = fTv3Dash.slice<0,2>();
359 return (dError * dError / (fv3Slice * fv3Slice + fTv3DashSlice * fTv3DashSlice));
365 assert(mvDecompositions.size() == 8);
366 for(
unsigned int i=0; i<mvDecompositions.size(); i++)
370 for(
unsigned int m=0; m<mvHomographyInliers.size(); m++)
372 Vector<2> &v2 = mvHomographyInliers[m].v2CamPlaneFirst;
373 double dVisibilityTest = (mm3BestHomography[2][0] * v2[0] + mm3BestHomography[2][1] * v2[1] + mm3BestHomography[2][2]) / decom.
d;
374 if(dVisibilityTest > 0.0)
377 decom.
nScore = -nPositive;
380 sort(mvDecompositions.begin(), mvDecompositions.end());
381 mvDecompositions.resize(4);
383 for(
unsigned int i=0; i<mvDecompositions.size(); i++)
387 for(
unsigned int m=0; m<mvHomographyInliers.size(); m++)
390 double dVisibilityTest = v3 * decom.
v3n / decom.
d;
391 if(dVisibilityTest > 0.0)
394 decom.
nScore = -nPositive;
397 sort(mvDecompositions.begin(), mvDecompositions.end());
398 mvDecompositions.resize(2);
402 double dRatio = (double) mvDecompositions[1].nScore / (
double) mvDecompositions[0].nScore;
405 mvDecompositions.erase(mvDecompositions.begin() + 1);
408 double dErrorSquaredLimit = mdMaxPixelErrorSquared * 4;
409 double adSampsonusScores[2];
410 for(
int i=0; i<2; i++)
412 SE3<> se3 = mvDecompositions[i].se3SecondFromFirst;
414 for(
int j=0; j<3; j++)
417 double dSumError = 0;
418 for(
unsigned int m=0; m < mvMatches.size(); m++ )
420 double d =
SampsonusError(mvMatches[m].v2CamPlaneSecond, m3Essential, mvMatches[m].v2CamPlaneFirst);
421 if(d > dErrorSquaredLimit)
422 d = dErrorSquaredLimit;
426 adSampsonusScores[i] = dSumError;
429 if(adSampsonusScores[0] <= adSampsonusScores[1])
430 mvDecompositions.erase(mvDecompositions.begin() + 1);
432 mvDecompositions.erase(mvDecompositions.begin());
void RefineHomographyWithInliers()
bool Compute(std::vector< HomographyMatch > vMatches, double dMaxPixelError, SE3<> &se3SecondCameraPose)
SO3< Precision > & get_rotation()
Returns the rotation part of the transformation as a SO3.
static Operator< Internal::Identity< Internal::One > > Identity
Matrix< Rows, Min_Dim, Precision, Reference::RowMajor > get_U()
static double FindSigmaSquared(std::vector< double > &vdErrorSquared)
Vector<(Size==Dynamic?Dynamic:Size+1), Precision > unproject(const Vector< Size, Precision, Base > &v)
bool IsHomographyInlier(Matrix< 3 > m3Homography, HomographyMatch match)
void add_mJ(Precision m, const Vector< Size, Precision, B2 > &J, Precision weight=1)
Matrix< 3 > HomographyFromMatches(std::vector< HomographyMatch > vMatches)
double M3Det(Matrix< 3 > m)
static double Weight(double dErrorSquared, double dSigmaSquared)
Vector< Size, Precision > & get_mu()
Returns the update. With no prior, this is the result of .
Vector< Min_Dim, Precision > & get_diagonal()
Return the singular values as a vector.
void BestHomographyFromMatches_MLESAC()
Matrix< 2 > m2PixelProjectionJac
double MLESACScore(Matrix< 3 > m3Homography, HomographyMatch match)
void DecomposeHomography()
Vector< 3, Precision > & get_translation()
Returns the translation part of the transformation as a Vector.
Matrix< Min_Dim, Cols, Precision, Reference::RowMajor > get_VT()
Vector< 2 > v2CamPlaneFirst
void ChooseBestDecomposition()
static double SampsonusError(Vector< 2 > &v2Dash, const Matrix< 3 > &m3Essential, Vector< 2 > &v2)
void add_prior(Precision val)
static Operator< Internal::Zero > Zeros
Vector<(Size==Dynamic?Dynamic:Size-1), Precision > project(const Vector< Size, Precision, Base > &v)
bool operator<(const HomographyDecomposition lhs, const HomographyDecomposition rhs)
Vector< 2 > v2CamPlaneSecond