size_t Homography::
computeMatchesInliers()
{
inliers.clear(); inliers.resize(fts_c1.size());
size_t n_inliers = 0;
for(size_t i=0; i<fts_c1.size(); i++)
{
Vector2d projected = project2d(H_c2_from_c1 * unproject2d(fts_c1[i]));
Vector2d e = fts_c2[i] - projected;
double e_px = error_multiplier2 * e.norm();
inliers[i] = (e_px < thresh);
n_inliers += inliers[i];
}
return n_inliers;
}
inline Vector3d invert_depth(const Vector3d & x) {
return unproject2d(x.head<2>())/x[2];
}
void Homography::
findBestDecomposition()
{
assert(decompositions.size() == 8);
for(size_t i=0; i<decompositions.size(); i++)
{
HomographyDecomposition &decom = decompositions[i];
size_t nPositive = 0;
for(size_t m=0; m<fts_c1.size(); m++)
{
if(!inliers[m])
continue;
const Vector2d& v2 = fts_c1[m];
double dVisibilityTest = (H_c2_from_c1(2,0) * v2[0] + H_c2_from_c1(2,1) * v2[1] + H_c2_from_c1(2,2)) / decom.d;
if(dVisibilityTest > 0.0)
nPositive++;
}
decom.score = -nPositive;
}
sort(decompositions.begin(), decompositions.end());
decompositions.resize(4);
for(size_t i=0; i<decompositions.size(); i++)
{
HomographyDecomposition &decom = decompositions[i];
int nPositive = 0;
for(size_t m=0; m<fts_c1.size(); m++)
{
if(!inliers[m])
continue;
Vector3d v3 = unproject2d(fts_c1[m]);
double dVisibilityTest = v3.dot(decom.n) / decom.d;
if(dVisibilityTest > 0.0)
nPositive++;
};
decom.score = -nPositive;
}
sort(decompositions.begin(), decompositions.end());
decompositions.resize(2);
// According to Faugeras and Lustman, ambiguity exists if the two scores are equal
// but in practive, better to look at the ratio!
double dRatio = (double) decompositions[1].score / (double) decompositions[0].score;
if(dRatio < 0.9) // no ambiguity!
decompositions.erase(decompositions.begin() + 1);
else // two-way ambiguity. Resolve by sampsonus score of all points.
{
double dErrorSquaredLimit = thresh * thresh * 4;
double adSampsonusScores[2];
for(size_t i=0; i<2; i++)
{
Sophus::SE3 T = decompositions[i].T;
Matrix3d Essential = T.rotationMatrix() * sqew(T.translation());
double dSumError = 0;
for(size_t m=0; m < fts_c1.size(); m++ )
{
double d = sampsonusError(fts_c1[m], Essential, fts_c2[m]);
if(d > dErrorSquaredLimit)
d = dErrorSquaredLimit;
dSumError += d;
}
adSampsonusScores[i] = dSumError;
}
if(adSampsonusScores[0] <= adSampsonusScores[1])
decompositions.erase(decompositions.begin() + 1);
else
decompositions.erase(decompositions.begin());
}
}
