se2.h

00001 // -*- c++ -*-
00002 
00003 // Copyright (C) 2005,2009 Tom Drummond (twd20@cam.ac.uk),
00004 // Ed Rosten (er258@cam.ac.uk), Gerhard Reitmayr (gr281@cam.ac.uk)
00005 //
00006 // This file is part of the TooN Library.  This library is free
00007 // software; you can redistribute it and/or modify it under the
00008 // terms of the GNU General Public License as published by the
00009 // Free Software Foundation; either version 2, or (at your option)
00010 // any later version.
00011 
00012 // This library is distributed in the hope that it will be useful,
00013 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00014 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015 // GNU General Public License for more details.
00016 
00017 // You should have received a copy of the GNU General Public License along
00018 // with this library; see the file COPYING.  If not, write to the Free
00019 // Software Foundation, 59 Temple Place - Suite 330, Boston, MA 02111-1307,
00020 // USA.
00021 
00022 // As a special exception, you may use this file as part of a free software
00023 // library without restriction.  Specifically, if other files instantiate
00024 // templates or use macros or inline functions from this file, or you compile
00025 // this file and link it with other files to produce an executable, this
00026 // file does not by itself cause the resulting executable to be covered by
00027 // the GNU General Public License.  This exception does not however
00028 // invalidate any other reasons why the executable file might be covered by
00029 // the GNU General Public License.
00030 
00031 /* This code mostly made by copying from se3.h !! */
00032 
00033 #ifndef TOON_INCLUDE_SE2_H
00034 #define TOON_INCLUDE_SE2_H
00035 
00036 #include <TooN/so2.h>
00037 
00038 
00039 namespace TooN {
00040 
00051 template <typename Precision = double>
00052 class SE2 {
00053 public:
00055     SE2() : my_translation(Zeros) {}
00056     template <class A> SE2(const SO2<Precision>& R, const Vector<2,Precision,A>& T) : my_rotation(R), my_translation(T) {}
00057     template <int S, class P, class A> SE2(const Vector<S, P, A> & v) { *this = exp(v); }
00058 
00060     SO2<Precision> & get_rotation(){return my_rotation;}
00062     const SO2<Precision> & get_rotation() const {return my_rotation;}
00064     Vector<2, Precision> & get_translation() {return my_translation;}
00066     const Vector<2, Precision> & get_translation() const {return my_translation;}
00067 
00071     template <int S, typename P, typename A>
00072     static inline SE2 exp(const Vector<S,P, A>& vect);
00073     
00076     static inline Vector<3, Precision> ln(const SE2& se2);
00078     Vector<3, Precision> ln() const { return SE2::ln(*this); }
00079 
00081     SE2 inverse() const {
00082         const SO2<Precision> & rinv = my_rotation.inverse();
00083         return SE2(rinv, -(rinv*my_translation));
00084     };
00085 
00088     inline SE2 operator *(const SE2& rhs) const { return SE2(my_rotation*rhs.my_rotation, my_translation + my_rotation*rhs.my_translation); }
00089 
00092     inline SE2& operator *=(const SE2& rhs) { 
00093         *this = *this * rhs; 
00094         return *this; 
00095     }
00096 
00102     static inline Matrix<3,3, Precision> generator(int i) {
00103         Matrix<3,3,Precision> result(Zeros);
00104         if(i < 2){
00105             result[i][2] = 1;
00106             return result;
00107         }
00108         result[0][1] = -1;
00109         result[1][0] = 1;
00110         return result;
00111     }
00112 
00115     template<typename Accessor>
00116     inline Vector<3, Precision> adjoint(const Vector<3,Precision, Accessor> & vect) const {
00117         Vector<3, Precision> result;
00118         result[2] = vect[2];
00119         result.template slice<0,2>() = my_rotation * vect.template slice<0,2>();
00120         result[0] += vect[2] * my_translation[1];
00121         result[1] -= vect[2] * my_translation[0];
00122         return result;
00123     }
00124 
00125     template <typename Accessor>
00126     inline Matrix<3,3,Precision> adjoint(const Matrix<3,3,Precision,Accessor>& M) const {
00127         Matrix<3,3,Precision> result;
00128         for(int i=0; i<3; ++i)
00129             result.T()[i] = adjoint(M.T()[i]);
00130         for(int i=0; i<3; ++i)
00131             result[i] = adjoint(result[i]);
00132         return result;
00133     }
00134 
00135 private:
00136     SO2<Precision> my_rotation;
00137     Vector<2, Precision> my_translation;
00138 };
00139 
00142 template <class Precision>
00143 inline std::ostream& operator<<(std::ostream& os, const SE2<Precision> & rhs){
00144     for(int i=0; i<2; i++)
00145         os << rhs.get_rotation().get_matrix()[i] << rhs.get_translation()[i] << std::endl;
00146     return os;
00147 }
00148 
00151 template <class Precision>
00152 inline std::istream& operator>>(std::istream& is, SE2<Precision>& rhs){
00153     for(int i=0; i<2; i++)
00154         is >> rhs.get_rotation().my_matrix[i].ref() >> rhs.get_translation()[i];
00155     rhs.get_rotation().coerce();
00156     return is;
00157 }
00158 
00159 
00161 // operator *   //
00162 // SE2 * Vector //
00164 
00165 namespace Internal {
00166 template<int S, typename P, typename PV, typename A>
00167 struct SE2VMult;
00168 }
00169 
00170 template<int S, typename P, typename PV, typename A>
00171 struct Operator<Internal::SE2VMult<S,P,PV,A> > {
00172     const SE2<P> & lhs;
00173     const Vector<S,PV,A> & rhs;
00174     
00175     Operator(const SE2<P> & l, const Vector<S,PV,A> & r ) : lhs(l), rhs(r) {}
00176     
00177     template <int S0, typename P0, typename A0>
00178     void eval(Vector<S0, P0, A0> & res ) const {
00179         SizeMismatch<3,S>::test(3, rhs.size());
00180         res.template slice<0,2>()=lhs.get_rotation()*rhs.template slice<0,2>();
00181         res.template slice<0,2>()+=lhs.get_translation() * rhs[2];
00182         res[2] = rhs[2];
00183     }
00184     int size() const { return 3; }
00185 };
00186 
00189 template<int S, typename P, typename PV, typename A>
00190 inline Vector<3, typename Internal::MultiplyType<P,PV>::type> operator*(const SE2<P> & lhs, const Vector<S,PV,A>& rhs){
00191     return Vector<3, typename Internal::MultiplyType<P,PV>::type>(Operator<Internal::SE2VMult<S,P,PV,A> >(lhs,rhs));
00192 }
00193 
00196 template <typename P, typename PV, typename A>
00197 inline Vector<2, typename Internal::MultiplyType<P,PV>::type> operator*(const SE2<P>& lhs, const Vector<2,PV,A>& rhs){
00198     return lhs.get_translation() + lhs.get_rotation() * rhs;
00199 }
00200 
00202 // operator *   //
00203 // Vector * SE2 //
00205 
00206 namespace Internal {
00207 template<int S, typename P, typename PV, typename A>
00208 struct VSE2Mult;
00209 }
00210 
00211 template<int S, typename P, typename PV, typename A>
00212 struct Operator<Internal::VSE2Mult<S,P,PV,A> > {
00213     const Vector<S,PV,A> & lhs;
00214     const SE2<P> & rhs;
00215     
00216     Operator(const Vector<S,PV,A> & l, const SE2<P> & r ) : lhs(l), rhs(r) {}
00217     
00218     template <int S0, typename P0, typename A0>
00219     void eval(Vector<S0, P0, A0> & res ) const {
00220         SizeMismatch<3,S>::test(3, lhs.size());
00221         res.template slice<0,2>() = lhs.template slice<0,2>()*rhs.get_rotation();
00222         res[2] = lhs[2];
00223         res[2] += lhs.template slice<0,2>() * rhs.get_translation();
00224     }
00225     int size() const { return 3; }
00226 };
00227 
00230 template<int S, typename P, typename PV, typename A>
00231 inline Vector<3, typename Internal::MultiplyType<PV,P>::type> operator*(const Vector<S,PV,A>& lhs, const SE2<P> & rhs){
00232     return Vector<3, typename Internal::MultiplyType<PV,P>::type>(Operator<Internal::VSE2Mult<S, P,PV,A> >(lhs,rhs));
00233 }
00234 
00236 // operator *   //
00237 // SE2 * Matrix //
00239 
00240 namespace Internal {
00241 template <int R, int C, typename PM, typename A, typename P>
00242 struct SE2MMult;
00243 }
00244 
00245 template<int R, int Cols, typename PM, typename A, typename P>
00246 struct Operator<Internal::SE2MMult<R, Cols, PM, A, P> > {
00247     const SE2<P> & lhs;
00248     const Matrix<R,Cols,PM,A> & rhs;
00249     
00250     Operator(const SE2<P> & l, const Matrix<R,Cols,PM,A> & r ) : lhs(l), rhs(r) {}
00251     
00252     template <int R0, int C0, typename P0, typename A0>
00253     void eval(Matrix<R0, C0, P0, A0> & res ) const {
00254         SizeMismatch<3,R>::test(3, rhs.num_rows());
00255         for(int i=0; i<rhs.num_cols(); ++i)
00256             res.T()[i] = lhs * rhs.T()[i];
00257     }
00258     int num_cols() const { return rhs.num_cols(); }
00259     int num_rows() const { return 3; }
00260 };
00261 
00264 template <int R, int Cols, typename PM, typename A, typename P> 
00265 inline Matrix<3,Cols, typename Internal::MultiplyType<P,PM>::type> operator*(const SE2<P> & lhs, const Matrix<R,Cols,PM, A>& rhs){
00266     return Matrix<3,Cols,typename Internal::MultiplyType<P,PM>::type>(Operator<Internal::SE2MMult<R, Cols, PM, A, P> >(lhs,rhs));
00267 }
00268 
00270 // operator *   //
00271 // Matrix * SE2 //
00273 
00274 namespace Internal {
00275 template <int Rows, int C, typename PM, typename A, typename P>
00276 struct MSE2Mult;
00277 }
00278 
00279 template<int Rows, int C, typename PM, typename A, typename P>
00280 struct Operator<Internal::MSE2Mult<Rows, C, PM, A, P> > {
00281     const Matrix<Rows,C,PM,A> & lhs;
00282     const SE2<P> & rhs;
00283     
00284     Operator( const Matrix<Rows,C,PM,A> & l, const SE2<P> & r ) : lhs(l), rhs(r) {}
00285     
00286     template <int R0, int C0, typename P0, typename A0>
00287     void eval(Matrix<R0, C0, P0, A0> & res ) const {
00288         SizeMismatch<3, C>::test(3, lhs.num_cols());
00289         for(int i=0; i<lhs.num_rows(); ++i)
00290             res[i] = lhs[i] * rhs;
00291     }
00292     int num_cols() const { return 3; }
00293     int num_rows() const { return lhs.num_rows(); }
00294 };
00295 
00298 template <int Rows, int C, typename PM, typename A, typename P> 
00299 inline Matrix<Rows,3, typename Internal::MultiplyType<PM,P>::type> operator*(const Matrix<Rows,C,PM, A>& lhs, const SE2<P> & rhs ){
00300     return Matrix<Rows,3,typename Internal::MultiplyType<PM,P>::type>(Operator<Internal::MSE2Mult<Rows, C, PM, A, P> >(lhs,rhs));
00301 }
00302 
00303 /* inline SE2 SE2::exp(const Vector<3>& mu){ */
00304 /*   SE2 result; */
00305 /*   double theta = mu[2]; */
00306 /*   result.get_rotation() = SO2::exp(theta); */
00307 /*   Matrix<2> m2; */
00308 /*   m2[0][0] = m2[1][1] = result.get_rotation().get_matrix()[1][0]; */
00309 /*   m2[0][1] = result.get_rotation().get_matrix()[0][0] - 1.0; */
00310 /*   m2[1][0] = - m2[0][1]; */
00311 /*   if(theta != 0.0)  */
00312 /*     result.get_translation() = m2 * mu.slice<0,2>() / fabs(theta); */
00313 /*   else */
00314 /*     result.get_translation() = mu.slice<0,2>(); */
00315 /*   return result; */
00316 /* } */
00317 
00318 template <typename Precision>
00319 template <int S, typename PV, typename Accessor>
00320 inline SE2<Precision> SE2<Precision>::exp(const Vector<S, PV, Accessor>& mu)
00321 {
00322     SizeMismatch<3,S>::test(3, mu.size());
00323 
00324     static const Precision one_6th = 1.0/6.0;
00325     static const Precision one_20th = 1.0/20.0;
00326   
00327     SE2<Precision> result;
00328   
00329     const Precision theta = mu[2];
00330     const Precision theta_sq = theta * theta;
00331   
00332     const Vector<2, Precision> cross = makeVector( -theta * mu[1], theta * mu[0]);
00333     result.get_rotation() = SO2<Precision>::exp(theta);
00334 
00335     if (theta_sq < 1e-8){
00336         result.get_translation() = mu.template slice<0,2>() + 0.5 * cross;
00337     } else {
00338         Precision A, B;
00339         if (theta_sq < 1e-6) {
00340             A = 1.0 - theta_sq * one_6th*(1.0 - one_20th * theta_sq);
00341             B = 0.5 - 0.25 * one_6th * theta_sq;
00342         } else {
00343             const Precision inv_theta = (1.0/theta);
00344             const Precision sine = result.my_rotation.get_matrix()[1][0];
00345             const Precision cosine = result.my_rotation.get_matrix()[0][0];
00346             A = sine * inv_theta;
00347             B = (1 - cosine) * (inv_theta * inv_theta);
00348         }
00349         result.get_translation() = A * mu.template slice<0,2>() + B * cross;
00350     }
00351     return result;
00352 }
00353  
00354 template <typename Precision>
00355 inline Vector<3, Precision> SE2<Precision>::ln(const SE2<Precision> & se2) {
00356     const Precision theta = se2.get_rotation().ln();
00357 
00358     Precision shtot = 0.5;  
00359     if(fabs(theta) > 0.00001)
00360         shtot = sin(theta/2)/theta;
00361 
00362     const SO2<Precision> halfrotator(theta * -0.5);
00363     Vector<3, Precision> result;
00364     result.template slice<0,2>() = (halfrotator * se2.get_translation())/(2 * shtot);
00365     result[2] = theta;
00366     return result;
00367 }
00368 
00372 template <typename Precision>
00373 inline SE2<Precision> operator*(const SO2<Precision> & lhs, const SE2<Precision>& rhs){
00374     return SE2<Precision>( lhs*rhs.get_rotation(), lhs*rhs.get_translation());
00375 }
00376 
00377 }
00378 #endif

Generated on Thu May 7 20:28:41 2009 for TooN by  doxygen 1.5.3