[Toon-members] tag src/absorient.cpp tag/absorient.h test/abso...

[Gerhard Reitmayr]

CVSROOT:        /cvsroot/toon
Module name:    tag
Changes by:     Gerhard Reitmayr <gerhard>      11/06/27 14:56:24

Modified files:
        src            : absorient.cpp 
        tag            : absorient.h 
Added files:
        test           : absorient.cpp 

Log message:
        updated absolute orientation functions with a different method to 
calculate the rotation and made them more robust against input with too little 
constraints as well

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/tag/src/absorient.cpp?cvsroot=toon&r1=1.10&r2=1.11
http://cvs.savannah.gnu.org/viewcvs/tag/tag/absorient.h?cvsroot=toon&r1=1.5&r2=1.6
http://cvs.savannah.gnu.org/viewcvs/tag/test/absorient.cpp?cvsroot=toon&rev=1.1

Patches:
Index: src/absorient.cpp
===================================================================
RCS file: /cvsroot/toon/tag/src/absorient.cpp,v
retrieving revision 1.10
retrieving revision 1.11
diff -u -b -r1.10 -r1.11
--- src/absorient.cpp   25 Jun 2011 09:12:21 -0000      1.10
+++ src/absorient.cpp   27 Jun 2011 14:56:24 -0000      1.11
@@ -2,11 +2,14 @@
 
 #include <cassert>
 
-#include <TooN/SymEigen.h>
+#include <TooN/svd.h>
 #include <TooN/helpers.h>
+#include <TooN/determinant.h>
 
 namespace tag {
 
+static const TooN::DefaultPrecision eps = 1e-8;
+
 TooN::Matrix<3> quaternionToMatrix( const TooN::Vector<4> & q ){
     TooN::Matrix<3> result;
     const int w = 0, x = 1, y = 2, z = 3;
@@ -22,39 +25,42 @@
     return result;
 }
 
-TooN::SO3<>  computeOrientation( const std::vector<TooN::Vector<3> > & a, 
const std::vector<TooN::Vector<3> > & b ){
+static std::pair<TooN::SO3<>, TooN::DefaultPrecision> computeOrientationScale( 
const std::vector<TooN::Vector<3> > & a, const std::vector<TooN::Vector<3> > & 
b ){
     const size_t N = a.size();
     // compute cross correlations
-    const int x = 0, y = 1, z = 2;
     TooN::Matrix<3> s = TooN::Zeros;
-    for( unsigned int i = 0; i < N; i++){
-        s += a[i].as_col() * b[i].as_row();
+       for( size_t i = 0; i < N; i++){
+               s += b[i].as_col() * a[i].as_row();
     }
+       s /= N;
 
-    // create symmetric M for eigenvalue analysis
-    TooN::Matrix<4> M;
-    M[0][0] = s[x][x] + s[y][y] + s[z][z];
-    M[1][0] = M[0][1] = s[y][z] - s[z][y];
-    M[2][0] = M[0][2] = s[z][x] - s[x][z];
-    M[3][0] = M[0][3] = s[x][y] - s[y][x];
-    M[1][1] = s[x][x] - s[y][y] - s[z][z];
-    M[2][1] = M[1][2] = s[x][y] + s[y][x];
-    M[3][1] = M[1][3] = s[z][x] + s[x][z];
-    M[2][2] = - s[x][x] + s[y][y] - s[z][z];
-    M[3][2] = M[2][3] = s[y][z] + s[z][y];
-    M[3][3] = - s[x][x] - s[y][y] + s[z][z];
-
-    // eigenvalue decomposition to find eigenvector to largest eigenvalue
-    TooN::SymEigen<4> ev(M);
-    TooN::Vector<4> evals = ev.get_evalues();
-    int index = 0;
-    for(unsigned int i = index+1; i<4; i++)
-        if( evals[i] > evals[index] )
-            index = i;
-    TooN::Vector<4> evec = ev.get_evectors()[index];
-    TooN::SO3<>  result;
-    result = quaternionToMatrix(evec);
-    return result;
+       // SVD of cross correlation matrix
+       TooN::SVD<3> svd(s);
+
+       // build S for rotation matrix
+       TooN::Matrix<3> S = TooN::Identity;
+
+       const TooN::DefaultPrecision ds = determinant_gaussian_elimination(s);
+       if(ds < -eps){
+               S(2,2) = -1;
+       } else if(ds < eps) { // close to 0 let U * VT decide
+               const TooN::DefaultPrecision duv = 
determinant_gaussian_elimination(svd.get_U()) 
+                                                                               
* determinant_gaussian_elimination(svd.get_VT());
+               if(duv <  0)
+                       S(2,2) = -1;
+       }
+
+       // compute trace(DS)
+       TooN::DefaultPrecision scale = 0;
+       for(int i = 0; i < 3; ++i)
+               scale += svd.get_diagonal()[i] * S(i,i);
+
+       return std::make_pair(TooN::SO3<>(svd.get_U() * S * svd.get_VT()), 
scale);
+}
+
+TooN::SO3<> computeOrientation( const std::vector<TooN::Vector<3> > & a, const 
std::vector<TooN::Vector<3> > & b ){
+       std::pair<TooN::SO3<>, TooN::DefaultPrecision> result = 
computeOrientationScale( a, b );
+       return result.first;
 }
 
 // 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
@@ -84,6 +90,10 @@
        assert(a.size() <= b.size());
     const size_t N = a.size();
 
+       if(N == 1){    // quick special case
+               return TooN::SE3<>(TooN::SO3<>(), b[0] - a[0]);
+       }
+
     TooN::Vector<3> ma = TooN::Zeros, mb = TooN::Zeros;
 
     // compute centroids
@@ -108,10 +118,14 @@
     return result;
 }
 
-std::pair<TooN::SE3<>, double> computeSimilarity( const 
std::vector<TooN::Vector<3> > & a, const std::vector<TooN::Vector<3> > & b){
+std::pair<TooN::SE3<>, TooN::DefaultPrecision> computeSimilarity( const 
std::vector<TooN::Vector<3> > & a, const std::vector<TooN::Vector<3> > & b){
        assert(a.size() <= b.size());
        const size_t N = a.size();
        
+       if(N == 1){    // quick special case
+               return std::make_pair(TooN::SE3<>(TooN::SO3<>(), b[0] - a[0]), 
1);
+       }
+       
        TooN::Vector<3> ma = TooN::Zeros, mb = TooN::Zeros;
        
        // compute centroids
@@ -131,16 +145,18 @@
        
        // put resulting transformation together 
        TooN::SE3<>  result;
-       result.get_rotation() = computeOrientation( ap, bp );
+       std::pair<TooN::SO3<>, TooN::DefaultPrecision> rs = 
computeOrientationScale( ap, bp );
+       result.get_rotation() = rs.first;
+       
+       std::cout << rs.second << std::endl;
        
        // compute scale
-       double sa = 0, sbRa = 0;
+       TooN::DefaultPrecision sa = 0;
        for( unsigned int i = 0; i < N; ++i){
                sa += norm_sq(ap[i]);
-               sbRa += bp[i] * (result.get_rotation() * ap[i]);
        }
-       
-       const double scale = sbRa/sa;
+       sa /= N;
+       const TooN::DefaultPrecision scale = rs.second / sa;
        
        result.get_translation() = mb - result.get_rotation() * (scale * ma);
        return std::make_pair(result, scale);

Index: tag/absorient.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/absorient.h,v
retrieving revision 1.5
retrieving revision 1.6
diff -u -b -r1.5 -r1.6
--- tag/absorient.h     25 Jun 2011 09:12:21 -0000      1.5
+++ tag/absorient.h     27 Jun 2011 14:56:24 -0000      1.6
@@ -13,8 +13,11 @@
 /// between sets of 3D correspondences.
 
 /// computes the rotation between two sets of points maximizing b * Ta
-/// this function is part of the absolute orientation algorithm after Horn
-/// and is used in @ref computeAbsoluteOrientation function.
+/// this function is part of the absolute orientation algorithm after 
+/// Shinji Umeyama, Least-squares estimation of transformation parameters 
+/// between two point patterns. IEEE PAMI, 13(4):376-380, 1991.
+/// @ref computeAbsoluteOrientation and @ref computeSimilarity functions use 
this
+/// function.
 /// @param[in] a vector of 3D points
 /// @param[in] b vector of 3D points
 /// @return TooN::SO3 containing the rotation such that b = T a
@@ -31,7 +34,9 @@
 /// @ingroup absorient
 TooN::SO3<> computeOrientation( const TooN::Vector<3> & a1, const 
TooN::Vector<3> & b1, const TooN::Vector<3> & a2, const TooN::Vector<3> & b2 );
 
-/// computes the rigid transformation between two corresponding point sets 
after Horn
+/// computes the rigid transformation between two corresponding point sets 
after 
+/// Shinji Umeyama, Least-squares estimation of transformation parameters 
+/// between two point patterns. IEEE PAMI, 13(4):376-380, 1991.
 /// The result is an SE3 that maps points from vector a to points from vector 
b : b[i] = SE3 * a[i]
 /// @param[in] a vector of 3D points
 /// @param[in] b vector of 3D points
@@ -39,13 +44,15 @@
 /// @ingroup absorient
 TooN::SE3<> computeAbsoluteOrientation( const std::vector<TooN::Vector<3> > & 
a, const std::vector<TooN::Vector<3> > & b);
 
-/// computes a similarity transformation between two corresponding point sets 
after Horn
+/// computes a similarity transformation between two corresponding point sets 
after
+/// Shinji Umeyama, Least-squares estimation of transformation parameters 
+/// between two point patterns. IEEE PAMI, 13(4):376-380, 1991.
 /// The result is an SE3 and a scale S that maps points from vector a to 
points from vector b : b[i] = SE3 * S * a[i]
 /// @param[in] a vector of 3D points
 /// @param[in] b vector of 3D points
 /// @return a pair consisting of a TooN::SE3 T and a double S containing the 
transformation such that b = T * S * a
 /// @ingroup absorient
-std::pair<TooN::SE3<>, double> computeSimilarity( const 
std::vector<TooN::Vector<3> > & a, const std::vector<TooN::Vector<3> > & b);
+std::pair<TooN::SE3<>, TooN::DefaultPrecision> computeSimilarity( const 
std::vector<TooN::Vector<3> > & a, const std::vector<TooN::Vector<3> > & b);
 
 /// computes the mean rotation of a set of rotations. This is the rotation R 
such that R^{-1} * R_i is minimal for all R_i.
 /// @param[in] r a vector of rotations
@@ -54,8 +61,7 @@
 TooN::SO3<> computeMeanOrientation( const std::vector<TooN::SO3<> > & r);
 
 /// computes a rotation matrix corresponding to a unit quaternion. The 
quaternion
-/// is in the format (q0,qx,qy,qz) to fit the absolute orientation algorithm. 
This
-/// is a helper function for the @ref computeOrientation function.
+/// is in the format (q0,qx,qy,qz).
 /// @param[in] q a 4-vector containing the coefficients of a unit quaternion 
as (q0,qx,qy,qz)
 /// @return a 3x3 rotation matrix corresponding to the quaternion
 /// @ingroup absorient

Index: test/absorient.cpp
===================================================================
RCS file: test/absorient.cpp
diff -N test/absorient.cpp
--- /dev/null   1 Jan 1970 00:00:00 -0000
+++ test/absorient.cpp  27 Jun 2011 14:56:24 -0000      1.1
@@ -0,0 +1,58 @@
+#include <iostream>
+#include <vector>
+
+#include <tag/absorient.h>
+
+using namespace std;
+using namespace TooN;
+using namespace tag;
+
+typedef vector<Vector<3> > Points;
+typedef pair<SE3<>, double> Sim;
+
+double inline rand_u(){
+    return (double)rand()/RAND_MAX;
+}
+
+double inline rand_u(double from, double to){
+    return from + (to - from)*rand_u();
+}
+
+template <int N>
+Vector<N> rand_vect(double from, double to){
+    Vector<N> v;
+    for(int i = 0; i < N; ++i)
+        v[i] = rand_u(from, to);
+    return v;
+}
+
+int main( int arg, char ** argv ){
+
+    const int N = arg > 1 ? atoi(argv[1]) : 10;
+
+    Points a(N), b(N), c(N);
+    
+    SE3<> pose(makeVector(1,2,3,2,1,0.5));
+    
+    for( unsigned i = 0; i < N; ++i ){
+        a[i] = rand_vect<3>(-10, 10);
+        b[i] = pose * a[i];
+        c[i] = b[i] + rand_vect<3>(-1, 1);
+    }
+
+    SE3<> sb = computeAbsoluteOrientation(a,b);
+    SE3<> sc = computeAbsoluteOrientation(a,c);
+
+    double eb = 0, ec = 0;
+    for( unsigned i = 0; i < N; ++i){
+        eb += norm_sq(b[i] - sb * a[i]);
+        ec += norm_sq(c[i] - sc * a[i]);
+    }
+    
+    
+    cout << "test\t" << pose.ln() << "\n";
+    cout << "exact\t" << sb.ln() << "\t" << sqrt( eb ) << "\n"
+         << "noisy\t" << sc.ln() << "\t" << sqrt( ec ) << endl;
+
+    return 0;
+}