toon-members
[Top][All Lists]
Advanced

[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]

[Toon-members] TooN helpers.h se2.h se3.h so2.h so3.h internal...


From: Gerhard Reitmayr
Subject: [Toon-members] TooN helpers.h se2.h se3.h so2.h so3.h internal...
Date: Sun, 18 Apr 2010 09:49:18 +0000

CVSROOT:        /cvsroot/toon
Module name:    TooN
Changes by:     Gerhard Reitmayr <gerhard>      10/04/18 09:49:18

Modified files:
        .              : helpers.h se2.h se3.h so2.h so3.h 
        internal       : config.hh 
        test           : sl.cc 
Added files:
        functions      : fadbad.h 
        test           : fadbad.cpp 

Log message:
        initial integration with FADBAD++ automatic differentiation (AD) library

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/TooN/helpers.h?cvsroot=toon&r1=1.89&r2=1.90
http://cvs.savannah.gnu.org/viewcvs/TooN/se2.h?cvsroot=toon&r1=1.8&r2=1.9
http://cvs.savannah.gnu.org/viewcvs/TooN/se3.h?cvsroot=toon&r1=1.28&r2=1.29
http://cvs.savannah.gnu.org/viewcvs/TooN/so2.h?cvsroot=toon&r1=1.11&r2=1.12
http://cvs.savannah.gnu.org/viewcvs/TooN/so3.h?cvsroot=toon&r1=1.44&r2=1.45
http://cvs.savannah.gnu.org/viewcvs/TooN/functions/fadbad.h?cvsroot=toon&rev=1.1
http://cvs.savannah.gnu.org/viewcvs/TooN/internal/config.hh?cvsroot=toon&r1=1.25&r2=1.26
http://cvs.savannah.gnu.org/viewcvs/TooN/test/sl.cc?cvsroot=toon&r1=1.3&r2=1.4
http://cvs.savannah.gnu.org/viewcvs/TooN/test/fadbad.cpp?cvsroot=toon&rev=1.1

Patches:
Index: helpers.h
===================================================================
RCS file: /cvsroot/toon/TooN/helpers.h,v
retrieving revision 1.89
retrieving revision 1.90
diff -u -b -r1.89 -r1.90
--- helpers.h   16 Dec 2009 16:44:14 -0000      1.89
+++ helpers.h   18 Apr 2010 09:49:18 -0000      1.90
@@ -156,7 +156,7 @@
        ///@ingroup gLinAlg
        template<int Size, typename Precision, typename Base> inline 
Vector<(Size==Dynamic?Dynamic:Size-1), Precision> project( const Vector<Size, 
Precision, Base> & v){
                static const int Len = (Size==Dynamic?Dynamic:Size-1);
-               return v.template slice<0, Len>(0, v.size()-1) / v[v.size() - 
1];
+               return TooN::operator/(v.template slice<0, Len>(0, 
v.size()-1),v[v.size() - 1]);
        }
        
        //This should probably be done with an operator to prevent an extra 
new[] for dynamic vectors.

Index: se2.h
===================================================================
RCS file: /cvsroot/toon/TooN/se2.h,v
retrieving revision 1.8
retrieving revision 1.9
diff -u -b -r1.8 -r1.9
--- se2.h       9 Jun 2009 13:33:22 -0000       1.8
+++ se2.h       18 Apr 2010 09:49:18 -0000      1.9
@@ -85,7 +85,10 @@
 
        /// Right-multiply by another SE2 (concatenate the two transformations)
        /// @param rhs The multipier
-       inline SE2 operator *(const SE2& rhs) const { return 
SE2(my_rotation*rhs.my_rotation, my_translation + 
my_rotation*rhs.my_translation); }
+       template <typename P>
+       SE2<typename Internal::MultiplyType<Precision,P>::type> operator 
*(const SE2<P>& rhs) const { 
+               return SE2<typename 
Internal::MultiplyType<Precision,P>::type>(my_rotation*rhs.get_rotation(), 
my_translation + my_rotation*rhs.get_translation()); 
+       }
 
        /// Self right-multiply by another SE2 (concatenate the two 
transformations)
        /// @param rhs The multipier
@@ -113,7 +116,7 @@
        /// transfers a vector in the Lie algebra, from one coord frame to 
another
        /// so that exp(adjoint(vect)) = (*this) * exp(vect) * (this->inverse())
        template<typename Accessor>
-       inline Vector<3, Precision> adjoint(const Vector<3,Precision, Accessor> 
& vect) const {
+       Vector<3, Precision> adjoint(const Vector<3,Precision, Accessor> & 
vect) const {
                Vector<3, Precision> result;
                result[2] = vect[2];
                result.template slice<0,2>() = my_rotation * vect.template 
slice<0,2>();
@@ -123,7 +126,7 @@
        }
 
        template <typename Accessor>
-       inline Matrix<3,3,Precision> adjoint(const 
Matrix<3,3,Precision,Accessor>& M) const {
+       Matrix<3,3,Precision> adjoint(const Matrix<3,3,Precision,Accessor>& M) 
const {
                Matrix<3,3,Precision> result;
                for(int i=0; i<3; ++i)
                        result.T()[i] = adjoint(M.T()[i]);
@@ -300,21 +303,6 @@
        return Matrix<Rows,3,typename 
Internal::MultiplyType<PM,P>::type>(Operator<Internal::MSE2Mult<Rows, C, PM, A, 
P> >(lhs,rhs));
 }
 
-/* inline SE2 SE2::exp(const Vector<3>& mu){ */
-/*   SE2 result; */
-/*   double theta = mu[2]; */
-/*   result.get_rotation() = SO2::exp(theta); */
-/*   Matrix<2> m2; */
-/*   m2[0][0] = m2[1][1] = result.get_rotation().get_matrix()[1][0]; */
-/*   m2[0][1] = result.get_rotation().get_matrix()[0][0] - 1.0; */
-/*   m2[1][0] = - m2[0][1]; */
-/*   if(theta != 0.0)  */
-/*     result.get_translation() = m2 * mu.slice<0,2>() / fabs(theta); */
-/*   else */
-/*     result.get_translation() = mu.slice<0,2>(); */
-/*   return result; */
-/* } */
-
 template <typename Precision>
 template <int S, typename PV, typename Accessor>
 inline SE2<Precision> SE2<Precision>::exp(const Vector<S, PV, Accessor>& mu)
@@ -346,7 +334,7 @@
                        A = sine * inv_theta;
                        B = (1 - cosine) * (inv_theta * inv_theta);
                }
-               result.get_translation() = A * mu.template slice<0,2>() + B * 
cross;
+               result.get_translation() = TooN::operator*(A,mu.template 
slice<0,2>()) + TooN::operator*(B,cross);
        }
        return result;
 }

Index: se3.h
===================================================================
RCS file: /cvsroot/toon/TooN/se3.h,v
retrieving revision 1.28
retrieving revision 1.29
diff -u -b -r1.28 -r1.29
--- se3.h       8 Apr 2010 16:12:30 -0000       1.28
+++ se3.h       18 Apr 2010 09:49:18 -0000      1.29
@@ -98,7 +98,10 @@
 
        /// Right-multiply by another SE3 (concatenate the two transformations)
        /// @param rhs The multipier
-       inline SE3 operator *(const SE3& rhs) const { return 
SE3(get_rotation()*rhs.get_rotation(), get_translation() + 
get_rotation()*rhs.get_translation()); }
+       template<typename P>
+       inline SE3<typename Internal::MultiplyType<Precision, P>::type> 
operator *(const SE3<P>& rhs) const { 
+           return SE3<typename Internal::MultiplyType<Precision, 
P>::type>(get_rotation()*rhs.get_rotation(), get_translation() + 
get_rotation()*rhs.get_translation()); 
+       }
 
        inline SE3& left_multiply_by(const SE3& left) {
                get_translation() = left.get_translation() + 
left.get_rotation() * get_translation();
@@ -413,7 +416,7 @@
                        B = (1 - cos(theta)) * (inv_theta * inv_theta);
                        C = (1 - A) * (inv_theta * inv_theta);
                }
-               result.get_translation() = mu.template slice<0,3>() + B * cross 
+ C * (w ^ cross);
+               result.get_translation() = mu.template slice<0,3>() + 
TooN::operator*(B, cross) + TooN::operator*(C, (w ^ cross));
        }
        rodrigues_so3_exp(w, A, B, result.get_rotation().my_matrix);
        return result;

Index: so2.h
===================================================================
RCS file: /cvsroot/toon/TooN/so2.h,v
retrieving revision 1.11
retrieving revision 1.12
diff -u -b -r1.11 -r1.12
--- so2.h       9 Jun 2009 13:33:22 -0000       1.11
+++ so2.h       18 Apr 2010 09:49:18 -0000      1.12
@@ -56,7 +56,10 @@
        SO2() : my_matrix(Identity) {} 
        
        /// Construct from a rotation matrix.
-       SO2(const Matrix<2,2,Precision>& rhs) {  *this = rhs; }
+       SO2(const Matrix<2,2,Precision>& rhs) {  
+               *this = rhs; 
+               coerce();
+       }
        
        /// Construct from an angle.
        SO2(const Precision l) { *this = exp(l); }
@@ -64,7 +67,7 @@
        /// Assigment operator from a general matrix. This also calls coerce()
        /// to make sure that the matrix is a valid rotation matrix.
        template <int R, int C, typename P, typename A> 
-       inline SO2& operator=(const Matrix<R,C,P,A>& rhs){
+       SO2& operator=(const Matrix<R,C,P,A>& rhs){
                my_matrix = rhs;
                coerce();
                return *this;
@@ -93,13 +96,17 @@
        SO2 inverse() const { return SO2(*this, Invert()); }
 
        /// Self right-multiply by another rotation matrix
-       SO2& operator *=(const SO2& rhs){
-               my_matrix=my_matrix*rhs.my_matrix;
+       template <typename P>
+       SO2& operator *=(const SO2<P>& rhs){
+               my_matrix=my_matrix*rhs.get_matrix();
                return *this;
        }
 
        /// Right-multiply by another rotation matrix
-       SO2 operator *(const SO2& rhs) const { return SO2(*this,rhs); }
+       template <typename P>
+       SO2<typename Internal::MultiplyType<Precision, P>::type> operator 
*(const SO2<P>& rhs) const { 
+               return SO2<typename Internal::MultiplyType<Precision, 
P>::type>(*this,rhs); 
+       }
 
        /// Returns the SO2 as a Matrix<2>
        const Matrix<2,2,Precision>& get_matrix() const {return my_matrix;}
@@ -112,10 +119,11 @@
                return result;
        }
 
- private:
+private:
        struct Invert {};
        inline SO2(const SO2& so2, const Invert&) : 
my_matrix(so2.my_matrix.T()) {}
-       inline SO2(const SO2& a, const SO2& b) : 
my_matrix(a.my_matrix*b.my_matrix) {}
+       template <typename PA, typename PB>
+       inline SO2(const SO2<PA>& a, const SO2<PB>& b) : 
my_matrix(a.get_matrix()*b.get_matrix()) {}
 
        Matrix<2,2,Precision> my_matrix;
 };

Index: so3.h
===================================================================
RCS file: /cvsroot/toon/TooN/so3.h,v
retrieving revision 1.44
retrieving revision 1.45
diff -u -b -r1.44 -r1.45
--- so3.h       1 Apr 2010 13:02:05 -0000       1.44
+++ so3.h       18 Apr 2010 09:49:18 -0000      1.45
@@ -95,7 +95,7 @@
                my_matrix = my_matrix * R1.T();
        }
        
-       /// Assigment operator from a general matrix. This also calls coerce()
+       /// Assignment operator from a general matrix. This also calls coerce()
        /// to make sure that the matrix is a valid rotation matrix.
        template <int R, int C, typename P, typename A>
        SO3& operator=(const Matrix<R,C,P,A> & rhs) {
@@ -134,7 +134,10 @@
        }
 
        /// Right-multiply by another rotation matrix
-       SO3 operator *(const SO3& rhs) const { return SO3(*this,rhs); }
+       template<typename P>
+       SO3<typename Internal::MultiplyType<Precision, P>::type> operator 
*(const SO3<P>& rhs) const { 
+           return SO3<typename Internal::MultiplyType<Precision, 
P>::type>(*this,rhs); 
+       }
 
        /// Returns the SO3 as a Matrix<3>
        const Matrix<3,3, Precision> & get_matrix() const {return my_matrix;}
@@ -175,7 +178,8 @@
 private:
        struct Invert {};
        inline SO3(const SO3& so3, const Invert&) : 
my_matrix(so3.my_matrix.T()) {}
-       inline SO3(const SO3& a, const SO3& b) : 
my_matrix(a.my_matrix*b.my_matrix) {}
+       template <typename PA, typename PB>
+       inline SO3(const SO3<PA>& a, const SO3<PB>& b) : 
my_matrix(a.get_matrix()*b.get_matrix()) {}
        
        Matrix<3,3, Precision> my_matrix;
 };

Index: internal/config.hh
===================================================================
RCS file: /cvsroot/toon/TooN/internal/config.hh,v
retrieving revision 1.25
retrieving revision 1.26
diff -u -b -r1.25 -r1.26
--- internal/config.hh  6 Apr 2010 15:34:48 -0000       1.25
+++ internal/config.hh  18 Apr 2010 09:49:18 -0000      1.26
@@ -0,0 +1,14 @@
+/* internal/config.hh.  Generated from config.hh.in by configure.  */
+/* #undef TOON_TYPEOF_DECLTYPE */
+
+/* #undef TOON_TYPEOF_TYPEOF */
+
+#define TOON_TYPEOF___TYPEOF__ 1
+
+/* #undef TOON_TYPEOF_BOOST */
+
+/* #undef TOON_TYPEOF_BUILTIN */
+
+#define TOON_DEPRECATED_GCC 1
+
+#define TOON_USE_LAPACK 1

Index: test/sl.cc
===================================================================
RCS file: /cvsroot/toon/TooN/test/sl.cc,v
retrieving revision 1.3
retrieving revision 1.4
diff -u -b -r1.3 -r1.4
--- test/sl.cc  29 Apr 2009 22:55:59 -0000      1.3
+++ test/sl.cc  18 Apr 2010 09:49:18 -0000      1.4
@@ -43,5 +43,10 @@
        
        cout << endl;
        
+/*
+       SO3<> so3(makeVector(1,0,1));
+       h = so3;
+       cout << so3.get_matrix() << h.get_matrix() << "\n";
+*/     
        return 0;
 }

Index: functions/fadbad.h
===================================================================
RCS file: functions/fadbad.h
diff -N functions/fadbad.h
--- /dev/null   1 Jan 1970 00:00:00 -0000
+++ functions/fadbad.h  18 Apr 2010 09:49:18 -0000      1.1
@@ -0,0 +1,130 @@
+// -*- c++ -*-
+
+// Copyright (C) 2005,2009 Tom Drummond (address@hidden)
+//
+// This file is part of the TooN Library.  This library is free
+// software; you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the
+// Free Software Foundation; either version 2, or (at your option)
+// any later version.
+
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU General Public License for more details.
+
+// You should have received a copy of the GNU General Public License along
+// with this library; see the file COPYING.  If not, write to the Free
+// Software Foundation, 59 Temple Place - Suite 330, Boston, MA 02111-1307,
+// USA.
+
+// As a special exception, you may use this file as part of a free software
+// library without restriction.  Specifically, if other files instantiate
+// templates or use macros or inline functions from this file, or you compile
+// this file and link it with other files to produce an executable, this
+// file does not by itself cause the resulting executable to be covered by
+// the GNU General Public License.  This exception does not however
+// invalidate any other reasons why the executable file might be covered by
+// the GNU General Public License.
+
+#ifndef TOON_FADBAD_INTEGATION_H
+#define TOON_FADBAD_INTEGATION_H
+
+#include <iostream>
+
+#include <TooN/TooN.h>
+#include <TooN/se3.h>
+#include <TooN/se2.h>
+
+#include <FADBAD++/fadiff.h>
+
+namespace fadbad {
+
+template <typename P>
+inline std::ostream & operator<<( std::ostream & out, const F<P> & val ){
+       return out << val.val();
+}
+
+}
+
+namespace TooN {
+
+template<class C> struct IsField<fadbad::F<C> >
+{
+       static const int value = numeric_limits<C>::is_specialized; ///<Is C a 
field?
+};
+
+template <int N> 
+inline Vector<N, fadbad::F<double> > make_fad_vector( const Vector<N, double> 
& val, const int start = 0, const int size = N ){
+       using std::min;
+       Vector<N, fadbad::F<double> > result = val;
+       for(int i = start; i < min(size, start+N); ++i)
+               result[i].diff(i,size);
+       return result;
+}
+
+template <int N>
+inline Vector<N, double> get_derivative( const Vector<N, fadbad::F<double> > & 
val, const int D ){
+       Vector<N, double> r;
+       for(int i = 0; i < N; ++i)
+               r[i] = val[i].deriv(D);
+       return r;
+}
+
+template <int N, int D>
+inline Matrix<N, D, double> get_jacobian( const Vector<N, fadbad::F<double> > 
& val ){
+       Matrix<N, D, double> result;
+       for(int i = 0; i < D; ++i)
+               result.T()[i] = get_derivative( val, i );
+       return result;
+}
+
+template <int R, int C>
+inline Matrix<R, C, double> get_derivative( const Matrix<R,C, 
fadbad::F<double> > & val, const int D ){
+       Matrix<R, C, double> result;
+       for(int r = 0; r < R; ++r)
+               for(int c = 0; c < C; ++c)
+                       result[r][c] = val[r][c].deriv(D);
+       return result;
+}
+
+inline SO3<fadbad::F<double> > make_fad_so3(){
+       const Vector<3, fadbad::F<double> > p = make_fad_vector(makeVector(0.0, 
0, 0));
+       return SO3<fadbad::F<double> >(p);
+}
+
+inline SE3<fadbad::F<double> > make_fad_se3(){
+       const Vector<6, fadbad::F<double> > p = make_fad_vector(makeVector(0.0, 
0, 0, 0, 0, 0));
+       return SE3<fadbad::F<double> >(p);
+}
+
+inline SE2<fadbad::F<double> > make_fad_se2() {
+       return SE2<fadbad::F<double> >(make_fad_vector(makeVector(0.0, 0, 0)));
+}
+
+inline SO2<fadbad::F<double> > make_fad_so2() {
+       fadbad::F<double> r = 0.0;
+       r.diff(0,1) = 1.0;
+       return SO2<fadbad::F<double> >(r);
+}
+
+inline SO3<fadbad::F<double> > make_left_fad_so3( const SO3<double> & r ){
+       return make_fad_so3() * r;
+}
+
+inline SE3<fadbad::F<double> > make_left_fad_se3( const SE3<double> & t ){
+       return make_fad_se3() * t;
+}
+
+inline SO2<fadbad::F<double> > make_left_fad_so2( const SO2<double> & r ){
+       return make_fad_so2() * r;
+}
+
+inline SE2<fadbad::F<double> > make_left_fad_se2( const SE2<double> & t ){
+       return make_fad_se2() * t;
+}
+
+}
+
+#endif // TOON_FADBAD_INTEGATION_H
+

Index: test/fadbad.cpp
===================================================================
RCS file: test/fadbad.cpp
diff -N test/fadbad.cpp
--- /dev/null   1 Jan 1970 00:00:00 -0000
+++ test/fadbad.cpp     18 Apr 2010 09:49:18 -0000      1.1
@@ -0,0 +1,45 @@
+#include <TooN/functions/fadbad.h>
+
+#include <TooN/se2.h>
+
+using namespace std;
+using namespace TooN;
+using namespace fadbad;
+
+void test() {
+    SE3<F<double> > se3id = make_fad_se3();
+    cout << "SE3 derivatives\n";
+    for(int i = 0; i < 6; ++i)
+        cout << get_derivative(se3id.get_rotation().get_matrix(), i) << 
get_derivative(se3id.get_translation(), i) <<  "\n\n";
+
+    SO3<F<double> > so3id = make_fad_so3();
+    cout << "SO3 derivatives\n";
+    for(int i = 0; i < 3; ++i)
+        cout << get_derivative(so3id.get_matrix(), i) << "\n";
+
+    SE2<F<double> > se2id = make_fad_se2();
+    cout << "SE2 derivatives\n";
+    for(int i = 0; i < 3; ++i)
+        cout << get_derivative(se2id.get_rotation().get_matrix(), i) << 
get_derivative(se2id.get_translation(), i) <<  "\n\n";
+    
+    SO2<F<double> > so2id = make_fad_so2();
+    cout << "SO2 derivatives\n";
+    cout << get_derivative(so2id.get_matrix(), 0) << "\n";
+    // cout << get_derivative((so2id * SO2<>(0.1)).get_matrix(), 0) << "\n";
+}
+
+int main(int argc, char ** argv){
+    test();
+
+#if 1
+    SE3<> id(makeVector(1,0,0,0,0,0));
+    
+    const SE3<F<double> > g = make_left_fad_se3(id);
+    for(int i = 0; i < 6; ++i)
+        cout << get_derivative(g.get_rotation().get_matrix(), i) << 
get_derivative(g.get_translation(), i) <<  "\n\n";
+
+    Vector<3> in = makeVector(1,2,3);
+    const Vector<3, F<double> > p = g * in;
+    cout << p << "\n" << get_jacobian<3,6>(p) << endl;
+#endif
+}




reply via email to

[Prev in Thread] Current Thread [Next in Thread]