LieAlgebra_so3.h
Go to the documentation of this file.
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2009-2013 CEA LIST (DIASI/LSI) <xde-support@saxifrage.cea.fr>
00005 //
00006 // This Source Code Form is subject to the terms of the Mozilla
00007 // Public License v. 2.0. If a copy of the MPL was not distributed
00008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
00009 
00010 #ifndef EIGEN_LGSM_LIE_ALGEBRA_so3_H
00011 #define EIGEN_LGSM_LIE_ALGEBRA_so3_H
00012 
00013 /***************************************************************************
00014 * Function used in exp derivatives
00015 ***************************************************************************/
00016 
00017 template<typename Scalar> inline Scalar ei_lie_algebra_so3_derivative_f2(Scalar n, Scalar n2, Scalar cos_n, Scalar precision){
00018   if(n2 < precision)
00019     return  (Scalar(0.5) + (Scalar(-1.) + n2 / Scalar(30.)) * n2 / Scalar(24.) );
00020   else
00021     return (Scalar(1.0) - cos_n)/n2;
00022 }
00023 
00024 template<typename Scalar> EIGEN_STRONG_INLINE Scalar ei_lie_algebra_so3_derivative_df2(Scalar n, Scalar n2, Scalar cos_n, Scalar sin_n, Scalar precision){ // 1.e-2
00025   if ( n2 < precision )
00026     return ( (Scalar(-1.)+(Scalar(1.)/Scalar(15.)+(Scalar(-1.)/Scalar(560.)+n2/Scalar(37800.))*n2)*n2)/Scalar(12.) );
00027   else
00028     return ( Scalar(2.)*( Scalar(0.5)*(n2-Scalar(2.)+Scalar(2.)*cos_n )/(n2*n2) ) - (n - std::sin(n))/(n2*n) );
00029 }
00030 
00031 template<typename Scalar> inline Scalar ei_lie_algebra_so3_derivative_f3(Scalar n, Scalar n2, Scalar sin_n, Scalar precision){
00032   if(n2 < precision)
00033     return (Scalar(20.)+(Scalar(-1.)+n2/Scalar(42.))*n2)/Scalar(120.);
00034   else
00035     return (n - sin_n)/(n2*n);
00036 }
00037 
00038 template<typename Scalar> EIGEN_STRONG_INLINE Scalar ei_lie_algebra_so3_derivative_df3(Scalar n, Scalar n2, Scalar cos_n, Scalar sin_n, Scalar precision){ // 1.e-2
00039   if (n2 < precision)
00040     return (-Scalar(21.)+(Scalar(1.)+(-Scalar(1.)/Scalar(48.)+Scalar(1.)/Scalar(3960.)*n2)*n2)*n2)/Scalar(1260.) ;
00041   else
00042     return Scalar(3)*( (n*n2-Scalar(6)*n+Scalar(6)*sin_n)/(Scalar(6)*n*n2*n2) ) - ( Scalar(0.5)*(n2-Scalar(2)+Scalar(2)*cos_n )/(n2*n2) );
00043 }
00044 
00045 /***********************************************************************************
00046 * Definition/implementation of LieAlgebraBase<Matrix<Scalar, 3, 1>, Derived>
00047 ************************************************************************************/
00048 
00059 template<class Derived> 
00060 class LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 3, 1>, Derived> : 
00061   public MatrixBase<Derived> 
00062 {
00063 protected:
00065   typedef MatrixBase<Derived> Base;
00066 public:  
00067   // inherit MatrixBase interface
00068   EIGEN_DENSE_PUBLIC_INTERFACE(LieAlgebraBase)
00069   // inherit operator= [XXX] inheriting Base::opertor= through warning C4522 with visual studio : multiple assignment operators specified 
00070   //EIGEN_INHERIT_ASSIGNMENT_OPERATORS(LieAlgebraBase)
00071   // accessor needed for MatrixBase inheritance
00072   LIE_INHERIT_MATRIX_BASE(3, 1)
00073 
00075   typedef Matrix<Scalar, 3, 1> BaseType;
00077   typedef typename internal::traits<Derived>::Coefficients Coefficients;
00079   typedef LieAlgebra<BaseType> PlainObject;   
00081   typedef LieAlgebraDual<BaseType> AlgebraDual;
00083   typedef typename internal::traits<Derived>::Group Group;
00085   typedef Matrix<Scalar, 3, 3> Matrix3;
00086 
00088   EIGEN_STRONG_INLINE LieAlgebraBase& operator=(const LieAlgebraBase& other);
00090   template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator=(const LieAlgebraBase<BaseType, OtherDerived>& other);
00091 
00092   template<class OtherDerived> EIGEN_STRONG_INLINE Derived& operator=(const MatrixBase<OtherDerived>& other) {
00093     this->get() = other;
00094     return derived();
00095   }
00096 
00097 
00099   template<class OtherDerived> inline PlainObject bracket(const LieAlgebraBase<BaseType, OtherDerived>& a) const;
00100 //  template<class OtherDerived> inline PlainObject adjoint(const LieAlgebraBase<BaseType, OtherDerived>& a) const;
00101 
00104   inline Group exp(Scalar precision = 1.e-5) const;
00108   inline Matrix<Scalar, 3, 3> dexp(Scalar precision = 1.e-6, Scalar precision2 = 1.e-2) const;
00112   template<class OtherDerived> Matrix<Scalar, 3, 3> inline d2exp(const MatrixBase<OtherDerived>& v, Scalar precision = 1.e-6, Scalar precision2 = 1.e-2) const;
00113   
00115   inline Coefficients& get() {return this->derived().get(); }
00117   inline const Coefficients& get() const {return this->derived().get(); }
00118 };
00119 
00120 /***************************************************************
00121  * Implementation of LieAlgebraBase<Array<Scalar, 3, 1> > methods
00122  ***************************************************************/
00123 
00124 // assignation 
00125 
00126 template<class Derived>
00127 inline LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 3, 1>, Derived>& 
00128   LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 3, 1>, Derived>::operator=(const LieAlgebraBase& other) 
00129 {
00130   this->get() = other.get();
00131   return *this;
00132 }
00133 
00134 template<class Derived>
00135 template<class OtherDerived>
00136 inline Derived& 
00137   LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 3, 1>, Derived>::operator=(const LieAlgebraBase<BaseType, OtherDerived>& other) 
00138 {
00139   this->get() = other.get();
00140   return derived();
00141 }
00142 
00143 // bracket
00144 template<class Derived> 
00145 template<class OtherDerived>
00146 inline typename LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 3, 1>, Derived>::PlainObject 
00147   LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 3, 1>, Derived>::bracket(const LieAlgebraBase<Matrix<Scalar, 3, 1>, OtherDerived>& ang) const 
00148 {
00149   return this->cross(ang);
00150 }
00151 
00152 // adjoint
00153 /*template<class Derived, typename _Scalar> 
00154 template<class OtherDerived>
00155 typename LieAlgebraBase<Matrix<_Scalar, 3, 1>, Derived >::PlainObject LieAlgebraBase<Matrix<_Scalar, 3, 1>, Derived >::adjoint(const LieAlgebraBase<Matrix<Scalar, 3, 1>, OtherDerived>& a) const {
00156   return this->bracket(ang);
00157 }*/
00158 
00159 // exp
00160 template<class Derived> 
00161 inline typename LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 3, 1>, Derived >::Group 
00162  LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar,3, 1>, Derived >::exp(const Scalar precision) const 
00163 {
00164   const Scalar n2 = this->squaredNorm();
00165 
00166   if (n2 < precision)
00167   {
00168     const Scalar w = Scalar(1.0) + (Scalar(-1.0) + n2 / Scalar(48.0)) * n2 / Scalar(8.0); // Series expansion of cos(n/2) O(4)
00169     return Group(w, (Scalar(1.0) + (Scalar(-1.0) + Scalar(0.0125) * n2) * n2 / Scalar(24.0)) / Scalar(2.0) * *this); // Series expansion of sin(n/2)/n O(4)
00170   }
00171   else
00172   {
00173     const Scalar n = std::sqrt(n2);
00174     const Scalar w = std::cos(n * Scalar(0.5));
00175     return Group(w, std::sin(n * Scalar(0.5)) / n * *this);
00176   }
00177 }
00178 
00179 //dexp
00180 template<class Derived> 
00181 inline typename LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 3, 1>, Derived >::Matrix3  
00182  LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 3, 1>, Derived >::dexp(Scalar precision, 
00183                                                                                                  Scalar precision2) const 
00184 {
00185   Scalar n2 = this->squaredNorm();
00186   Scalar n = std::sqrt(n2);
00187   Scalar sin_n = std::sin(n);
00188   Scalar cos_n = std::cos(n);
00189 
00190   Scalar f2 = ei_lie_algebra_so3_derivative_f2(n, n2, cos_n, precision);
00191   Scalar f3 = ei_lie_algebra_so3_derivative_f3(n, n2, sin_n, precision2);
00192 
00193   Matrix<Scalar, 3, 3> m;
00194 
00195   m(0,0) = Scalar(1.0) - f3 * (n2 - this->coeff(0)*this->coeff(0));
00196   m(1,1) = Scalar(1.0) - f3 * (n2 - this->coeff(1)*this->coeff(1));
00197   m(2,2) = Scalar(1.0) - f3 * (n2 - this->coeff(2)*this->coeff(2));
00198 
00199   m(1, 0) = f3 * this->coeff(0) * this->coeff(1) - f2 * this->coeff(2);
00200   m(0, 1) = m(1,0) + 2 * f2 * this->coeff(2);
00201 
00202   m(2, 0) = f3 * this->coeff(0) * this->coeff(2) + f2 * this->coeff(1);
00203   m(0, 2) = m(2,0) - 2 * f2 * this->coeff(1);
00204 
00205   m(2, 1) = f3 * this->coeff(1) * this->coeff(2) - f2 * this->coeff(0);
00206   m(1, 2) = m(2,1) + 2 * f2 * this->coeff(0);
00207 
00208   return m;
00209 }
00210 
00211 //d2exp
00212 template<class Derived> 
00213 template<class OtherDerived> 
00214 inline typename LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar, 3, 1>, Derived >::Matrix3 
00215  LieAlgebraBase<Matrix<typename internal::traits<Derived>::Scalar,3, 1>, Derived >::d2exp(const MatrixBase<OtherDerived>& vec, 
00216                                                                                                  Scalar precision, 
00217                                                                                                  Scalar precision2) const
00218 {
00219   Scalar n2 = this->squaredNorm();
00220   Scalar n = std::sqrt(n2);
00221   Scalar cos_n = std::cos(n);
00222   Scalar sin_n = std::sin(n);
00223 
00224   Scalar f2 = ei_lie_algebra_so3_derivative_f2(n, n2, cos_n, precision);
00225   Scalar f3 = ei_lie_algebra_so3_derivative_f3(n, n2, sin_n, precision2);
00226   Scalar df2 = ei_lie_algebra_so3_derivative_df2(n, n2, cos_n, sin_n, precision2);
00227   Scalar df3 = ei_lie_algebra_so3_derivative_df3(n, n2, cos_n, sin_n, precision2);
00228 
00229   Scalar vw = this->dot(vec);
00230 
00231   // computing w*v^T may be faster
00232 
00233   Matrix<Scalar, 3, 3> m;
00234 
00235   m(0,0) = - 2 * f3 * (this->coeff(2) * vec[2] + this->coeff(1) * vec[1]) - df3 * vw * (n2 - this->coeff(0)*this->coeff(0));
00236   m(1,1) = - 2 * f3 * (this->coeff(2) * vec[2] + this->coeff(0) * vec[0]) - df3 * vw * (n2 - this->coeff(1)*this->coeff(1));
00237   m(2,2) = - 2 * f3 * (this->coeff(1) * vec[1] + this->coeff(0) * vec[0]) - df3 * vw * (n2 - this->coeff(2)*this->coeff(2));
00238 
00239   m(1, 0) = -vec[2] * f2 +  f3 * (this->coeff(0)*vec[1] + this->coeff(1)*vec[0]) + vw * (df3 * this->coeff(0) * this->coeff(1) - df2 * this->coeff(2));
00240   m(0, 1) = m(1,0) + 2 * vec[2] * f2 + 2 * vw * df2 * this->coeff(2);
00241 
00242   m(2, 0) = vec[1] * f2  +  f3 * (this->coeff(0)*vec[2] + this->coeff(2)*vec[0]) + vw * (df3 * this->coeff(0) * this->coeff(2) + df2 * this->coeff(1));
00243   m(0, 2) = m(2,0) - 2 * vec[1] * f2 - 2 * vw * df2 * this->coeff(1);
00244 
00245   m(2, 1) = -vec[0] * f2 +  f3 * (this->coeff(2)*vec[1] + this->coeff(1)*vec[2]) + vw * (df3 * this->coeff(1) * this->coeff(2) - df2 * this->coeff(0));
00246   m(1, 2) = m(2,1) + 2 * vec[0] * f2 + 2 * vw * df2 * this->coeff(0);
00247 
00248   return m;
00249 }
00250 
00251 
00252 /***********************************************************************************
00253 * Definition/implementation of LieAlgebraDualBase<Matrix<Scalar, 3, 1>, Derived>
00254 ************************************************************************************/
00255 
00256 namespace internal {
00257   template<typename Scalar>
00258     struct traits<LieAlgebraDual<Matrix<Scalar, 3, 1> > > : public traits<Matrix<Scalar, 3, 1> >
00259     {
00260       typedef Matrix<Scalar, 3, 1> Coefficients;
00261       typedef LieGroup<Quaternion<Scalar> > Group;
00262     };
00263 
00264   template<typename Scalar, int MapOptions>
00265     struct traits<Map<LieAlgebraDual<Matrix<Scalar, 3, 1> >, MapOptions> > : public traits<Map<Matrix<Scalar, 3, 1>, MapOptions> >
00266     {
00267       typedef Map<Matrix<Scalar, 3, 1>, MapOptions> Coefficients;
00268       typedef LieGroup<Quaternion<Scalar> > Group;
00269     };
00270 
00271   template<typename Scalar, int MapOptions>
00272     struct traits<Map<const LieAlgebraDual<Matrix<Scalar, 3, 1> >, MapOptions> > : public traits<Map<const Matrix<Scalar, 3, 1>, MapOptions> >
00273     {
00274       typedef Map<const Matrix<Scalar, 3, 1>, MapOptions> Coefficients;
00275       typedef LieGroup<Quaternion<Scalar> > Group;
00276     };
00277 
00278 }
00279 
00280 // no function to implement
00281 
00282 /***************************************************************************
00283  * Definition/implementation of LieAlgebra<Matrix<Scalar, 3, 1> >
00284  ***************************************************************************/
00285 
00296 namespace internal {
00297   template<typename Scalar>
00298     struct traits<LieAlgebra<Matrix<Scalar, 3, 1> > > 
00299     : public traits<LieAlgebraBase<Matrix<Scalar, 3, 1>, LieAlgebra<Matrix<Scalar, 3, 1> > > >
00300     {
00301       typedef Matrix<Scalar, 3, 1> Coefficients;
00302       typedef LieGroup<Quaternion<Scalar> > Group;
00303     };
00304 
00305   template<typename Scalar, int Options>
00306     struct traits<Map<LieAlgebra<Matrix<Scalar, 3, 1> >, Options> > 
00307     : public traits<LieAlgebraBase<Matrix<Scalar, 3, 1>, LieAlgebra<Matrix<Scalar, 3, 1> > > >
00308     {
00309       typedef Map<Matrix<Scalar, 3, 1>, Options> Coefficients;
00310       typedef LieGroup<Quaternion<Scalar> > Group;
00311     };
00312 
00313   template<typename Scalar, int Options>
00314     struct traits<Map<const LieAlgebra<Matrix<Scalar, 3, 1> >, Options> > 
00315     : public traits<LieAlgebraBase<Matrix<Scalar, 3, 1>, LieAlgebra<Matrix<Scalar, 3, 1> > > >
00316     {
00317       typedef Map<const Matrix<Scalar, 3, 1>, Options> Coefficients;
00318       typedef LieGroup<Quaternion<Scalar> > Group;
00319     };
00320 
00321 }
00322 
00323 template<typename _Scalar> 
00324 class LieAlgebra<Matrix<_Scalar, 3, 1> > : 
00325   public LieAlgebraBase<Matrix<_Scalar, 3, 1>, LieAlgebra<Matrix<_Scalar, 3, 1> > > 
00326 {
00327 protected:
00329   typedef LieAlgebraBase<Matrix<_Scalar, 3, 1>, LieAlgebra<Matrix<_Scalar, 3, 1> > > Base;
00330 public:
00331   // inherit MatrixBase operator
00332   EIGEN_DENSE_PUBLIC_INTERFACE(LieAlgebra)
00333   // inherit assignement operator
00334   EIGEN_INHERIT_ASSIGNMENT_OPERATORS(LieAlgebra)
00335 
00337   typedef typename internal::traits<LieAlgebra<Matrix<Scalar, 3, 1> > >::Coefficients Coefficients;
00338 
00340   inline LieAlgebra() : m_coeffs() {}
00342   inline LieAlgebra(const LieAlgebra& a) : m_coeffs(a.get() ) {} // maybe supernumerary
00344   template<class Derived>
00345   inline LieAlgebra(const LieAlgebraBase<typename Base::BaseType, Derived>& a) : m_coeffs(a.get() ) {}
00347   inline LieAlgebra(const Coefficients& a) : m_coeffs(a) {}
00349   inline LieAlgebra(Scalar x, Scalar y, Scalar z) : m_coeffs(x, y, z) {}
00350 
00352   inline Coefficients& get() { return m_coeffs; }
00354   inline const Coefficients& get() const { return m_coeffs; }
00355 
00356 protected:
00358   Coefficients m_coeffs;
00359 };
00360 
00361 
00362 /*****************************************************************************************
00363 * Definition/implementation of Map<LieAlgebra<Matrix<Scalar, 3, 1>  > >
00364 ******************************************************************************************/
00365 
00366 // no new constructor needed
00367 
00368 /*****************************************************************************************
00369 * Definition/implementation of LieAlgebraDual<Matrix<Scalar, 3, 1>  >
00370 ******************************************************************************************/
00371 
00372 template<typename _Scalar> 
00373 class LieAlgebraDual<Matrix<_Scalar, 3, 1> > : 
00374   public LieAlgebraDualBase<Matrix<_Scalar, 3, 1>, LieAlgebraDual<Matrix<_Scalar, 3, 1> > > 
00375 {
00376 protected:
00378   typedef LieAlgebraDualBase<Matrix<_Scalar, 3, 1>, LieAlgebraDual<Matrix<_Scalar, 3, 1> > > Base;
00379 public:
00380   // inherit MatrixBase operator
00381   EIGEN_DENSE_PUBLIC_INTERFACE(LieAlgebraDual)
00382   // inherit assignement operator
00383   EIGEN_INHERIT_ASSIGNMENT_OPERATORS(LieAlgebraDual)
00384 
00386   typedef typename internal::traits<LieAlgebraDual<Matrix<Scalar, 3, 1> > >::Coefficients Coefficients;
00387 
00389   inline LieAlgebraDual() : m_coeffs() {}
00391   inline LieAlgebraDual(const LieAlgebraDual& a) : m_coeffs(a.get() ) {}
00393   inline LieAlgebraDual(const Coefficients& a) : m_coeffs(a) {}
00395   inline LieAlgebraDual(Scalar x, Scalar y, Scalar z) : m_coeffs(x, y, z) {}
00396 
00398   inline Coefficients& get() { return m_coeffs; }
00400   inline const Coefficients& get() const { return m_coeffs; }
00401 
00402 protected:
00404   Coefficients m_coeffs;
00405 };
00406 
00407 /*****************************************************************************************
00408 * Definition/implementation of Map<LieAlgebraDual<Matrix<Scalar, 3, 1>  > >
00409 ******************************************************************************************/
00410 
00411 // no new constructor needed
00412 
00413 #endif


lgsm
Author(s): Roberto Martín-Martín
autogenerated on Sat Jun 8 2019 18:26:30