[Toon-members] TooN so3.h [Maintenance_Branch_1_x]

[Gerhard Reitmayr]

CVSROOT:        /cvsroot/toon
Module name:    TooN
Branch:         Maintenance_Branch_1_x
Changes by:     Gerhard Reitmayr <gerhard>      09/03/11 15:30:05

Modified files:
        .              : so3.h 

Log message:
        simpler SO3::ln implementation for vanishing antisymmetric part, does 
not use LU anymore

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/TooN/so3.h?cvsroot=toon&only_with_tag=Maintenance_Branch_1_x&r1=1.23&r2=1.23.2.1

Patches:
Index: so3.h
===================================================================
RCS file: /cvsroot/toon/TooN/so3.h,v
retrieving revision 1.23
retrieving revision 1.23.2.1
diff -u -b -r1.23 -r1.23.2.1
--- so3.h       5 Nov 2008 13:24:23 -0000       1.23
+++ so3.h       11 Mar 2009 15:30:03 -0000      1.23.2.1
@@ -21,7 +21,6 @@
 #define __SO3_H
 
 #include <TooN/TooN.h>
-#include <TooN/LU.h>
 #include <TooN/helpers.h>
 
 #ifndef TOON_NO_NAMESPACE
@@ -211,17 +210,15 @@
 inline Vector<3> SO3::ln() const{
   Vector<3> result;
 
-  double trace = my_matrix[0][0] + my_matrix[1][1] + my_matrix[2][2];
-
-  // 2 * cos(theta) - 1 == trace
+  const double trace = my_matrix[0][0] + my_matrix[1][1] + my_matrix[2][2];
 
+  // 2 * cos(theta) + 1 == trace
   result[0] = (my_matrix[2][1]-my_matrix[1][2])/2;
   result[1] = (my_matrix[0][2]-my_matrix[2][0])/2;
   result[2] = (my_matrix[1][0]-my_matrix[0][1])/2;
 
-  if (trace > -0.95) {
       double sin_angle_abs = sqrt(result*result);
-      if (sin_angle_abs > 0.00001) {
+      if (sin_angle_abs > 0.000001 ) { // cannot use small angle approximation
          double angle;
          if(sin_angle_abs > 1.0) {
              sin_angle_abs = 1.0;
@@ -232,37 +229,29 @@
                  angle = M_PI - angle;
          }
          result *= angle / sin_angle_abs;
+      } else if( trace < 1) { // antisymmetric part vanishes, but still large 
rotation, need information from symmetric part
+       // diagonal elements provide squares of rotation axis elements
+       const double ANom = 1.0/(3.0 - trace);
+       // explicit solution of the linear system for the squares of rotation 
axis elements
+       TooN::Vector<3> r2 = TooN::makeVector(
+               sqrt((+ my_matrix[0][0] - my_matrix[1][1] - my_matrix[2][2]  + 
1.0) * ANom),
+               sqrt((- my_matrix[0][0] + my_matrix[1][1] - my_matrix[2][2]  + 
1.0) * ANom),
+               sqrt((- my_matrix[0][0] - my_matrix[1][1] + my_matrix[2][2]  + 
1.0) * ANom)
+       );
+       // antisymmetric part constrains sign (where possible)
+       if(result[0] < 0.0)
+               r2[0] *= -1;
+       if(result[1] < 0.0)
+               r2[1] *= -1;
+       if(result[2] < 0.0)
+               r2[2] *= -1;
+       // r2 contains now the rotation axis as unit vector, need to scale by 
correct angle !
+       const double angle = acos((trace - 1.0)* 0.5);
+       result = r2 * angle;
+      } else { // can use small angle approximation, because we are around zero
+       // nothing todo
       }
       return result;
-  } else {
-      TooN::Matrix<3> A = my_matrix;
-      A[0][0] -= 1.0;
-      A[1][1] -= 1.0;
-      A[2][2] -= 1.0;
-      TooN::LU<3> lu(A);
-      const TooN::Matrix<3,3,TooN::ColMajor>& u = lu.get_lu().T();
-      const double u0 = fabs(u[0][0]);
-      const double u1 = fabs(u[1][1]);
-      const double u2 = fabs(u[2][2]);
-      int row;
-      if (u0 < u1)
-         row = u0 < u2 ? 0 : 2;
-      else
-         row = u1 < u2 ? 1 : 2;
-      //std::cerr << u << std::endl;
-      //std::cerr << "row = " << row << std::endl;
-      TooN::Vector<3> r;
-      const double factor = acos(0.5*(trace-1));
-      switch (row) {
-      case 0: r[0] = factor; r[1] = 0; r[2] = 0; break;
-      case 1: r[0] = u[0][1]*u[2][2]; r[1] = -u[0][0]*u[2][2]; r[2] = 0; r *= 
factor/sqrt(r*r); break;
-      case 2: r[0] = u[0][1]*u[1][2] - u[0][2]*u[1][1]; r[1] = 
-u[0][0]*u[1][2]; r[2] = u[0][0]*u[1][1];  r *= factor/sqrt(r*r); break;
-      }
-      if (result * r < 0)
-         return -r;
-      else
-         return r;
-  }
 }
 
 inline SO3 SO3::inverse() const{