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

[Edward Rosten]

CVSROOT:        /cvsroot/toon
Module name:    tag
Changes by:     Edward Rosten <edrosten>        09/08/25 08:46:01

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

Log message:
        TooN-2 updates to four point pose.

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

Patches:
Index: tag/fourpointpose.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/fourpointpose.h,v
retrieving revision 1.9
retrieving revision 1.10
diff -u -b -r1.9 -r1.10
--- tag/fourpointpose.h 20 Apr 2009 10:02:35 -0000      1.9
+++ tag/fourpointpose.h 25 Aug 2009 08:46:01 -0000      1.10
@@ -27,7 +27,7 @@
 ///            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, const double angularError 
= 0.14 );
+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).
@@ -43,7 +43,7 @@
 ///            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, const 
double angularError = 0.14 );
+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
@@ -59,7 +59,7 @@
 template <int ImagePlaneZ = 1>
 struct Point4SE3Estimation {
     /// SE3 describing the transformation from world to camera coordinate frame
-    TooN::SE3 T;
+    TooN::SE3<> T;
     /// was the estimation valid
     bool valid;
     /// angular error to accept in the 4 point pose estimation

Index: src/fourpointpose.cpp
===================================================================
RCS file: /cvsroot/toon/tag/src/fourpointpose.cpp,v
retrieving revision 1.6
retrieving revision 1.7
diff -u -b -r1.6 -r1.7
--- src/fourpointpose.cpp       4 Sep 2006 16:23:01 -0000       1.6
+++ src/fourpointpose.cpp       25 Aug 2009 08:46:01 -0000      1.7
@@ -19,14 +19,14 @@
         double beta = std::asin(sqrt(distance) / x);
         double diff = fabs(alpha) - fabs(beta);
         if( diff > angularError ){
-            TooN::Zero(roots);
+                       roots = TooN::Zeros;
             return false;
         }
         det = 0;
     } else {
         det = sqrt(det);
     }
-    roots = (TooN::make_Vector, -p*0.5 + det, -p*0.5 - det);
+    roots = TooN::makeVector( -p*0.5 + det, -p*0.5 - det);
     return true;
 }
 
@@ -104,7 +104,7 @@
             angles[count] = 2* myPixels[i] * myPixels[j];
         }
 
-    TooN::Zero(A);
+       A = TooN::Zeros;
     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();
@@ -144,14 +144,14 @@
 
     bool valid = true;
     double x = sqrt( xx );
-    length[0] = (TooN::make_Vector, x, -x); // possible distances to point 0
+    length[0] = TooN::makeVector( x, -x); // possible distances to point 0
     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;
 }
 
-TooN::SE3 fourPointPose( const std::vector<TooN::Vector<3> > & points, const 
std::vector<TooN::Vector<3> > & pixels, bool & valid, const double angularError 
){
+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
@@ -164,7 +164,7 @@
     TooN::Vector<6> distances;
     valid = fourPointSolver( points, myPixels, distances, length, 
angularError);
     if( !valid )
-        return TooN::SE3();
+        return TooN::SE3<>();
 
     // figure out the right lengths
     // brute force through all combinations, with some optimizations to stop 
early, if a better hypothesis exists
@@ -195,7 +195,7 @@
     }
     if(minIndex == -1){
         valid = false;
-        return TooN::SE3();
+        return TooN::SE3<>();
     }
 
     // pixel directions extended to the right distance
@@ -209,7 +209,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, const 
double angularError ){
+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++)
@@ -220,7 +220,7 @@
     TooN::Vector<6> distances;
     valid = fourPointSolver( points, myPixels, distances, length, 
angularError);
     if( !valid )
-        return TooN::SE3();
+        return TooN::SE3<>();
 
     // figure out the right lengths
     // brute force through all combinations, with some optimizations to stop 
early, if a better hypothesis exists
@@ -248,7 +248,7 @@
     }
     if(minIndex == -1){
         valid = false;
-        return TooN::SE3();
+        return TooN::SE3<>();
     }
 
     // pixel directions extended to the right distance