[Top][All Lists]
[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
+}
[Prev in Thread] |
Current Thread |
[Next in Thread] |
- [Toon-members] TooN helpers.h se2.h se3.h so2.h so3.h internal...,
Gerhard Reitmayr <=