toon-members
[Top][All Lists]

[Toon-members] tag/src fourpointpose.cpp

 From: Gerhard Reitmayr Subject: [Toon-members] tag/src fourpointpose.cpp Date: Mon, 04 Sep 2006 16:23:01 +0000

```CVSROOT:        /cvsroot/toon
Module name:    tag
Changes by:     Gerhard Reitmayr <gerhard>      06/09/04 16:23:01

Modified files:
src            : fourpointpose.cpp

Log message:
refactored checks, avoids unnecessary use of acos, asin

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

Patches:
Index: fourpointpose.cpp
===================================================================
RCS file: /cvsroot/toon/tag/src/fourpointpose.cpp,v
retrieving revision 1.5
retrieving revision 1.6
diff -u -b -r1.5 -r1.6
--- fourpointpose.cpp   4 Sep 2006 15:50:04 -0000       1.5
+++ fourpointpose.cpp   4 Sep 2006 16:23:01 -0000       1.6
@@ -8,11 +8,15 @@

namespace tag {

-// 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 bool quadraticRoots( const double p, const double q,
TooN::Vector<2> & roots, const double alpha, const double beta, const double
angularError ){
+// simple helper function to get the two possible ray lengths given a known
point, and the distance to the other point
+// does various checks to account for numerical inaccuracies
+static inline bool computeDistances( const double xx, const double x, const
double angle, const double distance, const double angularError, TooN::Vector<2>
& roots){
+    double p = -x*angle;
+    double q = xx - distance;
double det = p*p*0.25 - q;
if( det < 0 ){
+        double alpha = std::acos(0.5 * angle);
+        double beta = std::asin(sqrt(distance) / x);
double diff = fabs(alpha) - fabs(beta);
if( diff > angularError ){
TooN::Zero(roots);
@@ -77,8 +81,6 @@
}

// contains the main computational part common to both of the high level
functions
-template<class T>
-inline T ABS(T x){return (x>0?x:-x);}
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, const double angularError){
TooN::Matrix<5> A;
TooN::Vector<5> v4, v5;
@@ -142,25 +144,10 @@

bool valid = true;
double x = sqrt( xx );
-       double x_inv = 1.0/x;
length[0] = (TooN::make_Vector, x, -x); // possible distances to point 0
-    //std::cout << (angles[0]/2) << "   " << (sqrt(distances[0])/x) <<
std::endl;
-       if(  distances[0] > xx )
-       valid &= quadraticRoots( -x*angles[0], xx - distances[0], length[1], 0,
0, angularError );
-    else
-       valid &= quadraticRoots( -x*angles[0], xx - distances[0], length[1],
std::acos(0.5*angles[0]), asin(sqrt(distances[0])*x_inv), angularError );
-
-
-       if(  distances[1] > xx )
-       valid &= quadraticRoots( -x*angles[1], xx - distances[1], length[2], 0,
0, angularError );
-    else
-       valid &= quadraticRoots( -x*angles[1], xx - distances[1], length[2],
std::acos(0.5*angles[1]), asin(sqrt(distances[1])*x_inv), angularError );
-
-       if(  distances[2] > xx )
-       valid &= quadraticRoots( -x*angles[2], xx - distances[2], length[3], 0,
0, angularError );
-    else
-       valid &= quadraticRoots( -x*angles[2], xx - distances[2], length[3],
std::acos(0.5*angles[2]), asin(sqrt(distances[2])*x_inv), angularError );
-
+    valid &= computeDistances( xx, x, angles[0], distances[0], angularError,
length[1]);
+    valid &= computeDistances( xx, x, angles[1], distances[1], angularError,
length[2]);
+    valid &= computeDistances( xx, x, angles[2], distances[2], angularError,
length[3]);
return valid;
}

```