TooN 2.1
functions/fadbad.h
00001 // -*- c++ -*-
00002 
00003 // Copyright (C) 2005,2009 Tom Drummond (twd20@cam.ac.uk)
00004 //
00005 // This file is part of the TooN Library.  This library is free
00006 // software; you can redistribute it and/or modify it under the
00007 // terms of the GNU General Public License as published by the
00008 // Free Software Foundation; either version 2, or (at your option)
00009 // any later version.
00010 
00011 // This library is distributed in the hope that it will be useful,
00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014 // GNU General Public License for more details.
00015 
00016 // You should have received a copy of the GNU General Public License along
00017 // with this library; see the file COPYING.  If not, write to the Free
00018 // Software Foundation, 59 Temple Place - Suite 330, Boston, MA 02111-1307,
00019 // USA.
00020 
00021 // As a special exception, you may use this file as part of a free software
00022 // library without restriction.  Specifically, if other files instantiate
00023 // templates or use macros or inline functions from this file, or you compile
00024 // this file and link it with other files to produce an executable, this
00025 // file does not by itself cause the resulting executable to be covered by
00026 // the GNU General Public License.  This exception does not however
00027 // invalidate any other reasons why the executable file might be covered by
00028 // the GNU General Public License.
00029 
00030 #ifndef TOON_FADBAD_INTEGATION_H
00031 #define TOON_FADBAD_INTEGATION_H
00032 
00033 #include <iostream>
00034 
00035 #include <TooN/TooN.h>
00036 #include <TooN/se3.h>
00037 #include <TooN/se2.h>
00038 
00039 #include <FADBAD++/fadiff.h>
00040 
00041 namespace fadbad {
00042 
00043 template <typename P, unsigned int N>
00044 inline std::ostream & operator<<( std::ostream & out, const F<P, N> & val ){
00045     return out << val.val();
00046 }
00047 
00048 template <typename P, unsigned int N>
00049 inline F<P, N> abs( const F<P, N> & val ){
00050     return (val.val() < 0) ? -val : val;
00051 }
00052 
00053 }
00054 
00055 namespace TooN {
00056 
00057 template<typename C, unsigned int N> struct IsField<fadbad::F<C, N> >
00058 {
00059     static const int value = numeric_limits<C>::is_specialized; ///<Is C a field?
00060 };
00061 
00062 template <int N, typename T, typename A, unsigned D>
00063 inline Vector<N, T> get_value( const Vector<N, fadbad::F<T, D>, A> & v ){
00064     Vector<N,T> result(v.size());
00065     for(int i = 0; i < result.size(); ++i)
00066         result[i] = v[i].val();
00067     return result;
00068 }
00069 
00070 template <typename P, int N, typename A>
00071 inline Vector<N, fadbad::F<P> > make_fad_vector( const Vector<N, P, A> & val, const unsigned start = 0, const unsigned size = N ){
00072     Vector<N, fadbad::F<P> > result = val;
00073     for(unsigned i = 0, d = start; i < val.size() && d < size; ++i, ++d)
00074         result[i].diff(d,size);
00075     return result;
00076 }
00077 
00078 template <unsigned D, typename P, int N, typename A>
00079 inline Vector<N, fadbad::F<P,D> > make_fad_vector( const Vector<N, P, A> & val, const unsigned start = 0 ){
00080     Vector<N, fadbad::F<P,D> > result = val;
00081     for(unsigned i = 0, d = start; i < unsigned(val.size()) && d < D; ++i, ++d)
00082         result[i].diff(d);
00083     return result;
00084 }
00085 
00086 template <unsigned D, typename P, int N, typename A>
00087 inline Vector<N, P> get_derivative( const Vector<N, fadbad::F<P,D>, A> & val, const int dim ){
00088     Vector<N, P> r;
00089     for(int i = 0; i < N; ++i)
00090         r[i] = val[i].deriv(dim);
00091     return r;
00092 }
00093 
00094 template <unsigned D, typename P, int N, typename A>
00095 inline Matrix<N, D, P> get_jacobian( const Vector<N, fadbad::F<P, D>,  A> & val ){
00096     Matrix<N, D, P> result(N, val[0].size());
00097     for(unsigned i = 0; i < val[0].size(); ++i)
00098         result.T()[i] = get_derivative( val, i );
00099     return result;
00100 }
00101 
00102 template <int R, int C, typename P, unsigned D, typename A>
00103 inline Matrix<R, C, P> get_derivative( const Matrix<R,C, fadbad::F<P, D>, A> & val, const int dim ){
00104     Matrix<R, C, P> result;
00105     for(int r = 0; r < R; ++r)
00106         for(int c = 0; c < C; ++c)
00107             result[r][c] = val[r][c].deriv(dim);
00108     return result;
00109 }
00110 
00111 template <typename P>
00112 inline SO3<fadbad::F<P> > make_fad_so3(int start = 0, int size = 3){
00113     // const Vector<3, fadbad::F<double> > p = make_fad_vector(makeVector(0.0, 0, 0), start, size);
00114     // return SO3<fadbad::F<double> >(p);
00115     SO3<fadbad::F<P> > r;
00116     // this is a hack
00117     Matrix<3,3,fadbad::F<P> > & m = const_cast<Matrix<3,3,fadbad::F<P> > &>(r.get_matrix());
00118     m(2,1).diff(start, size);
00119     m(1,2) = m(2,1) * -1;
00120 
00121     m(0,2).diff(start+1, size);
00122     m(2,0) = m(0,2) * -1;
00123 
00124     m(1,0).diff(start+2, size);
00125     m(0,1) = m(1,0) * -1;
00126 
00127     return r;
00128 }
00129 
00130 template <typename P, unsigned D>
00131 inline SO3<fadbad::F<P, D> > make_fad_so3(int start = 0){
00132     // const Vector<3, fadbad::F<double> > p = make_fad_vector(makeVector(0.0, 0, 0), start, size);
00133     // return SO3<fadbad::F<double> >(p);
00134     SO3<fadbad::F<P, D> > r;
00135     // this is a hack
00136     Matrix<3,3,fadbad::F<P, D> > & m = const_cast<Matrix<3,3,fadbad::F<P, D> > &>(r.get_matrix());
00137     m(2,1).diff(start);
00138     m(1,2) = m(2,1) * -1;
00139 
00140     m(0,2).diff(start+1);
00141     m(2,0) = m(0,2) * -1;
00142 
00143     m(1,0).diff(start+2);
00144     m(0,1) = m(1,0) * -1;
00145 
00146     return r;
00147 }
00148 
00149 template <typename P>
00150 inline SE3<fadbad::F<P> > make_fad_se3( int start = 0, int size = 6){
00151     return SE3<fadbad::F<P> >(make_fad_so3<P>( start+3, size ), make_fad_vector(makeVector<P>(0.0, 0, 0), start, size));
00152 }
00153 
00154 template <typename P, unsigned D>
00155 inline SE3<fadbad::F<P, D> > make_fad_se3( int start = 0){
00156     return SE3<fadbad::F<P, D> >(make_fad_so3<P, D>( start+3 ), make_fad_vector<D>(makeVector<P>(0.0, 0, 0), start));
00157 }
00158 
00159 template <typename P>
00160 inline SE2<fadbad::F<P> > make_fad_se2(int start = 0, int size = 3) {
00161     return SE2<fadbad::F<P> >(make_fad_vector(makeVector<P>(0.0, 0, 0), start, size));
00162 }
00163 
00164 template <typename P, unsigned D>
00165 inline SE2<fadbad::F<P, D> > make_fad_se2(int start = 0) {
00166     return SE2<fadbad::F<P, D> >(make_fad_vector<D>(makeVector<P>(0.0, 0, 0), start));
00167 }
00168 
00169 template <typename P>
00170 inline SO2<fadbad::F<P> > make_fad_so2(int start = 0, int size = 1) {
00171     fadbad::F<P> r = 0;
00172     r.diff(start,size) = 1;
00173     return SO2<fadbad::F<P> >(r);
00174 }
00175 
00176 template <typename P, unsigned D>
00177 inline SO2<fadbad::F<P, D> > make_fad_so2(int start = 0) {
00178     fadbad::F<P, D> r = 0;
00179     r.diff(start) = 1;
00180     return SO2<fadbad::F<P, D> >(r);
00181 }
00182 
00183 template <typename P>
00184 inline SO3<fadbad::F<P> > make_left_fad_so3( const SO3<P> & r, int start = 0, int size = 3 ){
00185     return make_fad_so3<P>(start, size) * r;
00186 }
00187 
00188 template <typename P, unsigned D>
00189 inline SO3<fadbad::F<P, D> > make_left_fad_so3( const SO3<P> & r, int start = 0){
00190     return make_fad_so3<P, D>(start) * r;
00191 }
00192 
00193 template <typename P>
00194 inline SE3<fadbad::F<P> > make_left_fad_se3( const SE3<P> & t,  int start = 0, int size = 6 ){
00195     return make_fad_se3<P>(start, size) * t;
00196 }
00197 
00198 template <typename P, unsigned D>
00199 inline SE3<fadbad::F<P, D> > make_left_fad_se3( const SE3<P> & t,  int start = 0){
00200     return make_fad_se3<P, D>(start) * t;
00201 }
00202 
00203 template <typename P>
00204 inline SO2<fadbad::F<P> > make_left_fad_so2( const SO2<P> & r, int start = 0, int size = 1 ){
00205     return make_fad_so2<P>(start, size) * r;
00206 }
00207 
00208 template <typename P, unsigned D>
00209 inline SO2<fadbad::F<P, D> > make_left_fad_so2( const SO2<P> & r, int start = 0 ){
00210     return make_fad_so2<P, D>(start) * r;
00211 }
00212 
00213 template <typename P>
00214 inline SE2<fadbad::F<P> > make_left_fad_se2( const SE2<P> & t, int start = 0, int size = 3 ){
00215     return make_fad_se2<P>(start, size) * t;
00216 }
00217 
00218 template <typename P, unsigned D>
00219 inline SE2<fadbad::F<P, D> > make_left_fad_se2( const SE2<P> & t, int start = 0){
00220     return make_fad_se2<P, D>(start) * t;
00221 }
00222 
00223 }
00224 
00225 #endif // TOON_FADBAD_INTEGATION_H
00226