[Toon-members] TooN so3.h se3.h test/SXX_test.cc

[Gerhard Reitmayr]

CVSROOT:        /cvsroot/toon
Module name:    TooN
Changes by:     Gerhard Reitmayr <gerhard>      10/02/01 17:33:18

Modified files:
        .              : so3.h se3.h 
        test           : SXX_test.cc 

Log message:
        test for det() > 0 in SO3::coerce and fix some const issues in SE3

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/TooN/so3.h?cvsroot=toon&r1=1.42&r2=1.43
http://cvs.savannah.gnu.org/viewcvs/TooN/se3.h?cvsroot=toon&r1=1.26&r2=1.27
http://cvs.savannah.gnu.org/viewcvs/TooN/test/SXX_test.cc?cvsroot=toon&r1=1.9&r2=1.10

Patches:
Index: so3.h
===================================================================
RCS file: /cvsroot/toon/TooN/so3.h,v
retrieving revision 1.42
retrieving revision 1.43
diff -u -b -r1.42 -r1.43
--- so3.h       18 Jan 2010 15:24:12 -0000      1.42
+++ so3.h       1 Feb 2010 17:33:18 -0000       1.43
@@ -32,6 +32,7 @@
 
 #include <TooN/TooN.h>
 #include <TooN/helpers.h>
+#include <cassert>
 
 namespace TooN {
 
@@ -107,6 +108,8 @@
                my_matrix[2] -= my_matrix[0] * (my_matrix[0]*my_matrix[2]);
                my_matrix[2] -= my_matrix[1] * (my_matrix[1]*my_matrix[2]);
                my_matrix[2] = unit(my_matrix[2]);
+               // check for positive determinant <=> right handed coordinate 
system of row vectors
+               assert( (my_matrix[0] ^ my_matrix[1]) * my_matrix[2] > 0 ); 
        }
        
        /// Exponentiate a vector in the Lie algebra to generate a new SO3.

Index: se3.h
===================================================================
RCS file: /cvsroot/toon/TooN/se3.h,v
retrieving revision 1.26
retrieving revision 1.27
diff -u -b -r1.26 -r1.27
--- se3.h       13 May 2009 14:28:38 -0000      1.26
+++ se3.h       1 Feb 2010 17:33:18 -0000       1.27
@@ -136,21 +136,21 @@
        /// \f$ B \f$, 
        /// \f$ e^{\text{Adj}(v)} = Be^{v}B^{-1} \f$
        /// @param M The Matrix to transfer
-       template<int S, typename Accessor>
-       inline Vector<6, Precision> adjoint(const Vector<S,Precision, 
Accessor>& vect)const;
+       template<int S, typename P2, typename Accessor>
+       inline Vector<6, Precision> adjoint(const Vector<S,P2, Accessor>& 
vect)const;
 
        /// Transfer covectors between frames (using the transpose of the 
inverse of the adjoint)
        /// so that trinvadjoint(vect1) * adjoint(vect2) = vect1 * vect2
-       template<int S, typename Accessor>
-       inline Vector<6, Precision> trinvadjoint(const 
Vector<S,Precision,Accessor>& vect)const;
+       template<int S, typename P2, typename Accessor>
+       inline Vector<6, Precision> trinvadjoint(const Vector<S,P2,Accessor>& 
vect)const;
        
        ///@overload
-       template <int R, int C, typename Accessor>
-       inline Matrix<6,6,Precision> adjoint(const 
Matrix<R,C,Precision,Accessor>& M)const;
+       template <int R, int C, typename P2, typename Accessor>
+       inline Matrix<6,6,Precision> adjoint(const Matrix<R,C,P2,Accessor>& 
M)const;
 
        ///@overload
-       template <int R, int C, typename Accessor>
-       inline Matrix<6,6,Precision> trinvadjoint(const 
Matrix<R,C,Precision,Accessor>& M)const;
+       template <int R, int C, typename P2, typename Accessor>
+       inline Matrix<6,6,Precision> trinvadjoint(const 
Matrix<R,C,P2,Accessor>& M)const;
 
 private:
        SO3<Precision> my_rotation;
@@ -161,8 +161,8 @@
 // from one coord frame to another
 // so that exp(adjoint(vect)) = (*this) * exp(vect) * (this->inverse())
 template<typename Precision>
-template<int S, typename Accessor>
-inline Vector<6, Precision> SE3<Precision>::adjoint(const Vector<S,Precision, 
Accessor>& vect) const{
+template<int S, typename P2, typename Accessor>
+inline Vector<6, Precision> SE3<Precision>::adjoint(const Vector<S,P2, 
Accessor>& vect) const{
        SizeMismatch<6,S>::test(6, vect.size());
        Vector<6, Precision> result;
        result.template slice<3,3>() = get_rotation() * vect.template 
slice<3,3>();
@@ -175,8 +175,8 @@
 // (using the transpose of the inverse of the adjoint)
 // so that trinvadjoint(vect1) * adjoint(vect2) = vect1 * vect2
 template<typename Precision>
-template<int S, typename Accessor>
-inline Vector<6, Precision> SE3<Precision>::trinvadjoint(const 
Vector<S,Precision, Accessor>& vect) const{
+template<int S, typename P2, typename Accessor>
+inline Vector<6, Precision> SE3<Precision>::trinvadjoint(const Vector<S,P2, 
Accessor>& vect) const{
        SizeMismatch<6,S>::test(6, vect.size());
        Vector<6, Precision> result;
        result.template slice<3,3>() = get_rotation() * vect.template 
slice<3,3>();
@@ -186,8 +186,8 @@
 }
 
 template<typename Precision>
-template<int R, int C, typename Accessor>
-inline Matrix<6,6,Precision> SE3<Precision>::adjoint(const 
Matrix<R,C,Precision,Accessor>& M)const{
+template<int R, int C, typename P2, typename Accessor>
+inline Matrix<6,6,Precision> SE3<Precision>::adjoint(const 
Matrix<R,C,P2,Accessor>& M)const{
        SizeMismatch<6,R>::test(6, M.num_cols());
        SizeMismatch<6,C>::test(6, M.num_rows());
 
@@ -202,8 +202,8 @@
 }
 
 template<typename Precision>
-template<int R, int C, typename Accessor>
-inline Matrix<6,6,Precision> SE3<Precision>::trinvadjoint(const 
Matrix<R,C,Precision,Accessor>& M)const{
+template<int R, int C, typename P2, typename Accessor>
+inline Matrix<6,6,Precision> SE3<Precision>::trinvadjoint(const 
Matrix<R,C,P2,Accessor>& M)const{
        SizeMismatch<6,R>::test(6, M.num_cols());
        SizeMismatch<6,C>::test(6, M.num_rows());
 

Index: test/SXX_test.cc
===================================================================
RCS file: /cvsroot/toon/TooN/test/SXX_test.cc,v
retrieving revision 1.9
retrieving revision 1.10
diff -u -b -r1.9 -r1.10
--- test/SXX_test.cc    30 Apr 2009 17:50:40 -0000      1.9
+++ test/SXX_test.cc    1 Feb 2010 17:33:18 -0000       1.10
@@ -154,7 +154,7 @@
     TooN::Matrix<3> m;
     m[0] = TooN::makeVector(0.5, 1,2);
     m[1] = TooN::makeVector(1,1,0);
-    m[2] = TooN::makeVector(0,-1,0);
+    m[2] = TooN::makeVector(0,1,0);
     cout << "set from matrix (uses coerce)\n" << m << "\n";
     r = m;
     cout << r << endl;