rotation_matrix.hpp
Go to the documentation of this file.
00001 // HOG-Man - Hierarchical Optimization for Pose Graphs on Manifolds
00002 // Copyright (C) 2010 G. Grisetti, R. K├╝mmerle, C. Stachniss
00003 // 
00004 // HOG-Man is free software: you can redistribute it and/or modify
00005 // it under the terms of the GNU Lesser General Public License as published
00006 // by the Free Software Foundation, either version 3 of the License, or
00007 // (at your option) any later version.
00008 // 
00009 // HOG-Man is distributed in the hope that it will be useful,
00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 // GNU Lesser General Public License for more details.
00013 // 
00014 // You should have received a copy of the GNU Lesser General Public License
00015 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016 
00017 
00018 template <int N, typename Base>
00019 _RotationMatrix<N, Base>::_RotationMatrix(){
00020   _Matrix<N, N, Base> *m =this;
00021   *m=_Matrix<N, N, Base>::eye(1.0);
00022 }
00023 
00024 
00025 template <int N, typename Base>
00026 _RotationMatrix<N, Base>::_RotationMatrix(const _Matrix<N, N, Base> &m){
00027   _Matrix<N, N, Base>* t=this;
00028   *t=m;
00029 }
00030 
00031 template <int N, typename Base>
00032 _RotationMatrix<N, Base>& _RotationMatrix<N, Base>::operator*=(const _RotationMatrix<N, Base>& m){
00033   _Matrix<N,N, Base>::operator*=(m);
00034   return *this;
00035 }
00036 
00037 template <int N, typename Base>
00038 _RotationMatrix<N, Base> _RotationMatrix<N, Base>::operator*(const _RotationMatrix<N, Base>& m) const{
00039   _RotationMatrix t(*this);
00040   t*=m;
00041   return t;
00042 }
00043 
00044 template <int N, typename Base>
00045 _Vector<N, Base> _RotationMatrix<N, Base>::operator*(const _Vector<N, Base>& v) const{
00046   return _Matrix<N,N,Base>::operator*(v);
00047 }
00048 
00049 template <int N, typename Base>
00050 _RotationMatrix<N, Base> _RotationMatrix<N, Base>::inverse() const{
00051   _RotationMatrix t=(*this);
00052   t.transposeInPlace();
00053   return t;
00054 }
00055 
00056 
00057 template <typename Base>
00058 _RotationMatrix2<Base>::_RotationMatrix2(Base angle){
00059   Base c=cos(angle), s=sin(angle);
00060   this->_allocator[0][0] = c;  
00061   this->_allocator[0][1] = -s; 
00062   this->_allocator[1][0] = s;  
00063   this->_allocator[1][1] = c; 
00064 }
00065 
00066 template <typename Base>
00067 _RotationMatrix2<Base>::_RotationMatrix2(const _Vector<1,Base>& a) {
00068   Base c=cos(a[0]), s=sin(a[0]);
00069   this->_allocator[0][0] = c;  
00070   this->_allocator[0][1] = -s; 
00071   this->_allocator[1][0] = s;  
00072   this->_allocator[1][1] = c; 
00073 }
00074 
00075 template <typename Base>
00076 Base _RotationMatrix2<Base>::angle() const{
00077   return atan2(this->_allocator[1][0], this->_allocator[0][0]);
00078 }
00079 
00080 
00081 template <typename Base>
00082 _RotationMatrix3<Base>::_RotationMatrix3(Base roll, Base pitch, Base yaw){
00083     Base sphi   = sin(roll);
00084     Base stheta = sin(pitch);
00085     Base spsi   = sin(yaw);
00086     Base cphi   = cos(roll);
00087     Base ctheta = cos(pitch);
00088     Base cpsi   = cos(yaw);
00089         
00090     Base _r[3][3] = { //create rotational Matrix
00091       {cpsi*ctheta, cpsi*stheta*sphi - spsi*cphi, cpsi*stheta*cphi + spsi*sphi},
00092       {spsi*ctheta, spsi*stheta*sphi + cpsi*cphi, spsi*stheta*cphi - cpsi*sphi},
00093       {    -stheta,                  ctheta*sphi,                  ctheta*cphi}
00094     };
00095     for (int i=0; i<3; i++)
00096       for (int j=0; j<3; j++)
00097         this->_allocator[i][j]=_r[i][j];
00098 
00099 }
00100 
00101 template <typename Base>
00102 _RotationMatrix3<Base>::_RotationMatrix3(const _Vector<3, Base>& angles) {
00103   *this=_RotationMatrix3(angles.roll(), angles.pitch(), angles.yaw());
00104 }
00105 
00106 template <typename Base>
00107 _RotationMatrix3<Base>::_RotationMatrix3():_RotationMatrix<3, Base>() {
00108 }
00109 
00110 template <typename Base>
00111 _RotationMatrix2<Base>::_RotationMatrix2():_RotationMatrix<2, Base>() {
00112 }
00113 
00114 
00115 template <typename Base>
00116 _Vector<3, Base> _RotationMatrix3<Base>::angles() const{
00117   _Vector<3, Base> aux;
00118   aux.roll() = atan2(this->_allocator[2][1],this->_allocator[2][2]);
00119   aux.pitch() = atan2(-this->_allocator[2][0],sqrt(this->_allocator[2][1]*this->_allocator[2][1] + this->_allocator[2][2]* this->_allocator[2][2]));
00120   aux.yaw() = atan2(this->_allocator[1][0],this->_allocator[0][0]);
00121   return aux;
00122 }


hogman_minimal
Author(s): Maintained by Juergen Sturm
autogenerated on Mon Oct 6 2014 00:06:59