TooN: sim3.h Source File

TooN 2.0.0-beta8

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

- File List

sim3.h

    00001 // -*- c++ -*-
    00002
    00003 // Copyright (C) 2011 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_INCLUDE_SIM3_H
    00031 #define TOON_INCLUDE_SIM3_H
    00032
    00033 #include <TooN/se3.h>
    00034
    00035 namespace TooN {
    00036
    00037
    00038 /// Represent a three-dimensional similarity transformation (a rotation, a scale factor and a translation).
    00039 /// This can be represented by a \f$3\times\f$4 matrix operating on a homogeneous co-ordinate,
    00040 /// so that a vector \f$\underline{x}\f$ is transformed to a new location \f$\underline{x}'\f$
    00041 /// by
    00042 /// \f[\begin{aligned}\underline{x}' &= E\times\underline{x}\\ \begin{bmatrix}x'\\y'\\z'\end{bmatrix} &= \begin{pmatrix}s r_{11} & s r_{12} & s r_{13} & t_1\\s r_{21} & s r_{22} & s r_{23} & t_2\\s r_{31} & s r_{32} & s r_{33} & t_3\end{pmatrix}\begin{bmatrix}x\\y\\z\\1\end{bmatrix}\end{aligned}\f]
    00043 ///
    00044 /// This transformation is a member of the Lie group SIM3. These can be parameterised with
    00045 /// seven numbers (in the space of the Lie Algebra). In this class, the first three parameters are a
    00046 /// translation vector while the second three are a rotation vector, whose direction is the axis of rotation
    00047 /// and length the amount of rotation (in radians), as for SO3. The seventh parameter is the log of the scale of the transformation.
    00048 /// @ingroup gTransforms
    00049 template <typename Precision = DefaultPrecision>
    00050 class SIM3 {
    00051 public:
    00052     /// Default constructor. Initialises the the rotation to zero (the identity), the scale to one and the translation to zero
    00053     inline SIM3() : my_scale(1) {}
    00054
    00055     template <int S, typename P, typename A>
    00056     SIM3(const SO3<Precision> & R, const Vector<S, P, A>& T, const Precision & s) : my_se3(R,T), my_scale(s) {}
    00057     template <int S, typename P, typename A>
    00058     SIM3(const Vector<S, P, A> & v) { *this = exp(v); }
    00059
    00060     /// Returns the rotation part of the transformation as a SO3
    00061     inline SO3<Precision>& get_rotation(){return my_se3.get_rotation();}
    00062     /// @overload
    00063     inline const SO3<Precision>& get_rotation() const {return my_se3.get_rotation();}
    00064
    00065     /// Returns the translation part of the transformation as a Vector
    00066     inline Vector<3, Precision>& get_translation() {return my_se3.get_translation();}
    00067     /// @overload
    00068     inline const Vector<3, Precision>& get_translation() const {return my_se3.get_translation();}
    00069
    00070     /// Returns the scale factor
    00071     inline Precision & get_scale() { return my_scale; }
    00072     /// @overload
    00073     inline const Precision & get_scale() const { return my_scale; }
    00074
    00075     /// Exponentiate a Vector in the Lie Algebra to generate a new SIM3.
    00076     /// See the Detailed Description for details of this vector.
    00077     /// @param vect The Vector to exponentiate
    00078     template <int S, typename P, typename A>
    00079     static inline SIM3 exp(const Vector<S, P, A>& vect);
    00080
    00081     /// Take the logarithm of the matrix, generating the corresponding vector in the Lie Algebra.
    00082     /// See the Detailed Description for details of this vector.
    00083     static inline Vector<7, Precision> ln(const SIM3& se3);
    00084     /// @overload
    00085     inline Vector<7, Precision> ln() const { return SIM3::ln(*this); }
    00086
    00087     inline SIM3 inverse() const {
    00088         const SO3<Precision> rinv = get_rotation().inverse();
    00089         const Precision inv_scale = 1/my_scale;
    00090         return SIM3(rinv, -(inv_scale*(rinv*get_translation())), inv_scale);
    00091     }
    00092
    00093     /// Right-multiply by another SIM3 (concatenate the two transformations)
    00094     /// @param rhs The multipier
    00095     inline SIM3& operator *=(const SIM3& rhs) {
    00096         get_translation() += get_rotation() * (get_scale() * rhs.get_translation());
    00097         get_rotation() *= rhs.get_rotation();
    00098         get_scale() *= rhs.get_scale();
    00099         return *this;
    00100     }
    00101
    00102     /// Right-multiply by another SIM3 (concatenate the two transformations)
    00103     /// @param rhs The multipier
    00104     template<typename P>
    00105     inline SIM3<typename Internal::MultiplyType<Precision, P>::type> operator *(const SIM3<P>& rhs) const {
    00106         return SIM3<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());
    00107     }
    00108
    00109     inline SIM3& left_multiply_by(const SIM3& left) {
    00110         get_translation() = left.get_translation() + left.get_rotation() * (left.get_scale() * get_translation());
    00111         get_rotation() = left.get_rotation() * get_rotation();
    00112         get_scale() = left.get_scale() * get_scale();
    00113         return *this;
    00114     }
    00115
    00116     static inline Matrix<4,4,Precision> generator(int i){
    00117         Matrix<4,4,Precision> result(Zeros);
    00118         if(i < 3){
    00119             result(i,3)=1;
    00120             return result;
    00121         }
    00122         if(i < 6){
    00123             result[(i+1)%3][(i+2)%3] = -1;
    00124             result[(i+2)%3][(i+1)%3] = 1;
    00125             return result;
    00126         }
    00127         result(0,0) = 1;
    00128         result(1,1) = 1;
    00129         result(2,2) = 1;
    00130         return result;
    00131     }
    00132
    00133   /// Returns the i-th generator times pos
    00134     template<typename Base>
    00135     inline static Vector<4,Precision> generator_field(int i, const Vector<4, Precision, Base>& pos)
    00136     {
    00137         Vector<4, Precision> result(Zeros);
    00138         if(i < 3){
    00139           result[i]=pos[3];
    00140           return result;
    00141         }
    00142         if(i < 6){
    00143             result[(i+1)%3] = - pos[(i+2)%3];
    00144             result[(i+2)%3] = pos[(i+1)%3];
    00145             return result;
    00146         }
    00147         result.template slice<0,3>() = pos.template slice<0,3>();
    00148         return result;
    00149     }
    00150
    00151     /// Transfer a matrix in the Lie Algebra from one
    00152     /// co-ordinate frame to another. This is the operation such that for a matrix
    00153     /// \f$ B \f$,
    00154     /// \f$ e^{\text{Adj}(v)} = Be^{v}B^{-1} \f$
    00155     /// @param M The Matrix to transfer
    00156     template<int S, typename P2, typename Accessor>
    00157     inline Vector<7, Precision> adjoint(const Vector<S,P2, Accessor>& vect)const;
    00158
    00159     /// Transfer covectors between frames (using the transpose of the inverse of the adjoint)
    00160     /// so that trinvadjoint(vect1) * adjoint(vect2) = vect1 * vect2
    00161     template<int S, typename P2, typename Accessor>
    00162     inline Vector<7, Precision> trinvadjoint(const Vector<S,P2,Accessor>& vect)const;
    00163
    00164     ///@overload
    00165     template <int R, int C, typename P2, typename Accessor>
    00166     inline Matrix<7,7,Precision> adjoint(const Matrix<R,C,P2,Accessor>& M)const;
    00167
    00168     ///@overload
    00169     template <int R, int C, typename P2, typename Accessor>
    00170     inline Matrix<7,7,Precision> trinvadjoint(const Matrix<R,C,P2,Accessor>& M)const;
    00171
    00172 private:
    00173     SE3<Precision> my_se3;
    00174     Precision my_scale;
    00175 };
    00176
    00177 // transfers a vector in the Lie algebra
    00178 // from one coord frame to another
    00179 // so that exp(adjoint(vect)) = (*this) * exp(vect) * (this->inverse())
    00180 template<typename Precision>
    00181 template<int S, typename P2, typename Accessor>
    00182 inline Vector<7, Precision> SIM3<Precision>::adjoint(const Vector<S,P2, Accessor>& vect) const{
    00183     SizeMismatch<7,S>::test(7, vect.size());
    00184     Vector<7, Precision> result;
    00185     result.template slice<3,3>() = get_rotation() * vect.template slice<3,3>();
    00186     result.template slice<0,3>() = get_rotation() * vect.template slice<0,3>();
    00187     result.template slice<0,3>() += get_translation() ^ result.template slice<3,3>();
    00188     return result;
    00189 }
    00190
    00191 // tansfers covectors between frames
    00192 // (using the transpose of the inverse of the adjoint)
    00193 // so that trinvadjoint(vect1) * adjoint(vect2) = vect1 * vect2
    00194 template<typename Precision>
    00195 template<int S, typename P2, typename Accessor>
    00196 inline Vector<7, Precision> SIM3<Precision>::trinvadjoint(const Vector<S,P2, Accessor>& vect) const{
    00197     SizeMismatch<7,S>::test(7, vect.size());
    00198     Vector<7, Precision> result;
    00199     result.template slice<3,3>() = get_rotation() * vect.template slice<3,3>();
    00200     result.template slice<0,3>() = get_rotation() * vect.template slice<0,3>();
    00201     result.template slice<3,3>() += get_translation() ^ result.template slice<0,3>();
    00202     return result;
    00203 }
    00204
    00205 template<typename Precision>
    00206 template<int R, int C, typename P2, typename Accessor>
    00207 inline Matrix<7,7,Precision> SIM3<Precision>::adjoint(const Matrix<R,C,P2,Accessor>& M)const{
    00208     SizeMismatch<7,R>::test(7, M.num_cols());
    00209     SizeMismatch<7,C>::test(7, M.num_rows());
    00210
    00211     Matrix<7,7,Precision> result;
    00212     for(int i=0; i<7; i++){
    00213         result.T()[i] = adjoint(M.T()[i]);
    00214     }
    00215     for(int i=0; i<7; i++){
    00216         result[i] = adjoint(result[i]);
    00217     }
    00218     return result;
    00219 }
    00220
    00221 template<typename Precision>
    00222 template<int R, int C, typename P2, typename Accessor>
    00223 inline Matrix<7,7,Precision> SIM3<Precision>::trinvadjoint(const Matrix<R,C,P2,Accessor>& M)const{
    00224     SizeMismatch<7,R>::test(7, M.num_cols());
    00225     SizeMismatch<7,C>::test(7, M.num_rows());
    00226
    00227     Matrix<7,7,Precision> result;
    00228     for(int i=0; i<7; i++){
    00229         result.T()[i] = trinvadjoint(M.T()[i]);
    00230     }
    00231     for(int i=0; i<7; i++){
    00232         result[i] = trinvadjoint(result[i]);
    00233     }
    00234     return result;
    00235 }
    00236
    00237 /// Write an SIM3 to a stream
    00238 /// @relates SIM3
    00239 template <typename Precision>
    00240 inline std::ostream& operator <<(std::ostream& os, const SIM3<Precision>& rhs){
    00241     std::streamsize fw = os.width();
    00242     for(int i=0; i<3; i++){
    00243         os.width(fw);
    00244         os << rhs.get_rotation().get_matrix()[i] * rhs.get_scale();
    00245         os.width(fw);
    00246         os << rhs.get_translation()[i] << '\n';
    00247     }
    00248     return os;
    00249 }
    00250
    00251
    00252 /// Reads an SIM3 from a stream
    00253 /// @relates SIM3
    00254 template <typename Precision>
    00255 inline std::istream& operator>>(std::istream& is, SIM3<Precision>& rhs){
    00256     for(int i=0; i<3; i++){
    00257         is >> rhs.get_rotation().my_matrix[i].ref() >> rhs.get_translation()[i];
    00258     }
    00259     rhs.get_scale() = (norm(rhs.get_rotation().my_matrix[0]) + norm(rhs.get_rotation().my_matrix[1]) + norm(rhs.get_rotation().my_matrix[2]))/3;
    00260     rhs.get_rotation().coerce();
    00261     return is;
    00262 }
    00263
    00264 //////////////////
    00265 // operator *   //
    00266 // SIM3 * Vector //
    00267 //////////////////
    00268
    00269 namespace Internal {
    00270 template<int S, typename PV, typename A, typename P>
    00271 struct SIM3VMult;
    00272 }
    00273
    00274 template<int S, typename PV, typename A, typename P>
    00275 struct Operator<Internal::SIM3VMult<S,PV,A,P> > {
    00276     const SIM3<P> & lhs;
    00277     const Vector<S,PV,A> & rhs;
    00278
    00279     Operator(const SIM3<P> & l, const Vector<S,PV,A> & r ) : lhs(l), rhs(r) {}
    00280
    00281     template <int S0, typename P0, typename A0>
    00282     void eval(Vector<S0, P0, A0> & res ) const {
    00283         SizeMismatch<4,S>::test(4, rhs.size());
    00284         res.template slice<0,3>()=lhs.get_rotation() * (lhs.get_scale() * rhs.template slice<0,3>());
    00285         res.template slice<0,3>()+=TooN::operator*(lhs.get_translation(),rhs[3]);
    00286         res[3] = rhs[3];
    00287     }
    00288     int size() const { return 4; }
    00289 };
    00290
    00291 /// Right-multiply by a Vector
    00292 /// @relates SIM3
    00293 template<int S, typename PV, typename A, typename P> inline
    00294 Vector<4, typename Internal::MultiplyType<P,PV>::type> operator*(const SIM3<P> & lhs, const Vector<S,PV,A>& rhs){
    00295     return Vector<4, typename Internal::MultiplyType<P,PV>::type>(Operator<Internal::SIM3VMult<S,PV,A,P> >(lhs,rhs));
    00296 }
    00297
    00298 /// Right-multiply by a Vector
    00299 /// @relates SIM3
    00300 template <typename PV, typename A, typename P> inline
    00301 Vector<3, typename Internal::MultiplyType<P,PV>::type> operator*(const SIM3<P>& lhs, const Vector<3,PV,A>& rhs){
    00302     return lhs.get_translation() + lhs.get_rotation() * (lhs.get_scale() * rhs);
    00303 }
    00304
    00305 //////////////////
    00306 // operator *   //
    00307 // Vector * SIM3 //
    00308 //////////////////
    00309
    00310 namespace Internal {
    00311 template<int S, typename PV, typename A, typename P>
    00312 struct VSIM3Mult;
    00313 }
    00314
    00315 template<int S, typename PV, typename A, typename P>
    00316 struct Operator<Internal::VSIM3Mult<S,PV,A,P> > {
    00317     const Vector<S,PV,A> & lhs;
    00318     const SIM3<P> & rhs;
    00319
    00320     Operator( const Vector<S,PV,A> & l, const SIM3<P> & r ) : lhs(l), rhs(r) {}
    00321
    00322     template <int S0, typename P0, typename A0>
    00323     void eval(Vector<S0, P0, A0> & res ) const {
    00324         SizeMismatch<4,S>::test(4, lhs.size());
    00325         res.template slice<0,3>()= rhs.get_scale() * lhs.template slice<0,3>() * rhs.get_rotation();
    00326         res[3] = lhs[3];
    00327         res[3] += lhs.template slice<0,3>() * rhs.get_translation();
    00328     }
    00329     int size() const { return 4; }
    00330 };
    00331
    00332 /// Left-multiply by a Vector
    00333 /// @relates SIM3
    00334 template<int S, typename PV, typename A, typename P> inline
    00335 Vector<4, typename Internal::MultiplyType<P,PV>::type> operator*( const Vector<S,PV,A>& lhs, const SIM3<P> & rhs){
    00336     return Vector<4, typename Internal::MultiplyType<P,PV>::type>(Operator<Internal::VSIM3Mult<S,PV,A,P> >(lhs,rhs));
    00337 }
    00338
    00339 //////////////////
    00340 // operator *   //
    00341 // SIM3 * Matrix //
    00342 //////////////////
    00343
    00344 namespace Internal {
    00345 template <int R, int C, typename PM, typename A, typename P>
    00346 struct SIM3MMult;
    00347 }
    00348
    00349 template<int R, int Cols, typename PM, typename A, typename P>
    00350 struct Operator<Internal::SIM3MMult<R, Cols, PM, A, P> > {
    00351     const SIM3<P> & lhs;
    00352     const Matrix<R,Cols,PM,A> & rhs;
    00353
    00354     Operator(const SIM3<P> & l, const Matrix<R,Cols,PM,A> & r ) : lhs(l), rhs(r) {}
    00355
    00356     template <int R0, int C0, typename P0, typename A0>
    00357     void eval(Matrix<R0, C0, P0, A0> & res ) const {
    00358         SizeMismatch<4,R>::test(4, rhs.num_rows());
    00359         for(int i=0; i<rhs.num_cols(); ++i)
    00360             res.T()[i] = lhs * rhs.T()[i];
    00361     }
    00362     int num_cols() const { return rhs.num_cols(); }
    00363     int num_rows() const { return 4; }
    00364 };
    00365
    00366 /// Right-multiply by a Matrix
    00367 /// @relates SIM3
    00368 template <int R, int Cols, typename PM, typename A, typename P> inline
    00369 Matrix<4,Cols, typename Internal::MultiplyType<P,PM>::type> operator*(const SIM3<P> & lhs, const Matrix<R,Cols,PM, A>& rhs){
    00370     return Matrix<4,Cols,typename Internal::MultiplyType<P,PM>::type>(Operator<Internal::SIM3MMult<R, Cols, PM, A, P> >(lhs,rhs));
    00371 }
    00372
    00373 //////////////////
    00374 // operator *   //
    00375 // Matrix * SIM3 //
    00376 //////////////////
    00377
    00378 namespace Internal {
    00379 template <int Rows, int C, typename PM, typename A, typename P>
    00380 struct MSIM3Mult;
    00381 }
    00382
    00383 template<int Rows, int C, typename PM, typename A, typename P>
    00384 struct Operator<Internal::MSIM3Mult<Rows, C, PM, A, P> > {
    00385     const Matrix<Rows,C,PM,A> & lhs;
    00386     const SIM3<P> & rhs;
    00387
    00388     Operator( const Matrix<Rows,C,PM,A> & l, const SIM3<P> & r ) : lhs(l), rhs(r) {}
    00389
    00390     template <int R0, int C0, typename P0, typename A0>
    00391     void eval(Matrix<R0, C0, P0, A0> & res ) const {
    00392         SizeMismatch<4, C>::test(4, lhs.num_cols());
    00393         for(int i=0; i<lhs.num_rows(); ++i)
    00394             res[i] = lhs[i] * rhs;
    00395     }
    00396     int num_cols() const { return 4; }
    00397     int num_rows() const { return lhs.num_rows(); }
    00398 };
    00399
    00400 /// Left-multiply by a Matrix
    00401 /// @relates SIM3
    00402 template <int Rows, int C, typename PM, typename A, typename P> inline
    00403 Matrix<Rows,4, typename Internal::MultiplyType<PM,P>::type> operator*(const Matrix<Rows,C,PM, A>& lhs, const SIM3<P> & rhs ){
    00404     return Matrix<Rows,4,typename Internal::MultiplyType<PM,P>::type>(Operator<Internal::MSIM3Mult<Rows, C, PM, A, P> >(lhs,rhs));
    00405 }
    00406
    00407 namespace Internal {
    00408
    00409 /// internal function that calculates the coefficients for the Rodrigues formula for SIM3 translation
    00410 template <typename Precision>
    00411 inline Vector<3, Precision> compute_rodrigues_coefficients_sim3( const Precision & s, const Precision & t ){
    00412     using std::exp;
    00413
    00414     Vector<3, Precision> coeff;
    00415     const Precision es = exp(s);
    00416
    00417     // 4 cases for s -> 0 and/or theta -> 0
    00418     // the Taylor expansions were calculated with Maple 12 and truncated at the 3rd power,
    00419     // such that eps^3 < 1e-18 which results in approximately 1 + eps^3 = 1
    00420     static const Precision eps = 1e-6;
    00421
    00422     if(fabs(s) < eps && fabs(t) < eps){
    00423         coeff[0] = 1 + s/2 + s*s/6;
    00424         coeff[1] = 1/2 + s/3 - t*t/24 + s*s/8;
    00425         coeff[2] = 1/6 + s/8 - t*t/120 + s*s/20;
    00426     } else if(fabs(s) < eps) {
    00427         coeff[0] = 1 + s/2 + s*s/6;
    00428         coeff[1] = (1-cos(t))/(t*t) + (sin(t)-cos(t)*t)*s/(t*t*t)+(2*sin(t)*t-t*t*cos(t)-2+2*cos(t))*s*s/(2*t*t*t*t);
    00429         coeff[2] = (t-sin(t))/(t*t*t) - (-t*t-2+2*cos(t)+2*sin(t)*t)*s/(2*t*t*t*t) - (-t*t*t+6*cos(t)*t+3*sin(t)*t*t-6*sin(t))*s*s/(6*t*t*t*t*t);
    00430     } else if(fabs(t) < eps) {
    00431         coeff[0] = (es - 1)/s;
    00432         coeff[1] = (s*es+1-es)/(s*s) - (6*s*es+6-6*es+es*s*s*s-3*es*s*s)*t*t/(6*s*s*s*s);
    00433         coeff[2] = (es*s*s-2*s*es+2*es-2)/(2*s*s*s) - (es*s*s*s*s-4*es*s*s*s+12*es*s*s-24*s*es+24*es-24)*t*t/(24*s*s*s*s*s);
    00434     } else {
    00435         const Precision a = es * sin(t);
    00436         const Precision b = es * cos(t);
    00437         const Precision inv_s_theta = 1/(s*s + t*t);
    00438
    00439         coeff[0] = (es - 1)/s;
    00440         coeff[1] = (a*s + (1-b)*t) * inv_s_theta / t;
    00441         coeff[2] = (coeff[0] - ((b-1)*s + a*t) * inv_s_theta) / (t*t);
    00442     }
    00443
    00444     return coeff;
    00445 }
    00446
    00447 }
    00448
    00449 template <typename Precision>
    00450 template <int S, typename P, typename VA>
    00451 inline SIM3<Precision> SIM3<Precision>::exp(const Vector<S, P, VA>& mu){
    00452     SizeMismatch<7,S>::test(7, mu.size());
    00453     using std::exp;
    00454
    00455     SIM3<Precision> result;
    00456
    00457     // scale
    00458     result.get_scale() = exp(mu[6]);
    00459
    00460     // rotation
    00461     const Vector<3,Precision> w = mu.template slice<3,3>();
    00462     const Precision t = norm(w);
    00463     result.get_rotation() = SO3<>::exp(w);
    00464
    00465     // translation
    00466     const Vector<3, Precision> coeff = Internal::compute_rodrigues_coefficients_sim3(mu[6],t);
    00467     const Vector<3,Precision> cross = w ^ mu.template slice<0,3>();
    00468     result.get_translation() = coeff[0] * mu.template slice<0,3>() + TooN::operator*(coeff[1], cross) + TooN::operator*(coeff[2], (w ^ cross));
    00469
    00470     return result;
    00471 }
    00472
    00473 template <typename Precision>
    00474 inline Vector<7, Precision> SIM3<Precision>::ln(const SIM3<Precision>& sim3) {
    00475     using std::sqrt;
    00476     using std::log;
    00477
    00478     Vector<7, Precision> result;
    00479
    00480     // rotation
    00481     result.template slice<3,3>() = sim3.get_rotation().ln();
    00482     const Precision theta = norm(result.template slice<3,3>());
    00483
    00484     // scale
    00485     const Precision es = sim3.get_scale();
    00486     const Precision s = log(sim3.get_scale());
    00487     result[6] = s;
    00488
    00489     // Translation
    00490     const Vector<3, Precision> coeff = Internal::compute_rodrigues_coefficients_sim3(s, theta);
    00491     const Matrix<3,3, Precision> cross = cross_product_matrix(result.template slice<3,3>());
    00492     const Matrix<3,3, Precision> W = Identity * coeff[0] + cross * coeff[1] + cross * cross * coeff[2];
    00493
    00494     result.template slice<0,3>() = gaussian_elimination(W, sim3.get_translation());
    00495
    00496     return result;
    00497 }
    00498
    00499 #if 0
    00500 template <typename Precision>
    00501 inline SE3<Precision> operator*(const SO3<Precision>& lhs, const SE3<Precision>& rhs){
    00502     return SE3<Precision>(lhs*rhs.get_rotation(),lhs*rhs.get_translation());
    00503 }
    00504 #endif
    00505
    00506 }
    00507
    00508 #endif