[Toon-members] tag/src fourpointpose.cpp

[Gerhard Reitmayr]

CVSROOT:        /cvsroot/toon
Module name:    tag
Changes by:     Gerhard Reitmayr <gerhard>      06/06/04 12:21:09

Modified files:
        src            : fourpointpose.cpp 

Log message:
        refactored common code to helper function

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/tag/src/fourpointpose.cpp?cvsroot=toon&r1=1.2&r2=1.3

Patches:
Index: fourpointpose.cpp
===================================================================
RCS file: /cvsroot/toon/tag/src/fourpointpose.cpp,v
retrieving revision 1.2
retrieving revision 1.3
diff -u -b -r1.2 -r1.3
--- fourpointpose.cpp   4 Jun 2006 12:09:19 -0000       1.2
+++ fourpointpose.cpp   4 Jun 2006 12:21:09 -0000       1.3
@@ -22,7 +22,7 @@
 }
 
 // helper function for @ref fourPointPose computing the coefficients of the A 
matrix
-static TooN::Vector<5> getACoeffs(double c1, double c2, double c3, double d1, 
double d2, double d3){
+TooN::Vector<5> getACoeffs(double c1, double c2, double c3, double d1, double 
d2, double d3){
     TooN::Vector<5> coeffs;
     // x^4 coefficient
     coeffs[4] = (c3*c3 - c1*c2*c3 - 4 + c1*c1 + c2*c2);
@@ -63,7 +63,7 @@
 }
 
 // helper function for @ref fourPointPose computing the coefficients of the B 
matrix
-static TooN::Vector<3> getBCoeffs( int i, int j, int k, int l, const 
TooN::Vector<5> & v4, const TooN::Vector<5> & v5 ){
+TooN::Vector<3> getBCoeffs( int i, int j, int k, int l, const TooN::Vector<5> 
& v4, const TooN::Vector<5> & v5 ){
     TooN::Vector<3> coeff;
     coeff[0] = v4[i]*v4[j] - v4[k]*v4[l];
     coeff[1] = v4[i]*v5[j] + v5[i]*v4[j] - (v4[k]*v5[l] + v5[k]*v4[l]);
@@ -71,8 +71,8 @@
     return coeff;
 }
 
-// this code is copied in the next function, modifications need to be made 
there too
-TooN::SE3 fourPointPose( const std::vector<TooN::Vector<3> > & points, const 
std::vector<TooN::Vector<3> > & pixels, bool & valid ){
+// contains the main computational part common to both of the high level 
functions
+static bool fourPointSolver ( const std::vector<TooN::Vector<3> > & points, 
std::vector<TooN::Vector<3> > & myPixels, TooN::Vector<6> & distances, 
std::vector<TooN::Vector<2> > & length){
     TooN::Matrix<5> A;
     TooN::Vector<5> v4, v5;
     TooN::Matrix<7,3> B;
@@ -80,15 +80,10 @@
     TooN::Vector<5> t;
     TooN::Matrix<3> vecsCamera, vecsWorld;
 
-    TooN::Vector<6> distances;
     TooN::Vector<6> angles;
 
-    double orientationTest = ((points[1] - points[0]) ^ (points[2] - 
points[0])) * (points[3] - points[0]);
-
     // normalising scales for angle computation in next loop
-    std::vector<TooN::Vector<3> > myPixels(4);
     for(unsigned int i = 0; i < 4; i++){
-        myPixels[i] = pixels[i];
         TooN::normalize(myPixels[i]);
     }
 
@@ -138,13 +133,27 @@
     if( xx < 0)
         xx *= -1;
 
-    valid = true;
-    std::vector<TooN::Vector<2> > length(4);
+    bool valid = true;
     double x = sqrt( xx );
     length[0] = (TooN::make_Vector, x, -x); // possible distances to point 0
     valid &= quadraticRoots( -x*angles[0], xx - distances[0], length[1] );
     valid &= quadraticRoots( -x*angles[1], xx - distances[1], length[2] );
     valid &= quadraticRoots( -x*angles[2], xx - distances[2], length[3] );
+    return valid;
+}
+
+TooN::SE3 fourPointPose( const std::vector<TooN::Vector<3> > & points, const 
std::vector<TooN::Vector<3> > & pixels, bool & valid ){
+    double orientationTest = ((points[1] - points[0]) ^ (points[2] - 
points[0])) * (points[3] - points[0]);
+
+    // normalising scales for angle computation in next loop
+    std::vector<TooN::Vector<3> > myPixels(4);
+    for(unsigned int i = 0; i < 4; i++)
+        myPixels[i] = pixels[i];
+
+    // resulting possible distances
+    std::vector<TooN::Vector<2> > length(4);
+    TooN::Vector<6> distances;
+    valid = fourPointSolver( points, myPixels, distances, length);
     if( !valid )
         return TooN::SE3();
 
@@ -191,77 +200,16 @@
 }
 
 // just a copy of the above code with modifications to the case search
-// could be refactored, but that breaks the nice algorithmic structure
 TooN::SE3 fourPointPoseFromCamera( const std::vector<TooN::Vector<3> > & 
points, const std::vector<TooN::Vector<3> > & pixels, bool & valid ){
-    TooN::Matrix<5> A;
-    TooN::Vector<5> v4, v5;
-    TooN::Matrix<7,3> B;
-    TooN::Vector<3> bnull;
-    TooN::Vector<5> t;
-    TooN::Matrix<3> vecsCamera, vecsWorld;
-
-    TooN::Vector<6> distances;
-    TooN::Vector<6> angles;
-
     // normalising scales for angle computation in next loop
     std::vector<TooN::Vector<3> > myPixels(4);
-    for(unsigned int i = 0; i < 4; i++){
+    for(unsigned int i = 0; i < 4; i++)
         myPixels[i] = pixels[i];
-        TooN::normalize(myPixels[i]);
-    }
-
-    int count = 0;
-    for( unsigned int i = 0; i < 3; i++)
-        for( unsigned int j = i+1; j < 4; j++, count++){
-            TooN::Vector<3> diff = points[i] - points[j];
-            distances[count] = diff * diff;
-            angles[count] = 2* myPixels[i] * myPixels[j];
-        }
-
-    TooN::Zero(A);
-    A.slice<0,0,1,5>() = getACoeffs(angles[0], angles[1], angles[3], 
distances[0], distances[1], distances[3]).as_row();
-    A.slice<1,0,1,5>() = getACoeffs(angles[0], angles[2], angles[4], 
distances[0], distances[2], distances[4]).as_row();
-    A.slice<2,0,1,5>() = getACoeffs(angles[1], angles[2], angles[5], 
distances[1], distances[2], distances[5]).as_row();
-    TooN::SVD<5> svdA(A);
 
-    v4.as_row() = svdA.get_VT().slice<3,0,1,5>();
-    v5.as_row() = svdA.get_VT().slice<4,0,1,5>();
-
-    B.slice<0,0,1,3>() = getBCoeffs(4,2,3,3, v4, v5).as_row();
-    B.slice<1,0,1,3>() = getBCoeffs(4,1,3,2, v4, v5).as_row();
-    B.slice<2,0,1,3>() = getBCoeffs(4,0,3,1, v4, v5).as_row();
-    B.slice<3,0,1,3>() = getBCoeffs(4,0,2,2, v4, v5).as_row();
-    B.slice<4,0,1,3>() = getBCoeffs(3,1,2,2, v4, v5).as_row();
-    B.slice<5,0,1,3>() = getBCoeffs(3,0,2,1, v4, v5).as_row();
-    B.slice<6,0,1,3>() = getBCoeffs(2,0,1,1, v4, v5).as_row();
-
-    TooN::SVD<7,3> svdB(B);
-    bnull.as_row() = svdB.get_VT().slice<2,0,1,3>();
-
-    double lambda, roh;
-    if( bnull[1] != 0 ){          // lambda * roh != 0 => both are != 0
-        double ratio = (bnull[0]/ bnull[1] + bnull[1] / bnull[2]) * 0.5;
-        roh = 1.0 / (v4[0] * ratio + v5[0]);
-        lambda = ratio * roh;
-    } else if( bnull[2] != 0 ) { // roh != 0 => lambda == 0
-        lambda = 0;
-        roh = 1/v5[0];
-    } else {                     // lambda != 0 and roh == 0
-        roh = 0;
-        lambda = 1/v4[0];
-    }
-
-    t = v4 * lambda + v5 * roh;
-    double xx = (t[1]/t[0] + t[2]/t[1] + t[3]/t[2] + t[4]/t[3]) / 4;
-    if( xx < 0)
-        xx *= -1;
-
-    valid = true;
+    // resulting possible distances
     std::vector<TooN::Vector<2> > length(4);
-    double x = sqrt( xx );
-    valid &= quadraticRoots( -x*angles[0], xx - distances[0], length[1] );
-    valid &= quadraticRoots( -x*angles[1], xx - distances[1], length[2] );
-    valid &= quadraticRoots( -x*angles[2], xx - distances[2], length[3] );
+    TooN::Vector<6> distances;
+    valid = fourPointSolver( points, myPixels, distances, length);
     if( !valid )
         return TooN::SE3();
 
@@ -271,8 +219,8 @@
     double minError = 1e10;
     int minIndex = -1;
     // we assume now that the point is in front of the camera and therefore 
the solution is the one with
-    // positive x
-    testPoint[0] = myPixels[0] * x;
+    // positive x (which is the first in the vector)
+    testPoint[0] = myPixels[0] * length[0][0];
     for(unsigned int h = 0; h < 8; h++){
         testPoint[1] = myPixels[1] * length[1][!!(h&1)];
         testPoint[2] = myPixels[2] * length[2][!!(h&2)];