source: pacpusframework/trunk/include/Pacpus/PacpusTools/math/ublas.hpp

Last change on this file was 89, checked in by morasjul, 11 years ago

PACPUS 2.0 Beta deployed in new branch

Major changes:
-Add communication interface between components
-Add examples for communications interface (TestComponents)
-Move to Qt5 support

  • Property svn:executable set to *
File size: 11.6 KB
Line 
1// %pacpus:license{
2// This file is part of the PACPUS framework distributed under the
3// CECILL-C License, Version 1.0.
4// %pacpus:license}
5/// @file
6/// @author Firstname Surname <firstname.surname@utc.fr>
7/// @date Month, Year
8/// @version $Id: ublas.hpp 76 2013-01-10 17:05:10Z kurdejma $
9/// @copyright Copyright (c) UTC/CNRS Heudiasyc 2006 - 2013. All rights reserved.
10/// @brief Brief description.
11///
12/// Detailed description.
13
14#ifndef __BOOST_UBLAS__
15#define __BOOST_UBLAS__
16
17#include <boost/numeric/ublas/vector.hpp>
18#include <boost/numeric/ublas/matrix.hpp>
19
20#include <boost/numeric/ublas/lu.hpp>
21#include <boost/numeric/ublas/vector_proxy.hpp>
22#include <boost/numeric/ublas/triangular.hpp>
23#include <boost/numeric/ublas/lu.hpp>
24#include <boost/numeric/ublas/io.hpp>
25
26#include <cmath>
27#include "math_exception.hpp"
28
29namespace math {
30namespace ublas {
31
32/// Definition of a matrix using double precision
33typedef boost::numeric::ublas::matrix<double> Matrix;
34/// Definition of a vector using double precision
35typedef boost::numeric::ublas::vector<double> Vector;
36/// Definition of empty vector using double precision
37typedef boost::numeric::ublas::zero_vector<double> ZeroVector;
38/// Definition of empty matrix using double precision
39typedef boost::numeric::ublas::zero_matrix<double> ZeroMatrix;
40
41/// Multiplication of two matrices.
42///
43/// @tparam T @todo Documentation
44/// @param m1 ublas matrix
45/// @param m2 ublas matrix
46/// @returns ublas matrix
47template<class T>
48inline boost::numeric::ublas::matrix<T> operator *(const boost::numeric::ublas::matrix<T> & m1, const boost::numeric::ublas::matrix<T> & m2)
49{
50 return prod(m1,m2);
51}
52
53/// product of a vector by a matrix
54///
55/// @tparam T @todo Documentation
56/// @param m ublas matrix
57/// @param v ublas vector
58/// @returns ublas vector
59template<class T>
60inline boost::numeric::ublas::vector<T> operator *(const boost::numeric::ublas::matrix<T> & m, const boost::numeric::ublas::vector<T> & v)
61{
62 return prod(m,v);
63}
64
65/// addition of two vectors
66///
67/// @tparam T @todo Documentation
68/// @param v1 ublas vector
69/// @param v2 ublas vector
70/// @returns ublas vector
71template<class T>
72inline boost::numeric::ublas::vector<T> operator +(const boost::numeric::ublas::vector<T> & v1, const boost::numeric::ublas::vector<T> & v2)
73{
74 boost::numeric::ublas::vector<T> tmp = v1;
75 return tmp+=v2;
76}
77
78/// subtraction of two vectors
79///
80/// @tparam T @todo Documentation
81/// @param v1 ublas vector
82/// @param v2 ublas vector
83/// @returns ublas vector
84template<class T>
85inline boost::numeric::ublas::vector<T> operator -(const boost::numeric::ublas::vector<T> & v1, const boost::numeric::ublas::vector<T> & v2)
86{
87 boost::numeric::ublas::vector<T> tmp = v1;
88 return tmp-=v2;
89}
90
91/// Transposes a matrix
92///
93/// @tparam T @todo Documentation
94/// @param m ublas matrix
95/// @returns ublas matrix
96template<class T>
97inline boost::numeric::ublas::matrix<T> Trans(boost::numeric::ublas::matrix<T> &m)
98{
99 return trans(m);
100}
101
102/// Converts a vector to a matrix.
103///
104/// @param v ublas vector
105/// @returns ublas matrix
106template <class T>
107inline boost::numeric::ublas::matrix<T> vector2matrix(const boost::numeric::ublas::vector<T> &v)
108{
109 boost::numeric::ublas::matrix<T> tmp(v.size(),1);
110 for(size_t i=0;i<v.size();i++) {
111 tmp(i,0)=v[i];
112 }
113 return tmp;
114}
115
116/// compute the norm of a vector
117///
118/// @param v ublas vector
119/// @returns norm value
120template <class T>
121inline double Norm(const boost::numeric::ublas::vector<T> & v){
122 double norm =0;
123 for(typename boost::numeric::ublas::vector<T>::const_iterator I=v.begin(); I != v.end(); ++I) {
124 norm+=(*I)*(*I);
125 }
126 return std::sqrt(norm);
127}
128
129/// term by term multiplication of two vectors
130///
131/// @param v1 : ublas vector
132/// @param v2 : ublas vector
133/// @returns ublas vector
134template <class T>
135inline boost::numeric::ublas::vector<T> Mult(const boost::numeric::ublas::vector<T> & v1, const boost::numeric::ublas::vector<T> & v2)
136{
137 if(v1.size()!=v2.size()) {
138 throw math_error("Dot(v1,v2) : vectors must have the same size");
139 }
140 boost::numeric::ublas::vector<T> v(v1.size());
141 for(size_t i=0;i<v1.size();i++) {
142 v[i]=v1[i]*v2[i];
143 }
144 return v;
145}
146
147/// dot product
148///
149/// @param v1 ublas vector
150/// @param v2 ublas vector
151/// @returns dot product value
152template <class T>
153inline double Dot(const boost::numeric::ublas::vector<T> & v1, const boost::numeric::ublas::vector<T> & v2)
154{
155 if(v1.size()!=v2.size()) {
156 throw math_error("Dot(v1,v2) : vectors must have the same size");
157 }
158 double dot=0;
159 for(size_t i=0;i<v1.size();i++) {
160 dot+=v1[i]*v2[i];
161 }
162 return dot;
163}
164
165/// matrix inversion using LU decomposition
166/// @param m ublas matrix
167/// @returns ublas matrix
168inline Matrix InvLU(const Matrix &m)
169 throw(math_error)
170{
171 using namespace boost::numeric::ublas;
172 typedef permutation_matrix<std::size_t> pmatrix;
173
174 if(m.size1() != m.size2()) throw math_error("Inv(m): matrix must be square");
175
176 // create a working copy of the input
177 Matrix A(m);
178 // create a permutation matrix for the LU-factorization
179 pmatrix pm(A.size1());
180 // perform LU-factorization
181 int res = lu_factorize(A,pm);
182 if( res != 0 ) throw math_error("Inv(m) : singular matrix");
183 // create identity matrix of "inverse"
184 Matrix inverse = identity_matrix<double>(A.size1());
185 // backsubstitute to get the inverse
186 lu_substitute(A, pm, inverse);
187
188 return inverse;
189}
190
191////////////////////////////////////////////////////////////////////////////////
192///////////////////////// QR DECOMPOSITION ////////////////////////////////////
193///////////////////////////////////////////////////////////////////////////////
194
195/// @todo Documentation
196/// @tparam T @todo Documentation
197template<class T>
198bool InvertMatrix (const boost::numeric::ublas::matrix<T>& input, boost::numeric::ublas::matrix<T>& inverse)
199{
200 using namespace boost::numeric::ublas;
201 typedef permutation_matrix<std::size_t> pmatrix;
202 // create a working copy of the input
203 matrix<T> A(input);
204 // create a permutation matrix for the LU-factorization
205 pmatrix pm(A.size1());
206
207 // perform LU-factorization
208 int res = lu_factorize(A,pm);
209 if( res != 0 ) return false;
210
211 // create identity matrix of "inverse"
212 inverse.assign(boost::numeric::ublas::identity_matrix<T>(A.size1()));
213
214 // backsubstitute to get the inverse
215 lu_substitute(A, pm, inverse);
216
217 return true;
218}
219
220/// @todo Documentation
221/// @tparam T @todo Documentation
222template<class T>
223void TransposeMultiply(const boost::numeric::ublas::vector<T>& vector,
224 boost::numeric::ublas::matrix<T>& result,
225 size_t size)
226{
227 result.resize (size,size);
228 result.clear ();
229 for(unsigned int row=0; row< vector.size(); ++row)
230 {
231 for(unsigned int col=0; col < vector.size(); ++col)
232 result(row,col) = vector(col) * vector(row);
233
234 }
235}
236
237/// @todo Documentation
238/// @tparam T @todo Documentation
239template<class T>
240void HouseholderCornerSubstraction (boost::numeric::ublas::matrix<T>& LeftLarge,
241 const boost::numeric::ublas::matrix<T>& RightSmall)
242{
243 using namespace boost::numeric::ublas;
244 using namespace std;
245 if(
246 !(
247 (LeftLarge.size1() >= RightSmall.size1())
248 && (LeftLarge.size2() >= RightSmall.size2())
249 )
250 )
251 {
252 cerr << "invalid matrix dimensions" << endl;
253 return;
254 }
255
256 size_t row_offset = LeftLarge.size2() - RightSmall.size2();
257 size_t col_offset = LeftLarge.size1() - RightSmall.size1();
258
259 for(unsigned int row = 0; row < RightSmall.size2(); ++row )
260 for(unsigned int col = 0; col < RightSmall.size1(); ++col )
261 LeftLarge(col_offset+col,row_offset+row) -= RightSmall(col,row);
262}
263
264/// @todo Documentation
265/// @tparam T @todo Documentation
266template<class T>
267void QR (const boost::numeric::ublas::matrix<T>& M,
268 boost::numeric::ublas::matrix<T>& Q,
269 boost::numeric::ublas::matrix<T>& R)
270{
271 using namespace boost::numeric::ublas;
272 using namespace std;
273
274 if(
275 !(
276 (M.size1() == M.size2())
277 )
278 )
279 {
280 cerr << "invalid matrix dimensions" << endl;
281 return;
282 }
283 size_t size = M.size1();
284
285 // init Matrices
286 matrix<T> H, HTemp;
287 HTemp = identity_matrix<T>(size);
288 Q = identity_matrix<T>(size);
289 R = M;
290
291 // find Householder reflection matrices
292 for(unsigned int col = 0; col < size-1; ++col)
293 {
294 // create X vector
295 boost::numeric::ublas::vector<T> RRowView = boost::numeric::ublas::column(R,col);
296 vector_range< boost::numeric::ublas::vector<T> > X2 (RRowView, range (col, size));
297 boost::numeric::ublas::vector<T> X = X2;
298
299 // X -> U~
300 if(X(0) >= 0)
301 X(0) += norm_2(X);
302 else
303 X(0) += -1*norm_2(X);
304
305 HTemp.resize(X.size(),X.size(),true);
306
307 TransposeMultiply(X, HTemp, X.size());
308
309 // HTemp = the 2UUt part of H
310 HTemp *= ( 2 / inner_prod(X,X) );
311
312 // H = I - 2UUt
313 H = identity_matrix<T>(size);
314 HouseholderCornerSubstraction(H,HTemp);
315
316 // add H to Q and R
317 Q = prod(Q,H);
318 R = prod(H,R);
319 }
320}
321//////////////////////////////////////////////////////////////////////////////////////////::
322
323/// Matrix inversion using QR decomposition
324/// @param m ublas matrix
325/// @returns ublas matrix
326inline Matrix InvQR(const Matrix &m)
327 throw(math_error)
328{
329 using namespace boost::numeric::ublas;
330
331 if(m.size1() != m.size2()) throw math_error("Inv(m): matrix must be square");
332 Matrix Q(m), R(m), Rinv(m);
333 QR (m,Q,R);
334 for( int i = 0 ; i < R.size1() ; i++ )
335 for( int j = 0 ; j < R.size2() ; j++ )
336 if( R(i,j) < 1e-10 )
337 R(i,j) = 0;
338 InvertMatrix(R,Rinv);
339 return Rinv*Trans(Q);
340}
341
342
343/// compute matrix determinant using LU decomposition
344/// @param m ublas matrix
345/// @returns ublas matrix
346inline double DetLU(const Matrix & m)
347 throw(math_error)
348{
349 using namespace boost::numeric::ublas;
350 typedef permutation_matrix<std::size_t> pmatrix;
351
352
353 if(m.size1() != m.size2()) throw math_error("Determinant(m): matrix must be square");
354
355 // create a working copy of the input
356 Matrix A(m);
357 // create a permutation matrix for the LU-factorization
358 pmatrix pm(m.size1());
359 // perform LU-factorization
360 int res = lu_factorize(A, pm);
361 if( res != 0 ) throw math_error("Determinant(m) : singular matrix");
362 //compute determinant
363 double det = 1.0;
364 for (std::size_t i=0; i < pm.size(); ++i) {
365 if (pm(i) != i)
366 det *= -1.0;
367 det *= A(i,i);
368 }
369 return det;
370}
371
372
373/// output stream function
374inline std::ostream& operator << (std::ostream& ostrm, const Matrix & m)
375{
376 for (size_t i=0; i < m.size1(); i++)
377 {
378 ostrm << '['<<'\t';
379 for (size_t j=0; j < m.size2(); j++)
380 {
381 double x = m(i,j);
382 ostrm << x << '\t';
383 }
384 ostrm << ']'<< std::endl;
385 }
386 return ostrm;
387}
388
389/// output stream function
390inline std::ostream& operator << (std::ostream& ostrm, const Vector & v)
391{
392 for (size_t i=0; i < v.size(); i++)
393 {
394 ostrm << '['<<'\t';
395 double x = v(i);
396 ostrm << x << '\t';
397 ostrm << ']'<< std::endl;
398 }
399 return ostrm;
400}
401
402} // namespace ublas
403} // namespace math
404
405#endif // __BOOST_UBLAS__
Note: See TracBrowser for help on using the repository browser.