OpenWalnut  1.4.0
WLinearAlgebraFunctions.h
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