TooN: se2.h Source File

TooN 2.0.0-beta8

- Main Page
- Related Pages
- Modules
- Namespaces
- Classes
- Files

- File List

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
    00041 /// Represent a two-dimensional Euclidean transformation (a rotation and a translation).
    00042 /// This can be represented by a \f$2\times 3\f$ matrix operating on a homogeneous co-ordinate,
    00043 /// so that a vector \f$\underline{x}\f$ is transformed to a new location \f$\underline{x}'\f$
    00044 /// by
    00045 /// \f[\begin{aligned}\underline{x}' &= E\times\underline{x}\\ \begin{bmatrix}x'\\y'\end{bmatrix} &= \begin{pmatrix}r_{11} & r_{12} & t_1\\r_{21} & r_{22} & t_2\end{pmatrix}\begin{bmatrix}x\\y\\1\end{bmatrix}\end{aligned}\f]
    00046 ///
    00047 /// This transformation is a member of the Special Euclidean Lie group SE2. These can be parameterised with
    00048 /// three numbers (in the space of the Lie Algebra). In this class, the first two parameters are a
    00049 /// translation vector while the third is the amount of rotation in the plane as for SO2.
    00050 /// @ingroup gTransforms
    00051 template <typename Precision = DefaultPrecision>
    00052 class SE2 {
    00053 public:
    00054     /// Default constructor. Initialises the the rotation to zero (the identity) and the translation to zero
    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
    00059     /// Returns the rotation part of the transformation as a SO2
    00060     SO2<Precision> & get_rotation(){return my_rotation;}
    00061     /// @overload
    00062     const SO2<Precision> & get_rotation() const {return my_rotation;}
    00063     /// Returns the translation part of the transformation as a Vector
    00064     Vector<2, Precision> & get_translation() {return my_translation;}
    00065     /// @overload
    00066     const Vector<2, Precision> & get_translation() const {return my_translation;}
    00067
    00068     /// Exponentiate a Vector in the Lie Algebra to generate a new SE2.
    00069     /// See the Detailed Description for details of this vector.
    00070     /// @param vect The Vector to exponentiate
    00071     template <int S, typename P, typename A>
    00072     static inline SE2 exp(const Vector<S,P, A>& vect);
    00073
    00074     /// Take the logarithm of the matrix, generating the corresponding vector in the Lie Algebra.
    00075     /// See the Detailed Description for details of this vector.
    00076     static inline Vector<3, Precision> ln(const SE2& se2);
    00077     /// @overload
    00078     Vector<3, Precision> ln() const { return SE2::ln(*this); }
    00079
    00080     /// compute the inverse of the transformation
    00081     SE2 inverse() const {
    00082         const SO2<Precision> & rinv = my_rotation.inverse();
    00083         return SE2(rinv, -(rinv*my_translation));
    00084     };
    00085
    00086     /// Right-multiply by another SE2 (concatenate the two transformations)
    00087     /// @param rhs The multipier
    00088     template <typename P>
    00089     SE2<typename Internal::MultiplyType<Precision,P>::type> operator *(const SE2<P>& rhs) const {
    00090         return SE2<typename Internal::MultiplyType<Precision,P>::type>(my_rotation*rhs.get_rotation(), my_translation + my_rotation*rhs.get_translation());
    00091     }
    00092
    00093     /// Self right-multiply by another SE2 (concatenate the two transformations)
    00094     /// @param rhs The multipier
    00095     template <typename P>
    00096     inline SE2& operator *=(const SE2<P>& rhs) {
    00097         *this = *this * rhs;
    00098         return *this;
    00099     }
    00100
    00101     /// returns the generators for the Lie group. These are a set of matrices that
    00102     /// form a basis for the vector space of the Lie algebra.
    00103     /// - 0 is translation in x
    00104     /// - 1 is translation in y
    00105     /// - 2 is rotation in the plane
    00106     static inline Matrix<3,3, Precision> generator(int i) {
    00107         Matrix<3,3,Precision> result(Zeros);
    00108         if(i < 2){
    00109             result[i][2] = 1;
    00110             return result;
    00111         }
    00112         result[0][1] = -1;
    00113         result[1][0] = 1;
    00114         return result;
    00115     }
    00116
    00117     /// transfers a vector in the Lie algebra, from one coord frame to another
    00118     /// so that exp(adjoint(vect)) = (*this) * exp(vect) * (this->inverse())
    00119     template<typename Accessor>
    00120     Vector<3, Precision> adjoint(const Vector<3,Precision, Accessor> & vect) const {
    00121         Vector<3, Precision> result;
    00122         result[2] = vect[2];
    00123         result.template slice<0,2>() = my_rotation * vect.template slice<0,2>();
    00124         result[0] += vect[2] * my_translation[1];
    00125         result[1] -= vect[2] * my_translation[0];
    00126         return result;
    00127     }
    00128
    00129     template <typename Accessor>
    00130     Matrix<3,3,Precision> adjoint(const Matrix<3,3,Precision,Accessor>& M) const {
    00131         Matrix<3,3,Precision> result;
    00132         for(int i=0; i<3; ++i)
    00133             result.T()[i] = adjoint(M.T()[i]);
    00134         for(int i=0; i<3; ++i)
    00135             result[i] = adjoint(result[i]);
    00136         return result;
    00137     }
    00138
    00139 private:
    00140     SO2<Precision> my_rotation;
    00141     Vector<2, Precision> my_translation;
    00142 };
    00143
    00144 /// Write an SE2 to a stream
    00145 /// @relates SE2
    00146 template <class Precision>
    00147 inline std::ostream& operator<<(std::ostream& os, const SE2<Precision> & rhs){
    00148     std::streamsize fw = os.width();
    00149     for(int i=0; i<2; i++){
    00150         os.width(fw);
    00151         os << rhs.get_rotation().get_matrix()[i];
    00152         os.width(fw);
    00153         os << rhs.get_translation()[i] << '\n';
    00154     }
    00155     return os;
    00156 }
    00157
    00158 /// Read an SE2 from a stream
    00159 /// @relates SE2
    00160 template <class Precision>
    00161 inline std::istream& operator>>(std::istream& is, SE2<Precision>& rhs){
    00162     for(int i=0; i<2; i++)
    00163         is >> rhs.get_rotation().my_matrix[i].ref() >> rhs.get_translation()[i];
    00164     rhs.get_rotation().coerce();
    00165     return is;
    00166 }
    00167
    00168
    00169 //////////////////
    00170 // operator *   //
    00171 // SE2 * Vector //
    00172 //////////////////
    00173
    00174 namespace Internal {
    00175 template<int S, typename P, typename PV, typename A>
    00176 struct SE2VMult;
    00177 }
    00178
    00179 template<int S, typename P, typename PV, typename A>
    00180 struct Operator<Internal::SE2VMult<S,P,PV,A> > {
    00181     const SE2<P> & lhs;
    00182     const Vector<S,PV,A> & rhs;
    00183
    00184     Operator(const SE2<P> & l, const Vector<S,PV,A> & r ) : lhs(l), rhs(r) {}
    00185
    00186     template <int S0, typename P0, typename A0>
    00187     void eval(Vector<S0, P0, A0> & res ) const {
    00188         SizeMismatch<3,S>::test(3, rhs.size());
    00189         res.template slice<0,2>()=lhs.get_rotation()*rhs.template slice<0,2>();
    00190         res.template slice<0,2>()+=lhs.get_translation() * rhs[2];
    00191         res[2] = rhs[2];
    00192     }
    00193     int size() const { return 3; }
    00194 };
    00195
    00196 /// Right-multiply with a Vector<3>
    00197 /// @relates SE2
    00198 template<int S, typename P, typename PV, typename A>
    00199 inline Vector<3, typename Internal::MultiplyType<P,PV>::type> operator*(const SE2<P> & lhs, const Vector<S,PV,A>& rhs){
    00200     return Vector<3, typename Internal::MultiplyType<P,PV>::type>(Operator<Internal::SE2VMult<S,P,PV,A> >(lhs,rhs));
    00201 }
    00202
    00203 /// Right-multiply with a Vector<2> (special case, extended to be a homogeneous vector)
    00204 /// @relates SE2
    00205 template <typename P, typename PV, typename A>
    00206 inline Vector<2, typename Internal::MultiplyType<P,PV>::type> operator*(const SE2<P>& lhs, const Vector<2,PV,A>& rhs){
    00207     return lhs.get_translation() + lhs.get_rotation() * rhs;
    00208 }
    00209
    00210 //////////////////
    00211 // operator *   //
    00212 // Vector * SE2 //
    00213 //////////////////
    00214
    00215 namespace Internal {
    00216 template<int S, typename P, typename PV, typename A>
    00217 struct VSE2Mult;
    00218 }
    00219
    00220 template<int S, typename P, typename PV, typename A>
    00221 struct Operator<Internal::VSE2Mult<S,P,PV,A> > {
    00222     const Vector<S,PV,A> & lhs;
    00223     const SE2<P> & rhs;
    00224
    00225     Operator(const Vector<S,PV,A> & l, const SE2<P> & r ) : lhs(l), rhs(r) {}
    00226
    00227     template <int S0, typename P0, typename A0>
    00228     void eval(Vector<S0, P0, A0> & res ) const {
    00229         SizeMismatch<3,S>::test(3, lhs.size());
    00230         res.template slice<0,2>() = lhs.template slice<0,2>()*rhs.get_rotation();
    00231         res[2] = lhs[2];
    00232         res[2] += lhs.template slice<0,2>() * rhs.get_translation();
    00233     }
    00234     int size() const { return 3; }
    00235 };
    00236
    00237 /// Left-multiply with a Vector<3>
    00238 /// @relates SE2
    00239 template<int S, typename P, typename PV, typename A>
    00240 inline Vector<3, typename Internal::MultiplyType<PV,P>::type> operator*(const Vector<S,PV,A>& lhs, const SE2<P> & rhs){
    00241     return Vector<3, typename Internal::MultiplyType<PV,P>::type>(Operator<Internal::VSE2Mult<S, P,PV,A> >(lhs,rhs));
    00242 }
    00243
    00244 //////////////////
    00245 // operator *   //
    00246 // SE2 * Matrix //
    00247 //////////////////
    00248
    00249 namespace Internal {
    00250 template <int R, int C, typename PM, typename A, typename P>
    00251 struct SE2MMult;
    00252 }
    00253
    00254 template<int R, int Cols, typename PM, typename A, typename P>
    00255 struct Operator<Internal::SE2MMult<R, Cols, PM, A, P> > {
    00256     const SE2<P> & lhs;
    00257     const Matrix<R,Cols,PM,A> & rhs;
    00258
    00259     Operator(const SE2<P> & l, const Matrix<R,Cols,PM,A> & r ) : lhs(l), rhs(r) {}
    00260
    00261     template <int R0, int C0, typename P0, typename A0>
    00262     void eval(Matrix<R0, C0, P0, A0> & res ) const {
    00263         SizeMismatch<3,R>::test(3, rhs.num_rows());
    00264         for(int i=0; i<rhs.num_cols(); ++i)
    00265             res.T()[i] = lhs * rhs.T()[i];
    00266     }
    00267     int num_cols() const { return rhs.num_cols(); }
    00268     int num_rows() const { return 3; }
    00269 };
    00270
    00271 /// Right-multiply with a Matrix<3>
    00272 /// @relates SE2
    00273 template <int R, int Cols, typename PM, typename A, typename P>
    00274 inline Matrix<3,Cols, typename Internal::MultiplyType<P,PM>::type> operator*(const SE2<P> & lhs, const Matrix<R,Cols,PM, A>& rhs){
    00275     return Matrix<3,Cols,typename Internal::MultiplyType<P,PM>::type>(Operator<Internal::SE2MMult<R, Cols, PM, A, P> >(lhs,rhs));
    00276 }
    00277
    00278 //////////////////
    00279 // operator *   //
    00280 // Matrix * SE2 //
    00281 //////////////////
    00282
    00283 namespace Internal {
    00284 template <int Rows, int C, typename PM, typename A, typename P>
    00285 struct MSE2Mult;
    00286 }
    00287
    00288 template<int Rows, int C, typename PM, typename A, typename P>
    00289 struct Operator<Internal::MSE2Mult<Rows, C, PM, A, P> > {
    00290     const Matrix<Rows,C,PM,A> & lhs;
    00291     const SE2<P> & rhs;
    00292
    00293     Operator( const Matrix<Rows,C,PM,A> & l, const SE2<P> & r ) : lhs(l), rhs(r) {}
    00294
    00295     template <int R0, int C0, typename P0, typename A0>
    00296     void eval(Matrix<R0, C0, P0, A0> & res ) const {
    00297         SizeMismatch<3, C>::test(3, lhs.num_cols());
    00298         for(int i=0; i<lhs.num_rows(); ++i)
    00299             res[i] = lhs[i] * rhs;
    00300     }
    00301     int num_cols() const { return 3; }
    00302     int num_rows() const { return lhs.num_rows(); }
    00303 };
    00304
    00305 /// Left-multiply with a Matrix<3>
    00306 /// @relates SE2
    00307 template <int Rows, int C, typename PM, typename A, typename P>
    00308 inline Matrix<Rows,3, typename Internal::MultiplyType<PM,P>::type> operator*(const Matrix<Rows,C,PM, A>& lhs, const SE2<P> & rhs ){
    00309     return Matrix<Rows,3,typename Internal::MultiplyType<PM,P>::type>(Operator<Internal::MSE2Mult<Rows, C, PM, A, P> >(lhs,rhs));
    00310 }
    00311
    00312 template <typename Precision>
    00313 template <int S, typename PV, typename Accessor>
    00314 inline SE2<Precision> SE2<Precision>::exp(const Vector<S, PV, Accessor>& mu)
    00315 {
    00316     SizeMismatch<3,S>::test(3, mu.size());
    00317
    00318     static const Precision one_6th = 1.0/6.0;
    00319     static const Precision one_20th = 1.0/20.0;
    00320
    00321     SE2<Precision> result;
    00322
    00323     const Precision theta = mu[2];
    00324     const Precision theta_sq = theta * theta;
    00325
    00326     const Vector<2, Precision> cross = makeVector( -theta * mu[1], theta * mu[0]);
    00327     result.get_rotation() = SO2<Precision>::exp(theta);
    00328
    00329     if (theta_sq < 1e-8){
    00330         result.get_translation() = mu.template slice<0,2>() + 0.5 * cross;
    00331     } else {
    00332         Precision A, B;
    00333         if (theta_sq < 1e-6) {
    00334             A = 1.0 - theta_sq * one_6th*(1.0 - one_20th * theta_sq);
    00335             B = 0.5 - 0.25 * one_6th * theta_sq;
    00336         } else {
    00337             const Precision inv_theta = (1.0/theta);
    00338             const Precision sine = result.my_rotation.get_matrix()[1][0];
    00339             const Precision cosine = result.my_rotation.get_matrix()[0][0];
    00340             A = sine * inv_theta;
    00341             B = (1 - cosine) * (inv_theta * inv_theta);
    00342         }
    00343         result.get_translation() = TooN::operator*(A,mu.template slice<0,2>()) + TooN::operator*(B,cross);
    00344     }
    00345     return result;
    00346 }
    00347
    00348 template <typename Precision>
    00349 inline Vector<3, Precision> SE2<Precision>::ln(const SE2<Precision> & se2) {
    00350     const Precision theta = se2.get_rotation().ln();
    00351
    00352     Precision shtot = 0.5;
    00353     if(fabs(theta) > 0.00001)
    00354         shtot = sin(theta/2)/theta;
    00355
    00356     const SO2<Precision> halfrotator(theta * -0.5);
    00357     Vector<3, Precision> result;
    00358     result.template slice<0,2>() = (halfrotator * se2.get_translation())/(2 * shtot);
    00359     result[2] = theta;
    00360     return result;
    00361 }
    00362
    00363 /// Multiply a SO2 with and SE2
    00364 /// @relates SE2
    00365 /// @relates SO2
    00366 template <typename Precision>
    00367 inline SE2<Precision> operator*(const SO2<Precision> & lhs, const SE2<Precision>& rhs){
    00368     return SE2<Precision>( lhs*rhs.get_rotation(), lhs*rhs.get_translation());
    00369 }
    00370
    00371 }
    00372 #endif