[Toon-members] TooN se3.h functions/fadbad.h

[Gerhard Reitmayr]

CVSROOT:        /cvsroot/toon
Module name:    TooN
Changes by:     Gerhard Reitmayr <gerhard>      11/05/02 21:46:10

Modified files:
        .              : se3.h 
        functions      : fadbad.h 

Log message:
        FADBAD integration for fixed number of derivatives, much faster

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/TooN/se3.h?cvsroot=toon&r1=1.33&r2=1.34
http://cvs.savannah.gnu.org/viewcvs/TooN/functions/fadbad.h?cvsroot=toon&r1=1.4&r2=1.5

Patches:
Index: se3.h
===================================================================
RCS file: /cvsroot/toon/TooN/se3.h,v
retrieving revision 1.33
retrieving revision 1.34
diff -u -b -r1.33 -r1.34
--- se3.h       19 Apr 2011 09:43:43 -0000      1.33
+++ se3.h       2 May 2011 21:46:10 -0000       1.34
@@ -267,7 +267,7 @@
        void eval(Vector<S0, P0, A0> & res ) const {
                SizeMismatch<4,S>::test(4, rhs.size());
                res.template slice<0,3>()=lhs.get_rotation() * rhs.template 
slice<0,3>();
-               res.template slice<0,3>()+=lhs.get_translation() * rhs[3];
+               res.template 
slice<0,3>()+=TooN::operator*(lhs.get_translation(),rhs[3]);
                res[3] = rhs[3];
        }
        int size() const { return 4; }

Index: functions/fadbad.h
===================================================================
RCS file: /cvsroot/toon/TooN/functions/fadbad.h,v
retrieving revision 1.4
retrieving revision 1.5
diff -u -b -r1.4 -r1.5
--- functions/fadbad.h  1 May 2011 15:27:56 -0000       1.4
+++ functions/fadbad.h  2 May 2011 21:46:10 -0000       1.5
@@ -40,13 +40,13 @@
 
 namespace fadbad {
 
-template <typename P>
-inline std::ostream & operator<<( std::ostream & out, const F<P> & val ){
+template <typename P, unsigned int N>
+inline std::ostream & operator<<( std::ostream & out, const F<P, N> & val ){
        return out << val.val();
 }
 
-template <typename P>
-inline F<P> abs( const F<P> & val ){
+template <typename P, unsigned int N>
+inline F<P, N> abs( const F<P, N> & val ){
     return (val.val() < 0) ? -val : val;
 }
 
@@ -54,59 +54,67 @@
 
 namespace TooN {
 
-template<class C> struct IsField<fadbad::F<C> >
+template<typename C, unsigned int N> struct IsField<fadbad::F<C, N> >
 {
        static const int value = numeric_limits<C>::is_specialized; ///<Is C a 
field?
 };
 
-template <int N, typename T, typename A>
-inline Vector<N, T> get_value( const Vector<N, fadbad::F<T>, A> & v ){
+template <int N, typename T, typename A, unsigned D>
+inline Vector<N, T> get_value( const Vector<N, fadbad::F<T, D>, A> & v ){
        Vector<N,T> result(v.size());
        for(int i = 0; i < result.size(); ++i)
                result[i] = v[i].val();
        return result;
 }
 
-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 = 0, d = start; i < val.size() && d < size; ++i, ++d)
+template <typename P, int N, typename A>
+inline Vector<N, fadbad::F<P> > make_fad_vector( const Vector<N, P, A> & val, 
const unsigned start = 0, const unsigned size = N ){
+       Vector<N, fadbad::F<P> > result = val;
+       for(unsigned i = 0, d = start; i < val.size() && d < size; ++i, ++d)
                result[i].diff(d,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;
+template <unsigned D, typename P, int N, typename A>
+inline Vector<N, fadbad::F<P,D> > make_fad_vector( const Vector<N, P, A> & 
val, const unsigned start = 0 ){
+       Vector<N, fadbad::F<P,D> > result = val;
+       for(unsigned i = 0, d = start; i < unsigned(val.size()) && d < D; ++i, 
++d)
+               result[i].diff(d);
+       return result;
+}
+
+template <unsigned D, typename P, int N, typename A>
+inline Vector<N, P> get_derivative( const Vector<N, fadbad::F<P,D>, A> & val, 
const int dim ){
+       Vector<N, P> r;
        for(int i = 0; i < N; ++i)
-               r[i] = val[i].deriv(D);
+               r[i] = val[i].deriv(dim);
        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)
+template <unsigned D, typename P, int N, typename A>
+inline Matrix<N, D, P> get_jacobian( const Vector<N, fadbad::F<P, D>,  A> & 
val ){
+       Matrix<N, D, P> result(N, val[0].size());
+       for(unsigned i = 0; i < val[0].size(); ++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;
+template <int R, int C, typename P, unsigned D, typename A>
+inline Matrix<R, C, P> get_derivative( const Matrix<R,C, fadbad::F<P, D>, A> & 
val, const int dim ){
+       Matrix<R, C, P> result;
        for(int r = 0; r < R; ++r)
                for(int c = 0; c < C; ++c)
-                       result[r][c] = val[r][c].deriv(D);
+                       result[r][c] = val[r][c].deriv(dim);
        return result;
 }
 
-inline SO3<fadbad::F<double> > make_fad_so3(int start = 0, int size = 3){
+template <typename P>
+inline SO3<fadbad::F<P> > make_fad_so3(int start = 0, int size = 3){
        // const Vector<3, fadbad::F<double> > p = 
make_fad_vector(makeVector(0.0, 0, 0), start, size);
        // return SO3<fadbad::F<double> >(p);
-       SO3<fadbad::F<double> > r;
+       SO3<fadbad::F<P> > r;
        // this is a hack
-       Matrix<3,3,fadbad::F<double> > & m = 
const_cast<Matrix<3,3,fadbad::F<double> > &>(r.get_matrix());
+       Matrix<3,3,fadbad::F<double> > & m = const_cast<Matrix<3,3,fadbad::F<P> 
> &>(r.get_matrix());
        m(2,1).diff(start, size);
        m(1,2) = m(2,1) * -1;
 
@@ -119,34 +127,97 @@
        return r;
 }
 
-inline SE3<fadbad::F<double> > make_fad_se3( int start = 0, int size = 6){
-       return SE3<fadbad::F<double> >(make_fad_so3( start+3, size ), 
make_fad_vector(makeVector(0.0, 0, 0), start, size));
+template <typename P, unsigned D>
+inline SO3<fadbad::F<P, D> > make_fad_so3(int start = 0){
+       // const Vector<3, fadbad::F<double> > p = 
make_fad_vector(makeVector(0.0, 0, 0), start, size);
+       // return SO3<fadbad::F<double> >(p);
+       SO3<fadbad::F<P, D> > r;
+       // this is a hack
+       Matrix<3,3,fadbad::F<P, D> > & m = const_cast<Matrix<3,3,fadbad::F<P, 
D> > &>(r.get_matrix());
+       m(2,1).diff(start);
+       m(1,2) = m(2,1) * -1;
+
+       m(0,2).diff(start+1);
+       m(2,0) = m(0,2) * -1;
+
+       m(1,0).diff(start+2);
+       m(0,1) = m(1,0) * -1;
+
+       return r;
 }
 
-inline SE2<fadbad::F<double> > make_fad_se2(int start = 0, int size = 3) {
-       return SE2<fadbad::F<double> >(make_fad_vector(makeVector(0.0, 0, 0), 
start, size));
+template <typename P>
+inline SE3<fadbad::F<P> > make_fad_se3( int start = 0, int size = 6){
+       return SE3<fadbad::F<P> >(make_fad_so3<P>( start+3, size ), 
make_fad_vector(makeVector<P>(0.0, 0, 0), start, size));
 }
 
-inline SO2<fadbad::F<double> > make_fad_so2(int start = 0, int size = 1) {
-       fadbad::F<double> r = 0.0;
-       r.diff(start,size) = 1.0;
-       return SO2<fadbad::F<double> >(r);
+template <typename P, unsigned D>
+inline SE3<fadbad::F<P, D> > make_fad_se3( int start = 0){
+       return SE3<fadbad::F<P, D> >(make_fad_so3<P, D>( start+3 ), 
make_fad_vector<D>(makeVector<P>(0.0, 0, 0), start));
 }
 
-inline SO3<fadbad::F<double> > make_left_fad_so3( const SO3<double> & r, int 
start = 0, int size = 3 ){
-       return make_fad_so3(start, size) * r;
+template <typename P>
+inline SE2<fadbad::F<P> > make_fad_se2(int start = 0, int size = 3) {
+       return SE2<fadbad::F<P> >(make_fad_vector(makeVector<P>(0.0, 0, 0), 
start, size));
 }
 
-inline SE3<fadbad::F<double> > make_left_fad_se3( const SE3<double> & t,  int 
start = 0, int size = 6 ){
-       return make_fad_se3(start, size) * t;
+template <typename P, unsigned D>
+inline SE2<fadbad::F<P, D> > make_fad_se2(int start = 0) {
+       return SE2<fadbad::F<P, D> >(make_fad_vector<D>(makeVector<P>(0.0, 0, 
0), start));
 }
 
-inline SO2<fadbad::F<double> > make_left_fad_so2( const SO2<double> & r, int 
start = 0, int size = 1 ){
-       return make_fad_so2(start, size) * r;
+template <typename P>
+inline SO2<fadbad::F<P> > make_fad_so2(int start = 0, int size = 1) {
+       fadbad::F<P> r = 0;
+       r.diff(start,size) = 1;
+       return SO2<fadbad::F<P> >(r);
+}
+
+template <typename P, unsigned D>
+inline SO2<fadbad::F<P, D> > make_fad_so2(int start = 0) {
+       fadbad::F<P, D> r = 0;
+       r.diff(start) = 1;
+       return SO2<fadbad::F<P, D> >(r);
+}
+
+template <typename P>
+inline SO3<fadbad::F<P> > make_left_fad_so3( const SO3<P> & r, int start = 0, 
int size = 3 ){
+       return make_fad_so3<P>(start, size) * r;
+}
+
+template <typename P, unsigned D>
+inline SO3<fadbad::F<P, D> > make_left_fad_so3( const SO3<P> & r, int start = 
0){
+       return make_fad_so3<P, D>(start) * r;
+}
+
+template <typename P>
+inline SE3<fadbad::F<P> > make_left_fad_se3( const SE3<P> & t,  int start = 0, 
int size = 6 ){
+       return make_fad_se3<P>(start, size) * t;
+}
+
+template <typename P, unsigned D>
+inline SE3<fadbad::F<P, D> > make_left_fad_se3( const SE3<P> & t,  int start = 
0){
+       return make_fad_se3<P, D>(start) * t;
+}
+
+template <typename P>
+inline SO2<fadbad::F<P> > make_left_fad_so2( const SO2<P> & r, int start = 0, 
int size = 1 ){
+       return make_fad_so2<P>(start, size) * r;
+}
+
+template <typename P, unsigned D>
+inline SO2<fadbad::F<P, D> > make_left_fad_so2( const SO2<P> & r, int start = 
0 ){
+       return make_fad_so2<P, D>(start) * r;
+}
+
+template <typename P>
+inline SE2<fadbad::F<P> > make_left_fad_se2( const SE2<P> & t, int start = 0, 
int size = 3 ){
+       return make_fad_se2<P>(start, size) * t;
 }
 
-inline SE2<fadbad::F<double> > make_left_fad_se2( const SE2<double> & t, int 
start = 0, int size = 3 ){
-       return make_fad_se2(start, size) * t;
+template <typename P, unsigned D>
+inline SE2<fadbad::F<P, D> > make_left_fad_se2( const SE2<P> & t, int start = 
0){
+       return make_fad_se2<P, D>(start) * t;
 }
 
 }