OpenWalnut
1.4.0
|
00001 //--------------------------------------------------------------------------- 00002 // 00003 // Project: OpenWalnut ( http://www.openwalnut.org ) 00004 // 00005 // Copyright 2009 OpenWalnut Community, BSV@Uni-Leipzig and CNCF@MPI-CBS 00006 // For more information see http://www.openwalnut.org/copying 00007 // 00008 // This file is part of OpenWalnut. 00009 // 00010 // OpenWalnut is free software: you can redistribute it and/or modify 00011 // it under the terms of the GNU Lesser General Public License as published by 00012 // the Free Software Foundation, either version 3 of the License, or 00013 // (at your option) any later version. 00014 // 00015 // OpenWalnut is distributed in the hope that it will be useful, 00016 // but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 // GNU Lesser General Public License for more details. 00019 // 00020 // You should have received a copy of the GNU Lesser General Public License 00021 // along with OpenWalnut. If not, see <http://www.gnu.org/licenses/>. 00022 // 00023 //--------------------------------------------------------------------------- 00024 00025 #ifndef WLINEARALGEBRAFUNCTIONS_H 00026 #define WLINEARALGEBRAFUNCTIONS_H 00027 00028 #include <Eigen/Core> 00029 #include <Eigen/SVD> 00030 00031 #include "WMatrix.h" 00032 #include "linearAlgebra/WPosition.h" 00033 00034 template< typename > class WMatrix; 00035 00036 /** 00037 * helper routine to multiply a 3x3 matrix with a vector 00038 * 00039 * \param mat 3x3 matrix 00040 * \param vec vector 00041 */ 00042 WVector3d multMatrixWithVector3D( WMatrix<double> mat, WVector3d vec ); 00043 00044 /** 00045 * Applies a coordinate transformation in homogenous coordinates to a vector. 00046 * This differs from transformPosition3DWithMatrix4D in that it DOES NOT incorporate the translation 00047 * 00048 * \param mat 4x4 matrix 00049 * \param vec vector 00050 */ 00051 WVector3d transformVector3DWithMatrix4D( WMatrix<double> mat, WVector3d vec ); 00052 00053 /** 00054 * Applies a coordinate transformation in homogenous coordinates to a position. 00055 * This differs from transformVector3DWithMatrix4D in that it incorporates the translation. 00056 * 00057 * \param mat 4x4 matrix 00058 * \param vec vector 00059 */ 00060 WPosition transformPosition3DWithMatrix4D( WMatrix<double> mat, WPosition vec ); 00061 00062 /** 00063 * helper routine to invert a 3x3 matrix 00064 * 00065 * \param mat 3x3 matrix 00066 * 00067 * \return inverted 3x3 matrix 00068 */ 00069 WMatrix<double> invertMatrix3x3( WMatrix<double> mat ); 00070 00071 /** 00072 * helper routine to invert a 4x4 matrix 00073 * 00074 * \param mat 4x4 matrix 00075 * 00076 * \return inverted 4x4 matrix 00077 */ 00078 WMatrix<double> invertMatrix4x4( WMatrix<double> mat ); 00079 00080 /** 00081 * Checks if the given two vectors are linearly independent. 00082 * 00083 * \param u First vector 00084 * \param v Second vector 00085 * 00086 * \return True if they are linear independent. 00087 * 00088 * \note This check is performed with the cross product != (0,0,0) but in numerical stability with FLT_EPS. 00089 */ 00090 bool linearIndependent( const WVector3d& u, const WVector3d& v ); 00091 00092 /** 00093 * Computes the SVD for the Matrix \param A 00094 * 00095 * A=U*S*V^T 00096 * 00097 * \tparam T The data type. 00098 * \param A Input Matrix 00099 * \param U Output Matrix 00100 * \param S Output of the entries in the diagonal matrix 00101 * \param V Output Matrix 00102 * 00103 */ 00104 template< typename T > 00105 void computeSVD( const WMatrix< T >& A, WMatrix< T >& U, WMatrix< T >& V, WValue< T >& S ); 00106 00107 /** 00108 * Calculates for a matrix the pseudo inverse. 00109 * 00110 * \tparam T The data type. 00111 * \param input Matrix to invert 00112 * 00113 * \return Inverted Matrix 00114 * 00115 */ 00116 template< typename T > 00117 WMatrix< T > pseudoInverse( const WMatrix< T >& input ); 00118 00119 00120 template< typename T > 00121 void computeSVD( const WMatrix< T >& A, 00122 WMatrix< T >& U, 00123 WMatrix< T >& V, 00124 WValue< T >& S ) 00125 { 00126 Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > eigenA( A ); 00127 Eigen::JacobiSVD< Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > > svd( eigenA, Eigen::ComputeFullU | Eigen::ComputeFullV ); 00128 U = WMatrix< T >( svd.matrixU() ); 00129 V = WMatrix< T >( svd.matrixV() ); 00130 S = WValue< T >( svd.singularValues() ); 00131 } 00132 00133 template< typename T > 00134 WMatrix< T > pseudoInverse( const WMatrix< T >& input ) 00135 { 00136 // calc pseudo inverse 00137 WMatrix< T > U( input.getNbRows(), input.getNbCols() ); 00138 WMatrix< T > V( input.getNbCols(), input.getNbCols() ); 00139 WValue< T > Svec( input.size() ); 00140 computeSVD( input, U, V, Svec ); 00141 00142 // create diagonal matrix S 00143 WMatrix< T > S( input.getNbCols(), input.getNbCols() ); 00144 S.setZero(); 00145 for( size_t i = 0; i < Svec.size() && i < S.getNbRows() && i < S.getNbCols(); i++ ) 00146 { 00147 S( i, i ) = ( Svec[ i ] == 0.0 ) ? 0.0 : 1.0 / Svec[ i ]; 00148 } 00149 00150 return WMatrix< T >( V*S*U.transposed() ); 00151 } 00152 00153 #endif // WLINEARALGEBRAFUNCTIONS_H