int main() {
double first;
int i;
printf("Clock sqew test\n");
for ( i = 0; i < 30; i++){
first = sqew();
printf("sqew %f \n",first);
sleep(few_seconds);
}
by_now();
}
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());
}
}
