toon-members
[Top][All Lists]

## [Toon-members] tag src/fourpointpose.cpp tag/fourpointpose.h

 From: Gerhard Reitmayr Subject: [Toon-members] tag src/fourpointpose.cpp tag/fourpointpose.h Date: Wed, 26 Jul 2006 17:08:07 +0000

```CVSROOT:        /cvsroot/toon
Module name:    tag
Changes by:     Gerhard Reitmayr <gerhard>      06/07/26 17:08:07

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

Log message:
added a tolerance value to four point pose estimation to come up with
an estimation under measurement errors

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

Patches:
Index: src/fourpointpose.cpp
===================================================================
RCS file: /cvsroot/toon/tag/src/fourpointpose.cpp,v
retrieving revision 1.3
retrieving revision 1.4
diff -u -b -r1.3 -r1.4
--- src/fourpointpose.cpp       4 Jun 2006 12:21:09 -0000       1.3
+++ src/fourpointpose.cpp       26 Jul 2006 17:08:07 -0000      1.4
@@ -10,19 +10,24 @@

// 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( double p, double q, TooN::Vector<2> & roots
){
+static inline bool quadraticRoots( const double p, const double q,
TooN::Vector<2> & roots, const double alpha, const double beta, const double
angularError ){
double det = p*p*0.25 - q;
if( det < 0 ){
+        double diff = fabs(alpha) - fabs(beta);
+        if( diff > angularError ){
TooN::Zero(roots);
return false;
}
+        det = 0;
+    } else {
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
-TooN::Vector<5> getACoeffs(double c1, double c2, double c3, double d1, double
d2, double d3){
+static inline 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 +68,7 @@
}

// helper function for @ref fourPointPose computing the coefficients of the B
matrix
-TooN::Vector<3> getBCoeffs( int i, int j, int k, int l, const TooN::Vector<5>
& v4, const TooN::Vector<5> & v5 ){
+static inline 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]);
@@ -72,7 +77,7 @@
}

// 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){
+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;
TooN::Matrix<7,3> B;
@@ -136,13 +141,13 @@
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] );
+    valid &= quadraticRoots( -x*angles[0], xx - distances[0], length[1],
std::acos(angles[0]/2), asin(sqrt(distances[0])/x), angularError );
+    valid &= quadraticRoots( -x*angles[1], xx - distances[1], length[2],
std::acos(angles[1]/2), asin(sqrt(distances[1])/x), angularError );
+    valid &= quadraticRoots( -x*angles[2], xx - distances[2], length[3],
std::acos(angles[2]/2), asin(sqrt(distances[2])/x), angularError );
return valid;
}

-TooN::SE3 fourPointPose( const std::vector<TooN::Vector<3> > & points, const
std::vector<TooN::Vector<3> > & pixels, bool & valid ){
+TooN::SE3 fourPointPose( const std::vector<TooN::Vector<3> > & points, const
std::vector<TooN::Vector<3> > & pixels, bool & valid, const double angularError
){
double orientationTest = ((points[1] - points[0]) ^ (points[2] -
points[0])) * (points[3] - points[0]);

// normalising scales for angle computation in next loop
@@ -153,7 +158,7 @@
// resulting possible distances
std::vector<TooN::Vector<2> > length(4);
TooN::Vector<6> distances;
-    valid = fourPointSolver( points, myPixels, distances, length);
+    valid = fourPointSolver( points, myPixels, distances, length,
angularError);
if( !valid )

@@ -200,7 +205,7 @@
}

// just a copy of the above code with modifications to the case search
-TooN::SE3 fourPointPoseFromCamera( const std::vector<TooN::Vector<3> > &
points, const std::vector<TooN::Vector<3> > & pixels, bool & valid ){
+TooN::SE3 fourPointPoseFromCamera( const std::vector<TooN::Vector<3> > &
points, const std::vector<TooN::Vector<3> > & pixels, bool & valid, const
double angularError ){
// normalising scales for angle computation in next loop
std::vector<TooN::Vector<3> > myPixels(4);
for(unsigned int i = 0; i < 4; i++)
@@ -209,7 +214,7 @@
// resulting possible distances
std::vector<TooN::Vector<2> > length(4);
TooN::Vector<6> distances;
-    valid = fourPointSolver( points, myPixels, distances, length);
+    valid = fourPointSolver( points, myPixels, distances, length,
angularError);
if( !valid )

Index: tag/fourpointpose.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/fourpointpose.h,v
retrieving revision 1.4
retrieving revision 1.5
diff -u -b -r1.4 -r1.5
--- tag/fourpointpose.h 13 Jun 2006 17:19:27 -0000      1.4
+++ tag/fourpointpose.h 26 Jul 2006 17:08:07 -0000      1.5
@@ -23,9 +23,11 @@
/// @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
+/// @param[in] angularError maximal angular error that will be tolerated
before no result can be computed. The default value
+///            corresponds to 90 deg VOW over 640 pixels.
/// @return SE3 describing the camera pose
/// @ingroup fourpointpose
-TooN::SE3 fourPointPose( const std::vector<TooN::Vector<3> > & points, const
std::vector<TooN::Vector<3> > & pixels, bool & valid );
+TooN::SE3 fourPointPose( const std::vector<TooN::Vector<3> > & points, const
std::vector<TooN::Vector<3> > & pixels, bool & valid, const double angularError
= 0.14 );

/// 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).
@@ -37,9 +39,11 @@
/// @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
+/// @param[in] angularError maximal angular error that will be tolerated
before no result can be computed. The default value
+///            corresponds to 90 deg VOW over 640 pixels.
/// @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 );
+TooN::SE3 fourPointPoseFromCamera( const std::vector<TooN::Vector<3> > &
points, const std::vector<TooN::Vector<3> > & pixels, bool & valid, const
double angularError = 0.14 );

/// A RANSAC estimator using the @ref fourPointPose function. The
/// Correspondence datatype must provide a member position for the 3D point and
@@ -56,8 +60,9 @@
struct Point4SE3Estimation {
TooN::SE3 T;
bool valid;
+    double angularError;

-    inline Point4SE3Estimation() : valid(false) {  }
+    inline Point4SE3Estimation(double ang = 0.14) : valid(false),
angularError(ang) {  }

template<class It> inline bool estimate(It begin, It end) {
assert(end - begin >= 4);
@@ -71,7 +76,7 @@
pixels[i][2] *= ImagePlaneZ;
points[i] = match->position;
}
-        T = fourPointPoseFromCamera( points, pixels, valid );
+        T = fourPointPoseFromCamera( points, pixels, valid, angularError );
return valid;
}

```