[Toon-members] tag srcfourpointpose.cpp tag/fourpointpose.h

[Gerhard Reitmayr]

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

Modified files:
        src            : fourpointpose.cpp 
        tag            : fourpointpose.h 

Log message:
        fixed four point pose estimation

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

Patches:
Index: src/fourpointpose.cpp
===================================================================
RCS file: /cvsroot/toon/tag/src/fourpointpose.cpp,v
retrieving revision 1.1
retrieving revision 1.2
diff -u -b -r1.1 -r1.2
--- src/fourpointpose.cpp       29 May 2006 13:54:03 -0000      1.1
+++ src/fourpointpose.cpp       4 Jun 2006 12:09:19 -0000       1.2
@@ -4,17 +4,21 @@
 #include <TooN/SVD.h>
 #include <TooN/helpers.h>
 
+#include <iostream>
+
 namespace tag {
 
-// simple helper function to compute larger of a quadratic equation x^2 + px + 
q = 0
+// simple helper function to compute the roots of a quadratic equation x^2 + 
px + q = 0
 // also returns if the result is real or not (in which case valid is set to 
false)
-static inline double quadraticRoots( double p, double q, bool & valid ){
+static inline bool quadraticRoots( double p, double q, TooN::Vector<2> & roots 
){
     double det = p*p*0.25 - q;
     if( det < 0 ){
-        valid = false;
-        return 0;
+        TooN::Zero(roots);
+        return false;
     }
-    return -p*0.5 + sqrt(det);
+    det = sqrt(det);
+    roots = (TooN::make_Vector, -p*0.5 + det, -p*0.5 - det);
+    return true;
 }
 
 // helper function for @ref fourPointPose computing the coefficients of the A 
matrix
@@ -67,18 +71,20 @@
     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 ){
     TooN::Matrix<5> A;
     TooN::Vector<5> v4, v5;
     TooN::Matrix<7,3> B;
     TooN::Vector<3> bnull;
     TooN::Vector<5> t;
-    TooN::Vector<4> x;
     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++){
@@ -89,8 +95,6 @@
     int count = 0;
     for( unsigned int i = 0; i < 3; i++)
         for( unsigned int j = i+1; j < 4; j++, count++){
-            if(i == j)
-                continue;
             TooN::Vector<3> diff = points[i] - points[j];
             distances[count] = diff * diff;
             angles[count] = 2* myPixels[i] * myPixels[j];
@@ -135,21 +139,169 @@
         xx *= -1;
 
     valid = true;
-    x[0] = sqrt( xx );  // distance to point 0
-    x[1] = quadraticRoots( -x[0]*angles[0], xx - distances[0], valid );
-    x[2] = quadraticRoots( -x[0]*angles[1], xx - distances[1], valid );
-    x[3] = quadraticRoots( -x[0]*angles[2], xx - distances[2], valid );
-
+    std::vector<TooN::Vector<2> > length(4);
+    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] );
     if( !valid )
         return TooN::SE3();
 
+    // figure out the right lengths
+    // brute force through all combinations, with some optimizations to stop 
early, if a better hypothesis exists
+    std::vector<TooN::Vector<3> > testPoint(4);
+    double minError = 1e10;
+    int minIndex = -1;
+    for(unsigned int h = 0; h < 16; h++){
+        // first going through all possibilities where the point is in front 
of the camera
+        // then switch to the other ones.
+        testPoint[0] = myPixels[0] * length[0][!!(h&8)];
+        testPoint[1] = myPixels[1] * length[1][!!(h&1)] * (h&8?-1:1);
+        testPoint[2] = myPixels[2] * length[2][!!(h&2)] * (h&8?-1:1);
+        testPoint[3] = myPixels[3] * length[3][!!(h&4)] * (h&8?-1:1);
+        double error = 0;
+        int count = 3; // skip the first 3 distances because they will not 
produce any error
+        for( unsigned int i = 1; i < 3; i++)
+            for( unsigned int j = i+1; j < 4; j++, count++){
+                TooN::Vector<3> diff = testPoint[i] - testPoint[j];
+                error += fabs(diff * diff - distances[count]);
+            }
+        if( minError > error ){
+            double myOrientationTest = ((testPoint[1] - testPoint[0]) ^ 
(testPoint[2] - testPoint[0])) * (testPoint[3] - testPoint[0]);
+            if(myOrientationTest * orientationTest > 0){
+                minError = error;
+                minIndex = h;
+            }
+        }
+    }
+    if(minIndex == -1){
+        valid = false;
+        return TooN::SE3();
+    }
+
     // pixel directions extended to the right distance
+    myPixels[0] *= length[0][!!(minIndex&8)];
+    myPixels[1] *= length[1][!!(minIndex&1)] * (minIndex&8?-1:1);
+    myPixels[2] *= length[2][!!(minIndex&2)] * (minIndex&8?-1:1);
+    myPixels[3] *= length[3][!!(minIndex&4)] * (minIndex&8?-1:1);
+
+    // absolute orientation
+    return computeAbsoluteOrientation(points, myPixels);
+}
+
+// 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++){
-        myPixels[i] *= x[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;
+    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] );
+    if( !valid )
+        return TooN::SE3();
+
+    // figure out the right lengths
+    // brute force through all combinations, with some optimizations to stop 
early, if a better hypothesis exists
+    std::vector<TooN::Vector<3> > testPoint(4);
+    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;
+    for(unsigned int h = 0; h < 8; h++){
+        testPoint[1] = myPixels[1] * length[1][!!(h&1)];
+        testPoint[2] = myPixels[2] * length[2][!!(h&2)];
+        testPoint[3] = myPixels[3] * length[3][!!(h&4)];
+        double error = 0;
+        int count = 3; // skip the first 3 distances because they will not 
produce any error
+        for( unsigned int i = 1; i < 3; i++)
+            for( unsigned int j = i+1; j < 4; j++, count++){
+                TooN::Vector<3> diff = testPoint[i] - testPoint[j];
+                error += fabs(diff * diff - distances[count]);
+            }
+        if( minError > error ){
+            minError = error;
+            minIndex = h;
+        }
+    }
+    if(minIndex == -1){
+        valid = false;
+        return TooN::SE3();
+    }
+
+    // pixel directions extended to the right distance
+    myPixels[0] = testPoint[0];
+    myPixels[1] *= length[1][!!(minIndex&1)];
+    myPixels[2] *= length[2][!!(minIndex&2)];
+    myPixels[3] *= length[3][!!(minIndex&4)];
+
     // absolute orientation
-    return computeAbsoluteOrientation(points, pixels);
+    return computeAbsoluteOrientation(points, myPixels);
 }
 
 }

Index: tag/fourpointpose.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/fourpointpose.h,v
retrieving revision 1.2
retrieving revision 1.3
diff -u -b -r1.2 -r1.3
--- tag/fourpointpose.h 31 May 2006 16:46:20 -0000      1.2
+++ tag/fourpointpose.h 4 Jun 2006 12:09:19 -0000       1.3
@@ -14,7 +14,11 @@
 
 /// The main function for pose estimation from 4 2D - 3D point correspondences.
 /// It implements the algorithm given by
-/// Input is a list of 3D positions and a list of 3D vectors describing the 
pixels on the image plane.
+/// This function assumes that the 4 points are in general position (not in a 
single plane) and
+/// can solve the problem for all cases, e.g. points do not have to lie in 
front of the camera etc.
+/// Input is a list of 3D positions and a list of 3D vectors describing the 
ray under which the transformed points are
+/// seen by the origin. It does not assume any special camera setup other than 
the origin being the center of the
+/// camera.
 /// Ouput is the SE3 describing the camera pose and a flag signaling if the 
result is valid.
 /// @param[in] points a vector containing 4 3D points
 /// @param[in] pixels a vector containing 4 2D pixels as 3D vectors to allow 
arbitrary image planes
@@ -23,6 +27,20 @@
 /// @ingroup fourpointpose
 TooN::SE3 fourPointPose( const std::vector<TooN::Vector<3> > & points, const 
std::vector<TooN::Vector<3> > & pixels, bool & valid );
 
+/// A special case of the general @ref fourPointPose function which assumes 
that points are
+/// in front of a given camera plane but now may also lie in the same plane 
(but not all on one line).
+/// The frontal assumption is made for the data, but not encoded in any 
constraint. Care should be taken
+/// that the given 3D vectors for the directions are actually pointing towards 
the points, not away from them.
+/// In the general case, all points in a plane has two solutions, if we assume 
that all points are
+/// in front of the camera, there is only one solution. It also is slighlty 
more optimized, because
+/// it does not need to check as many cases.
+/// @param[in] points a vector containing 4 3D points
+/// @param[in] pixels a vector containing 4 2D pixels as 3D vectors to allow 
arbitrary image planes
+/// @param[out] valid output argument, it is set to true to signal a valid 
result and false otherwise
+/// @return SE3 describing the camera pose
+/// @ingroup fourpointpose
+TooN::SE3 fourPointPoseFromCamera( const std::vector<TooN::Vector<3> > & 
points, const std::vector<TooN::Vector<3> > & pixels, bool & valid );
+
 /// A RANSAC estimator using the @ref fourPointPose function. The
 /// Correspondence datatype must provide a member position for the 3D point and
 /// a member pixel for the 2D pixel.