WLinearAlgebraFunctions.h 4.41 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27
//---------------------------------------------------------------------------
//
// Project: OpenWalnut ( http://www.openwalnut.org )
//
// Copyright 2009 OpenWalnut Community, BSV@Uni-Leipzig and CNCF@MPI-CBS
// For more information see http://www.openwalnut.org/copying
//
// This file is part of OpenWalnut.
//
// OpenWalnut is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// OpenWalnut is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with OpenWalnut. If not, see <http://www.gnu.org/licenses/>.
//
//---------------------------------------------------------------------------

#ifndef WLINEARALGEBRAFUNCTIONS_H
#define WLINEARALGEBRAFUNCTIONS_H

28
#include <Eigen/Core>
29
#include <Eigen/SVD>
30

31
#include "WMatrix.h"
32
#include "linearAlgebra/WPosition.h"
33

Mathias Goldau's avatar
Mathias Goldau committed
34
template< typename > class WMatrix;
35

Mathias Goldau's avatar
Mathias Goldau committed
36 37 38 39 40 41
/**
 * helper routine to multiply a 3x3 matrix with a vector
 *
 * \param mat 3x3 matrix
 * \param vec vector
 */
42
WVector3d multMatrixWithVector3D( WMatrix<double> mat, WVector3d vec );
43

Mathias Goldau's avatar
Mathias Goldau committed
44 45 46 47 48 49 50
/**
 * Applies a coordinate transformation in homogenous coordinates to a vector.
 * This differs from transformPosition3DWithMatrix4D in that it DOES NOT incorporate the translation
 *
 * \param mat 4x4 matrix
 * \param vec vector
 */
51
WVector3d transformVector3DWithMatrix4D( WMatrix<double> mat, WVector3d vec );
52

Mathias Goldau's avatar
Mathias Goldau committed
53 54 55 56 57 58 59
/**
 * Applies a coordinate transformation in homogenous coordinates to a position.
 * This differs from transformVector3DWithMatrix4D in that it incorporates the translation.
 *
 * \param mat 4x4 matrix
 * \param vec vector
 */
60
WPosition transformPosition3DWithMatrix4D( WMatrix<double> mat, WPosition vec );
61

Mathias Goldau's avatar
Mathias Goldau committed
62 63 64 65 66 67 68
/**
 * helper routine to invert a 3x3 matrix
 *
 * \param mat 3x3 matrix
 *
 * \return inverted 3x3 matrix
 */
69
WMatrix<double> invertMatrix3x3( WMatrix<double> mat );
70

Mathias Goldau's avatar
Mathias Goldau committed
71 72 73 74 75 76 77
/**
 * helper routine to invert a 4x4 matrix
 *
 * \param mat 4x4 matrix
 *
 * \return inverted 4x4 matrix
 */
78
WMatrix<double> invertMatrix4x4( WMatrix<double> mat );
79

Mathias Goldau's avatar
Mathias Goldau committed
80 81 82 83 84 85 86 87 88 89
/**
 * Checks if the given two vectors are linearly independent.
 *
 * \param u First vector
 * \param v Second vector
 *
 * \return True if they are linear independent.
 *
 * \note This check is performed with the cross product != (0,0,0) but in numerical stability with FLT_EPS.
 */
90
bool linearIndependent( const WVector3d& u, const WVector3d& v );
91

Mathias Goldau's avatar
Mathias Goldau committed
92 93 94 95 96
/**
 * Computes the SVD for the Matrix \param A
 *
 * A=U*S*V^T
 *
97
 * \tparam T The data type.
Mathias Goldau's avatar
Mathias Goldau committed
98 99 100 101 102 103
 * \param A Input Matrix
 * \param U Output Matrix
 * \param S Output of the entries in the diagonal matrix
 * \param V Output Matrix
 *
 */
104 105
template< typename T >
void computeSVD( const WMatrix< T >& A, WMatrix< T >& U, WMatrix< T >& V, WValue< T >& S );
106

Mathias Goldau's avatar
Mathias Goldau committed
107 108 109
/**
 * Calculates for a matrix the pseudo inverse.
 *
110
 * \tparam T The data type.
Mathias Goldau's avatar
Mathias Goldau committed
111 112 113 114 115
 * \param input Matrix to invert
 *
 * \return Inverted Matrix
 *
 */
116 117 118 119 120 121 122 123 124 125
template< typename T >
WMatrix< T > pseudoInverse( const WMatrix< T >& input );


template< typename T >
void computeSVD( const WMatrix< T >& A,
                 WMatrix< T >& U,
                 WMatrix< T >& V,
                 WValue< T >& S )
{
126 127
    Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > eigenA( A );
    Eigen::JacobiSVD< Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > > svd( eigenA, Eigen::ComputeFullU | Eigen::ComputeFullV );
128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151
    U = WMatrix< T >( svd.matrixU() );
    V = WMatrix< T >( svd.matrixV() );
    S = WValue< T >( svd.singularValues() );
}

template< typename T >
WMatrix< T > pseudoInverse( const WMatrix< T >& input )
{
    // calc pseudo inverse
    WMatrix< T > U( input.getNbRows(), input.getNbCols() );
    WMatrix< T > V( input.getNbCols(), input.getNbCols() );
    WValue< T > Svec( input.size() );
    computeSVD( input, U, V, Svec );

    // create diagonal matrix S
    WMatrix< T > S( input.getNbCols(), input.getNbCols() );
    S.setZero();
    for( size_t i = 0; i < Svec.size() && i < S.getNbRows() && i < S.getNbCols(); i++ )
    {
        S( i, i ) = ( Svec[ i ] == 0.0 ) ? 0.0 : 1.0 / Svec[ i ];
    }

    return WMatrix< T >( V*S*U.transposed() );
}
152 153

#endif  // WLINEARALGEBRAFUNCTIONS_H