TooN: sim2.h Source File

TooN 2.0.0-beta8

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

- File List

sim2.h

    00001 // -*- c++ -*-
    00002
    00003 // Copyright (C) 2011 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 sim3.h !! */
    00032
    00033 #ifndef TOON_INCLUDE_SIM2_H
    00034 #define TOON_INCLUDE_SIM2_H
    00035
    00036 #include <TooN/se2.h>
    00037 #include <TooN/sim3.h>
    00038
    00039 namespace TooN {
    00040
    00041 /// Represent a two-dimensional Similarity transformation (a rotation, a uniform scale 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 Lie group SIM2. These can be parameterised with
    00048 /// four 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. The forth is the logarithm of the scale factor.
    00050 /// @ingroup gTransforms
    00051 template <typename Precision = DefaultPrecision>
    00052 class SIM2 {
    00053 public:
    00054     /// Default constructor. Initialises the the rotation to zero (the identity), the scale factor to one and the translation to zero
    00055     SIM2() : my_scale(1) {}
    00056     template <class A> SIM2(const SO2<Precision>& R, const Vector<2,Precision,A>& T, const Precision s) : my_se2(R,T), my_scale(s) {}
    00057     template <int S, class P, class A> SIM2(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_se2.get_rotation();}
    00061     /// @overload
    00062     const SO2<Precision> & get_rotation() const {return my_se2.get_rotation();}
    00063     /// Returns the translation part of the transformation as a Vector
    00064     Vector<2, Precision> & get_translation() {return my_se2.get_translation();}
    00065     /// @overload
    00066     const Vector<2, Precision> & get_translation() const {return my_se2.get_translation();}
    00067
    00068     /// Returns the scale factor of the transformation
    00069     Precision & get_scale() {return my_scale;}
    00070     /// @overload
    00071     const Precision & get_scale() const {return my_scale;}
    00072
    00073     /// Exponentiate a Vector in the Lie Algebra to generate a new SIM2.
    00074     /// See the Detailed Description for details of this vector.
    00075     /// @param vect The Vector to exponentiate
    00076     template <int S, typename P, typename A>
    00077     static inline SIM2 exp(const Vector<S,P, A>& vect);
    00078
    00079     /// Take the logarithm of the matrix, generating the corresponding vector in the Lie Algebra.
    00080     /// See the Detailed Description for details of this vector.
    00081     static inline Vector<4, Precision> ln(const SIM2& se2);
    00082     /// @overload
    00083     Vector<4, Precision> ln() const { return SIM2::ln(*this); }
    00084
    00085     /// compute the inverse of the transformation
    00086     SIM2 inverse() const {
    00087         const SO2<Precision> & rinv = get_rotation().inverse();
    00088         const Precision inv_scale = 1/get_scale();
    00089         return SIM2(rinv, -(rinv*(inv_scale*get_translation())), inv_scale);
    00090     };
    00091
    00092     /// Right-multiply by another SIM2 (concatenate the two transformations)
    00093     /// @param rhs The multipier
    00094     template <typename P>
    00095     SIM2<typename Internal::MultiplyType<Precision,P>::type> operator *(const SIM2<P>& rhs) const {
    00096         return SIM2<typename Internal::MultiplyType<Precision,P>::type>(get_rotation()*rhs.get_rotation(), get_translation() + get_rotation() * (get_scale()*rhs.get_translation()), get_scale() * rhs.get_scale());
    00097     }
    00098
    00099     /// Self right-multiply by another SIM2 (concatenate the two transformations)
    00100     /// @param rhs The multipier
    00101     inline SIM2& operator *=(const SIM2& rhs) {
    00102         *this = *this * rhs;
    00103         return *this;
    00104     }
    00105
    00106     /// returns the generators for the Lie group. These are a set of matrices that
    00107     /// form a basis for the vector space of the Lie algebra.
    00108     /// - 0 is translation in x
    00109     /// - 1 is translation in y
    00110     /// - 2 is rotation in the plane
    00111     /// - 3 is uniform scale
    00112     static inline Matrix<3,3, Precision> generator(int i) {
    00113         Matrix<3,3,Precision> result(Zeros);
    00114         switch(i){
    00115         case 0:
    00116         case 1:
    00117             result(i,2) = 1;
    00118             break;
    00119         case 2:
    00120             result(0,1) = -1;
    00121             result(1,0) = 1;
    00122             break;
    00123         case 3:
    00124             result(0,0) = 1;
    00125             result(1,1) = 1;
    00126             break;
    00127         }
    00128         return result;
    00129     }
    00130
    00131     /// transfers a vector in the Lie algebra, from one coord frame to another
    00132     /// so that exp(adjoint(vect)) = (*this) * exp(vect) * (this->inverse())
    00133     template<typename Accessor>
    00134     Vector<4, Precision> adjoint(const Vector<4,Precision, Accessor> & vect) const {
    00135         Vector<4, Precision> result;
    00136         result[2] = vect[2];
    00137         result.template slice<0,2>() = get_rotation() * vect.template slice<0,2>();
    00138         result[0] += vect[2] * get_translation()[1];
    00139         result[1] -= vect[2] * get_translation()[0];
    00140         return result;
    00141     }
    00142
    00143     template <typename Accessor>
    00144     Matrix<4,4,Precision> adjoint(const Matrix<4,4,Precision,Accessor>& M) const {
    00145         Matrix<4,4,Precision> result;
    00146         for(int i=0; i<4; ++i)
    00147             result.T()[i] = adjoint(M.T()[i]);
    00148         for(int i=0; i<4; ++i)
    00149             result[i] = adjoint(result[i]);
    00150         return result;
    00151     }
    00152
    00153 private:
    00154     SE2<Precision> my_se2;
    00155     Precision my_scale;
    00156 };
    00157
    00158 /// Write an SIM2 to a stream
    00159 /// @relates SIM2
    00160 template <class Precision>
    00161 inline std::ostream& operator<<(std::ostream& os, const SIM2<Precision> & rhs){
    00162     std::streamsize fw = os.width();
    00163     for(int i=0; i<2; i++){
    00164         os.width(fw);
    00165         os << rhs.get_rotation().get_matrix()[i] * rhs.get_scale();
    00166         os.width(fw);
    00167         os << rhs.get_translation()[i] << '\n';
    00168     }
    00169     return os;
    00170 }
    00171
    00172 /// Read an SIM2 from a stream
    00173 /// @relates SIM2
    00174 template <class Precision>
    00175 inline std::istream& operator>>(std::istream& is, SIM2<Precision>& rhs){
    00176     for(int i=0; i<2; i++)
    00177         is >> rhs.get_rotation().my_matrix[i].ref() >> rhs.get_translation()[i];
    00178     rhs.get_scale() = (norm(rhs.get_rotation().my_matrix[0]) + norm(rhs.get_rotation().my_matrix[1]))/2;
    00179     rhs.get_rotation().coerce();
    00180     return is;
    00181 }
    00182
    00183
    00184 //////////////////
    00185 // operator *   //
    00186 // SIM2 * Vector //
    00187 //////////////////
    00188
    00189 namespace Internal {
    00190 template<int S, typename P, typename PV, typename A>
    00191 struct SIM2VMult;
    00192 }
    00193
    00194 template<int S, typename P, typename PV, typename A>
    00195 struct Operator<Internal::SIM2VMult<S,P,PV,A> > {
    00196     const SIM2<P> & lhs;
    00197     const Vector<S,PV,A> & rhs;
    00198
    00199     Operator(const SIM2<P> & l, const Vector<S,PV,A> & r ) : lhs(l), rhs(r) {}
    00200
    00201     template <int S0, typename P0, typename A0>
    00202     void eval(Vector<S0, P0, A0> & res ) const {
    00203         SizeMismatch<3,S>::test(3, rhs.size());
    00204         res.template slice<0,2>()=lhs.get_rotation()*(lhs.get_scale()*rhs.template slice<0,2>());
    00205         res.template slice<0,2>()+=lhs.get_translation() * rhs[2];
    00206         res[2] = rhs[2];
    00207     }
    00208     int size() const { return 3; }
    00209 };
    00210
    00211 /// Right-multiply with a Vector<3>
    00212 /// @relates SIM2
    00213 template<int S, typename P, typename PV, typename A>
    00214 inline Vector<3, typename Internal::MultiplyType<P,PV>::type> operator*(const SIM2<P> & lhs, const Vector<S,PV,A>& rhs){
    00215     return Vector<3, typename Internal::MultiplyType<P,PV>::type>(Operator<Internal::SIM2VMult<S,P,PV,A> >(lhs,rhs));
    00216 }
    00217
    00218 /// Right-multiply with a Vector<2> (special case, extended to be a homogeneous vector)
    00219 /// @relates SIM2
    00220 template <typename P, typename PV, typename A>
    00221 inline Vector<2, typename Internal::MultiplyType<P,PV>::type> operator*(const SIM2<P>& lhs, const Vector<2,PV,A>& rhs){
    00222     return lhs.get_translation() + lhs.get_rotation() * (lhs.get_scale() * rhs);
    00223 }
    00224
    00225 //////////////////
    00226 // operator *   //
    00227 // Vector * SIM2 //
    00228 //////////////////
    00229
    00230 namespace Internal {
    00231 template<int S, typename P, typename PV, typename A>
    00232 struct VSIM2Mult;
    00233 }
    00234
    00235 template<int S, typename P, typename PV, typename A>
    00236 struct Operator<Internal::VSIM2Mult<S,P,PV,A> > {
    00237     const Vector<S,PV,A> & lhs;
    00238     const SIM2<P> & rhs;
    00239
    00240     Operator(const Vector<S,PV,A> & l, const SIM2<P> & r ) : lhs(l), rhs(r) {}
    00241
    00242     template <int S0, typename P0, typename A0>
    00243     void eval(Vector<S0, P0, A0> & res ) const {
    00244         SizeMismatch<3,S>::test(3, lhs.size());
    00245         res.template slice<0,2>() = (lhs.template slice<0,2>()* rhs.get_scale())*rhs.get_rotation();
    00246         res[2] = lhs[2];
    00247         res[2] += lhs.template slice<0,2>() * rhs.get_translation();
    00248     }
    00249     int size() const { return 3; }
    00250 };
    00251
    00252 /// Left-multiply with a Vector<3>
    00253 /// @relates SIM2
    00254 template<int S, typename P, typename PV, typename A>
    00255 inline Vector<3, typename Internal::MultiplyType<PV,P>::type> operator*(const Vector<S,PV,A>& lhs, const SIM2<P> & rhs){
    00256     return Vector<3, typename Internal::MultiplyType<PV,P>::type>(Operator<Internal::VSIM2Mult<S, P,PV,A> >(lhs,rhs));
    00257 }
    00258
    00259 //////////////////
    00260 // operator *   //
    00261 // SIM2 * Matrix //
    00262 //////////////////
    00263
    00264 namespace Internal {
    00265 template <int R, int C, typename PM, typename A, typename P>
    00266 struct SIM2MMult;
    00267 }
    00268
    00269 template<int R, int Cols, typename PM, typename A, typename P>
    00270 struct Operator<Internal::SIM2MMult<R, Cols, PM, A, P> > {
    00271     const SIM2<P> & lhs;
    00272     const Matrix<R,Cols,PM,A> & rhs;
    00273
    00274     Operator(const SIM2<P> & l, const Matrix<R,Cols,PM,A> & r ) : lhs(l), rhs(r) {}
    00275
    00276     template <int R0, int C0, typename P0, typename A0>
    00277     void eval(Matrix<R0, C0, P0, A0> & res ) const {
    00278         SizeMismatch<3,R>::test(3, rhs.num_rows());
    00279         for(int i=0; i<rhs.num_cols(); ++i)
    00280             res.T()[i] = lhs * rhs.T()[i];
    00281     }
    00282     int num_cols() const { return rhs.num_cols(); }
    00283     int num_rows() const { return 3; }
    00284 };
    00285
    00286 /// Right-multiply with a Matrix<3>
    00287 /// @relates SIM2
    00288 template <int R, int Cols, typename PM, typename A, typename P>
    00289 inline Matrix<3,Cols, typename Internal::MultiplyType<P,PM>::type> operator*(const SIM2<P> & lhs, const Matrix<R,Cols,PM, A>& rhs){
    00290     return Matrix<3,Cols,typename Internal::MultiplyType<P,PM>::type>(Operator<Internal::SIM2MMult<R, Cols, PM, A, P> >(lhs,rhs));
    00291 }
    00292
    00293 //////////////////
    00294 // operator *   //
    00295 // Matrix * SIM2 //
    00296 //////////////////
    00297
    00298 namespace Internal {
    00299 template <int Rows, int C, typename PM, typename A, typename P>
    00300 struct MSIM2Mult;
    00301 }
    00302
    00303 template<int Rows, int C, typename PM, typename A, typename P>
    00304 struct Operator<Internal::MSIM2Mult<Rows, C, PM, A, P> > {
    00305     const Matrix<Rows,C,PM,A> & lhs;
    00306     const SIM2<P> & rhs;
    00307
    00308     Operator( const Matrix<Rows,C,PM,A> & l, const SIM2<P> & r ) : lhs(l), rhs(r) {}
    00309
    00310     template <int R0, int C0, typename P0, typename A0>
    00311     void eval(Matrix<R0, C0, P0, A0> & res ) const {
    00312         SizeMismatch<3, C>::test(3, lhs.num_cols());
    00313         for(int i=0; i<lhs.num_rows(); ++i)
    00314             res[i] = lhs[i] * rhs;
    00315     }
    00316     int num_cols() const { return 3; }
    00317     int num_rows() const { return lhs.num_rows(); }
    00318 };
    00319
    00320 /// Left-multiply with a Matrix<3>
    00321 /// @relates SIM2
    00322 template <int Rows, int C, typename PM, typename A, typename P>
    00323 inline Matrix<Rows,3, typename Internal::MultiplyType<PM,P>::type> operator*(const Matrix<Rows,C,PM, A>& lhs, const SIM2<P> & rhs ){
    00324     return Matrix<Rows,3,typename Internal::MultiplyType<PM,P>::type>(Operator<Internal::MSIM2Mult<Rows, C, PM, A, P> >(lhs,rhs));
    00325 }
    00326
    00327 template <typename Precision>
    00328 template <int S, typename PV, typename Accessor>
    00329 inline SIM2<Precision> SIM2<Precision>::exp(const Vector<S, PV, Accessor>& mu){
    00330     SizeMismatch<4,S>::test(4, mu.size());
    00331
    00332     static const Precision one_6th = 1.0/6.0;
    00333     static const Precision one_20th = 1.0/20.0;
    00334
    00335     using std::exp;
    00336
    00337     SIM2<Precision> result;
    00338
    00339     // rotation
    00340     const Precision theta = mu[2];
    00341     result.get_rotation() = SO2<Precision>::exp(theta);
    00342
    00343     // scale
    00344     result.get_scale() = exp(mu[3]);
    00345
    00346     // translation
    00347     const Vector<3, Precision> coeff = Internal::compute_rodrigues_coefficients_sim3(mu[3], theta);
    00348     const Vector<2, Precision> cross = makeVector( -theta * mu[1], theta * mu[0]);
    00349     result.get_translation() = coeff[2] * mu.template slice<0,2>() + TooN::operator*(coeff[1], cross);
    00350
    00351     return result;
    00352 }
    00353
    00354 template <typename Precision>
    00355 inline Vector<4, Precision> SIM2<Precision>::ln(const SIM2<Precision> & sim2) {
    00356     using std::log;
    00357
    00358     Vector<4, Precision> result;
    00359
    00360     // rotation
    00361     const Precision theta = sim2.get_rotation().ln();
    00362     result[2] = theta;
    00363
    00364     // scale
    00365     result[3] = log(sim2.get_scale());
    00366
    00367     // translation
    00368     const Vector<3, Precision> coeff = Internal::compute_rodrigues_coefficients_sim3(result[3], theta);
    00369     Matrix<2,2, Precision> cross = Zeros; cross(0,1) = -theta; cross(1,0) = theta;
    00370     const Matrix<2,2, Precision> W = coeff[2] * Identity + coeff[1] * cross;
    00371     result.template slice<0,2>() = gaussian_elimination(W, sim2.get_translation());
    00372
    00373     return result;
    00374 }
    00375
    00376 #if 0
    00377 /// Multiply a SO2 with and SE2
    00378 /// @relates SE2
    00379 /// @relates SO2
    00380 template <typename Precision>
    00381 inline SE2<Precision> operator*(const SO2<Precision> & lhs, const SE2<Precision>& rhs){
    00382     return SE2<Precision>( lhs*rhs.get_rotation(), lhs*rhs.get_translation());
    00383 }
    00384 #endif
    00385
    00386 }
    00387 #endif