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

[Gerhard Reitmayr]

CVSROOT:        /cvsroot/toon
Module name:    tag
Changes by:     Gerhard Reitmayr <gerhard>      08/12/09 20:33:42

Modified files:
        src            : absorient.cpp 
        tag            : absorient.h kalmanfilter.h helpers.h 

Log message:
        fast rotation from rays

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/tag/src/absorient.cpp?cvsroot=toon&r1=1.2&r2=1.3
http://cvs.savannah.gnu.org/viewcvs/tag/tag/absorient.h?cvsroot=toon&r1=1.2&r2=1.3
http://cvs.savannah.gnu.org/viewcvs/tag/tag/kalmanfilter.h?cvsroot=toon&r1=1.8&r2=1.9
http://cvs.savannah.gnu.org/viewcvs/tag/tag/helpers.h?cvsroot=toon&r1=1.1&r2=1.2

Patches:
Index: src/absorient.cpp
===================================================================
RCS file: /cvsroot/toon/tag/src/absorient.cpp,v
retrieving revision 1.2
retrieving revision 1.3
diff -u -b -r1.2 -r1.3
--- src/absorient.cpp   29 May 2008 16:36:23 -0000      1.2
+++ src/absorient.cpp   9 Dec 2008 20:33:41 -0000       1.3
@@ -3,6 +3,7 @@
 #include <cassert>
 
 #include <TooN/SymEigen.h>
+#include <TooN/helpers.h>
 
 namespace tag {
 
@@ -57,6 +58,28 @@
     return result;
 }
 
+// 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;
+    TooN::normalize(result.T()[0]);
+    TooN::normalize(result.T()[2]);
+    result.T()[1] = result.T()[2] ^ result.T()[0];
+    return TooN::SO3(result);
+}
+
+TooN::SO3 computeOrientation( const TooN::Vector<3> & a1, const 
TooN::Vector<3> & b1, const TooN::Vector<3> & a2, const TooN::Vector<3> & b2 ){
+    TooN::SO3 r1 = canonicalOrientation( a1, a2 );
+    TooN::SO3 r2 = canonicalOrientation( b1, b2 );
+    const TooN::SO3 rAB = r2 * r1.inverse();
+    r1 = canonicalOrientation( a2, a1 );
+    r2 = canonicalOrientation( b2, b1 );
+    const TooN::SO3 rBA = r2 * r1.inverse();
+    const TooN::SO3 diff = rBA * rAB.inverse();
+    return TooN::SO3::exp(diff.ln() * 0.5) * rAB;
+}
+
 TooN::SE3 computeAbsoluteOrientation( const std::vector<TooN::Vector<3> > & a, 
const std::vector<TooN::Vector<3> > & b){
     //std::assert(a.size() == b.size());
     const size_t N = a.size();

Index: tag/absorient.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/absorient.h,v
retrieving revision 1.2
retrieving revision 1.3
diff -u -b -r1.2 -r1.3
--- tag/absorient.h     31 May 2006 16:46:20 -0000      1.2
+++ tag/absorient.h     9 Dec 2008 20:33:42 -0000       1.3
@@ -21,6 +21,16 @@
 /// @ingroup absorient
 TooN::SO3 computeOrientation( const std::vector<TooN::Vector<3> > & a, const 
std::vector<TooN::Vector<3> > & b );
 
+/// directly computes a rotation between two pairs of rays in space maximizing 
b * T a
+/// its about 8x faster then using the general computeOrientation for 2 
correspondences.
+/// @param[in] a1 first input vector
+/// @param[in] b1 first output vector
+/// @param[in] a2 second input vector
+/// @param[in] b2 second output vector
+/// @return TooN::SO3 containing the rotation such that b = T a
+/// @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
 /// 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

Index: tag/kalmanfilter.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/kalmanfilter.h,v
retrieving revision 1.8
retrieving revision 1.9
diff -u -b -r1.8 -r1.9
--- tag/kalmanfilter.h  14 Dec 2006 14:00:46 -0000      1.8
+++ tag/kalmanfilter.h  9 Dec 2008 20:33:42 -0000       1.9
@@ -94,9 +94,9 @@
     /// predicts the state by applying the process model over the time 
interval dt
     /// @param[in] dt time interval
     void predict(double dt){
-        model.updateState( state, dt );
         state.covariance = TooN::transformCovariance(model.getJacobian( state, 
dt ), state.covariance) + model.getNoiseCovariance( dt );
         TooN::Symmetrize(state.covariance);
+        model.updateState( state, dt );
     }
 
     /// incorporates a measurement

Index: tag/helpers.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/helpers.h,v
retrieving revision 1.1
retrieving revision 1.2
diff -u -b -r1.1 -r1.2
--- tag/helpers.h       31 Oct 2007 10:26:22 -0000      1.1
+++ tag/helpers.h       9 Dec 2008 20:33:42 -0000       1.2
@@ -37,8 +37,8 @@
         const TooN::Vector<2>& b = second_point(*it);
         const double rows[2][8] = {{a[0], a[1], 1, 0, 0, 0, -b[0]*a[0], 
-b[0]*a[1]},
                         {0, 0, 0, a[0], a[1], 1, -b[1]*a[0], -b[1]*a[1]}};
-        wls.add_df(b[0], TooN::Vector<8>(rows[0]));
-        wls.add_df(b[1], TooN::Vector<8>(rows[1]));
+        wls.add_df(b[0], TooN::Vector<8>(rows[0]), noise(*it));
+        wls.add_df(b[1], TooN::Vector<8>(rows[1]), noise(*it));
     }
     wls.compute();
     TooN::Vector<8> h = wls.get_mu();