[Toon-members] tag src/absorient.cpp src/five_point.cpp test/f...

[Gerhard Reitmayr]

CVSROOT:        /cvsroot/toon
Module name:    tag
Changes by:     Gerhard Reitmayr <gerhard>      09/04/28 10:36:41

Modified files:
        src            : absorient.cpp five_point.cpp 
        test           : fivepoint.cpp 
        tag            : five_point.h 

Log message:
        essential matrix optimization code for more then 5 measurements

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/tag/src/absorient.cpp?cvsroot=toon&r1=1.6&r2=1.7
http://cvs.savannah.gnu.org/viewcvs/tag/src/five_point.cpp?cvsroot=toon&r1=1.12&r2=1.13
http://cvs.savannah.gnu.org/viewcvs/tag/test/fivepoint.cpp?cvsroot=toon&r1=1.1&r2=1.2
http://cvs.savannah.gnu.org/viewcvs/tag/tag/five_point.h?cvsroot=toon&r1=1.3&r2=1.4

Patches:
Index: src/absorient.cpp
===================================================================
RCS file: /cvsroot/toon/tag/src/absorient.cpp,v
retrieving revision 1.6
retrieving revision 1.7
diff -u -b -r1.6 -r1.7
--- src/absorient.cpp   27 Apr 2009 13:45:43 -0000      1.6
+++ src/absorient.cpp   28 Apr 2009 10:36:41 -0000      1.7
@@ -60,10 +60,9 @@
 // computes the orientation from (e1,e2,e3) -> (a,(a^b)^a,a^b), which means 
that b the second vector is in the a, b plane
 static inline TooN::SO3<>  canonicalOrientation( const TooN::Vector<3> & a, 
const TooN::Vector<3> & b ){
     TooN::Matrix<3> result;
-    result.T()[0] = a;
-    result.T()[2] = a ^ b;
+    result.T()[0] = unit(a);
+    result.T()[2] = unit(a ^ b);
     result.T()[1] = result.T()[2] ^ result.T()[0];
-    // normalization happens in SO3 constructor as part of coerce anyway
     return TooN::SO3<> (result);
 }
 

Index: src/five_point.cpp
===================================================================
RCS file: /cvsroot/toon/tag/src/five_point.cpp,v
retrieving revision 1.12
retrieving revision 1.13
diff -u -b -r1.12 -r1.13
--- src/five_point.cpp  27 Apr 2009 13:45:43 -0000      1.12
+++ src/five_point.cpp  28 Apr 2009 10:36:41 -0000      1.13
@@ -1,6 +1,7 @@
 #include <tag/five_point.h>
 #include <tag/stdpp.h>
 #include <tag/helpers.h>
+#include <tag/absorient.h>
 
 #include <TooN/helpers.h>
 #include <TooN/gauss_jordan.h>
@@ -114,7 +115,7 @@
        return Matrix<3, 3, double, Reference::RowMajor>(&v[0]);
 }
 
-vector<Matrix<3> > five_point(array<pair<Vector<3>, Vector<3> >, 5> points)
+vector<Matrix<3> > five_point(const array<pair<Vector<3>, Vector<3> >, 5> & 
points)
 {
        //Equations numbers are given with reference to:
        // "An efficient Solution to the Five-Point Relative Pose Problem",
@@ -289,4 +290,47 @@
        return SE3s;
 }
 
+
+TooN::SE3<> optimize_epipolar(const std::vector<std::pair<TooN::Vector<3>, 
TooN::Vector<3> > > & points, const TooN::SE3<> & initial){
+       static const Vector<3> X = makeVector(1,0,0);
+
+       // We represent E = [t]x R with t = Rn * [ 1 0 0 ]' and R = Rt
+       // The parameterisation of the optimization is then
+       // 3 parameters for Rt with left multiplication as update
+       // 2 parameters for Rn with _right_ multiplication as update !!
+       //   This allows as to have a minimal parameterization as 
+       //   Rn * G_0 * [1 0 0]' = 0
+       SO3<> Rt = initial.get_rotation();
+       SO3<> Rn; // Identity 
+       Vector<3> normal = X ^ unit(initial.get_translation());
+       // if the initial translation differs from X enough, then 
+       // compute a rotation in Rn that takes X -> initial translation
+       if(norm_sq(normal) > 1e-15)
+               Rn = computeOrientation(X, initial.get_translation(), normal, 
normal);
+
+       int count = 0;
+       WLS<5> wls;
+       do {
+               wls.clear();
+               const Matrix<3> C = getCrossProductMatrix(Rn * X);
+               double error = 0;
+               for(unsigned i = 0; i < points.size(); ++i){
+                       Vector<5> J;
+                       J[0] = (points[i].second * C) * Rt.generator(0) * (Rt * 
points[i].first);
+                       J[1] = (points[i].second * C) * Rt.generator(1) * (Rt * 
points[i].first);
+                       J[2] = (points[i].second * C) * Rt.generator(2) * (Rt * 
points[i].first);
+                       J[3] = points[i].second * getCrossProductMatrix(Rn * 
Rn.generator(1) * X) * (Rt * points[i].first);
+                       J[4] = points[i].second * getCrossProductMatrix(Rn * 
Rn.generator(2) * X) * (Rt * points[i].first);
+                       const double e = 0 - (points[i].second * C) * (Rt * 
points[i].first);
+                       wls.add_mJ(e, J);
+                       error += e*e;
+               }
+               wls.compute();
+               Rt = SO3<>::exp(wls.get_mu().slice<0,3>()) * Rt;
+               Rn = Rn * SO3<>::exp(makeVector(0, wls.get_mu()[3], 
wls.get_mu()[4]));
+               ++count;
+       } while(norm_sq(wls.get_mu()) > 1e-10 && count < 10);
+       return SE3<>(Rt, Rn * X);
+}
+
 }

Index: test/fivepoint.cpp
===================================================================
RCS file: /cvsroot/toon/tag/test/fivepoint.cpp,v
retrieving revision 1.1
retrieving revision 1.2
diff -u -b -r1.1 -r1.2
--- test/fivepoint.cpp  23 Apr 2009 15:06:21 -0000      1.1
+++ test/fivepoint.cpp  28 Apr 2009 10:36:41 -0000      1.2
@@ -123,9 +123,38 @@
     cout << "average error " << error / ITERATIONS << endl;
 }
 
+void testPerturbation(){
+    Vector<6> params;
+
+    srand(0); // same seed always
+
+    double error = 0;
+
+    const int ITERATIONS = 1000000;
+    const double scale = 1;
+    const double sigma = 0.1;
+
+    //ofstream diffile("diffs.txt");
+
+    const SE3<> start(makeVector(1,0,0,0,0,0));
+
+    for(int i = 0; i < ITERATIONS; ++i){
+        params = scale * makeVector(rand_u(), rand_u(), rand_u(), rand_u(), 
rand_u(), rand_u()); 
+        SE3<> T(params);
+        
+        vector<pair<Vector<3>, Vector<3> > > data;
+        for(int j = 0; j < 10; ++j){
+            Vector<3> p = scale * makeVector(rand_u(), rand_u(), rand_u());
+            data.push_back(make_pair(p + sigma * makeVector(rand_u(), 
rand_u(), rand_u()), T * p + sigma * makeVector(rand_u(), rand_u(), rand_u())));
+        }
+        SE3<> opt = tag::optimize_epipolar(data, SE3<>::exp(T.ln() + sigma * 
makeVector(rand_u(), rand_u(), rand_u(), rand_u(), rand_u(), rand_u())));
+        cout << norm_sq((opt.get_rotation().inverse() * 
T.get_rotation()).ln()) << "\t" << norm_sq(unit(opt.get_translation()) - 
unit(T.get_translation())) << endl;
+    }
+}
+
 int main(int argc, char ** argv){
 
-    testRandom();
+    testPerturbation();
 
     return 0;
 }

Index: tag/five_point.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/five_point.h,v
retrieving revision 1.3
retrieving revision 1.4
diff -u -b -r1.3 -r1.4
--- tag/five_point.h    23 Apr 2009 12:07:45 -0000      1.3
+++ tag/five_point.h    28 Apr 2009 10:36:41 -0000      1.4
@@ -15,7 +15,7 @@
 
 
 /// @ingroup essentialgroup
-std::vector<TooN::Matrix<3> > 
five_point(std::tr1::array<std::pair<TooN::Vector<3>, TooN::Vector<3> >, 5> 
points);
+std::vector<TooN::Matrix<3> > five_point(const 
std::tr1::array<std::pair<TooN::Vector<3>, TooN::Vector<3> >, 5> & points);
 
 /// reconstructs possible R,t from essential matrix E.
 /// The implementation follows the algorithm in 
@@ -26,6 +26,15 @@
 /// @ingroup essentialgroup
 std::vector<TooN::SE3<> > se3_from_E( const TooN::Matrix<3> & E );
 
+/// optimizes a transformation representing the epipolar geometry between 
correspondences.
+/// This function minimizes the algebraic error of the epipolar geometry 
through non-linear optimization of the
+/// rotation and direction of the translation.
+/// @arg points a vector of pairs of directions in 3 space containing 
correspondences
+/// @arg initial an inital value for the transformation used as starting point 
of the optimization
+/// @return the optimized transformation
+/// @ingroup essentialgroup
+TooN::SE3<> optimize_epipolar(const std::vector<std::pair<TooN::Vector<3>, 
TooN::Vector<3> > > & points, const TooN::SE3<> & initial);
+
 }
 
 #endif // TAG_FIVE_POINT