iCub-main
Math.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2011 RobotCub Consortium, European Commission FP6 Project IST-004370
3  * author: Arjan Gijsberts
4  * email: arjan.gijsberts@iit.it
5  * website: www.robotcub.org
6  * Permission is granted to copy, distribute, and/or modify this program
7  * under the terms of the GNU General Public License, version 2 or any
8  * later version published by the Free Software Foundation.
9  *
10  * A copy of the license can be found at
11  * http://www.robotcub.org/icub/license/gpl.txt
12  *
13  * This program is distributed in the hope that it will be useful, but
14  * WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16  * Public License for more details
17  */
18 
19 #include <cassert>
20 #include <stdexcept>
21 #include <cmath>
22 
23 #include <gsl/gsl_blas.h>
24 
25 #include <yarp/gsl/Gsl.h>
26 
28 
29 namespace iCub {
30 namespace learningmachine {
31 namespace math {
32 
33 void dchud(double* r, int ldr, int p, double* x, double* z, int ldz, int nz,
34  double* y, double* rho, double* c, double* s,
35  unsigned char rtrans, unsigned char ztrans) {
36  unsigned int i;
37  double* work = (double*) 0x0;
38  double* tbuff = (double*) 0x0;
39  unsigned int stp;
40  unsigned int stp2;
41  double scale, workscale, rhoscale;
42 
43  // create working copy of x
44  work = (double*) malloc(p * sizeof(double));
45  cblas_dcopy(p, x, 1, work, 1);
46 
47  stp = (rtrans == 1) ? p : 1;
48 
49  // update r and fill c and s on the go
50  for(i = 0, tbuff = r; (int)i < p; tbuff+=(p+1), i++) {
51  // compute givens rotation
52  cblas_drotg(tbuff, work+i, c+i, s+i);
53 
54  // force positive values on the diagonal (not strictly necessary)
55  //if(*tbuff < 0) {
56  // *tbuff = -(*tbuff);
57  // c[i] = -c[i];
58  // s[i] = -s[i];
59  //}
60 
61  // apply givens rotation
62  if((int)i < p - 1) {
63  cblas_drot(p-i-1, tbuff+stp, stp, work+i+1, 1, c[i], s[i]);
64  }
65  }
66  free(work);
67 
68  // update z and rho if applicable
69  if(nz > 0) {
70  work = (double*) malloc(nz * sizeof(double));
71  cblas_dcopy(nz, y, 1, work, 1);
72 
73  stp = (ztrans == 1) ? 1 : nz;
74  stp2 = (ztrans == 1) ? ldz : 1;
75 
76  // update z
77  for(i = 0; (int)i < p; i++) {
78  cblas_drot(nz, z+i*stp, stp2, work, 1, c[i], s[i]);
79  }
80 
81  // update rho
82  for(i = 0; (int)i < nz; i++) {
83  if(work[i] < 0) {
84  work[i] = -work[i];
85  }
86  if(rho[i] > 0) {
87  scale = work[i] + rho[i];
88  workscale = work[i] / scale;
89  rhoscale = rho[i] / scale;
90  rho[i] = scale * sqrt((workscale)*(workscale) + (rhoscale)*(rhoscale));
91  // above is equal to line below, but less sensitive to numerical instability
92  //rho[i] = sqrt(work[i]*work[i] + rho[i]*rho[i]);
93  }
94  }
95  free(work);
96  }
97 }
98 
99 void gsl_linalg_cholesky_update(gsl_matrix* R, gsl_vector* x, gsl_vector* c, gsl_vector* s,
100  gsl_matrix* Z, gsl_vector* y, gsl_vector* rho,
101  unsigned char rtrans, unsigned char ztrans) {
102  int i, j;
103  int ldr, p;
104  int ldz = 0;
105  int nz = 0;
106  double* Zp = NULL;
107  double* yp = NULL;
108  double* rhop = NULL;
109 
110  ldr = rtrans ? R->size2 : R->size1;
111  p = rtrans ? R->size1 : R->size2;
112 
113  // fill Z, y, and rho pointers and dimensions if not null
114  if(Z != NULL && y != NULL && rho != NULL) {
115  ldz = ztrans ? Z->size2 : Z->size1;
116  nz = ztrans ? Z->size1 : Z->size2;
117  Zp = Z->data;
118  yp = y->data;
119  rhop = rho->data;
120  }
121 
122  dchud(R->data, ldr, p, x->data, Zp, ldz, nz, yp, rhop, c->data, s->data, rtrans, ztrans);
123 
124  // reflect, as GSL functions expects duplicate information (i.e., lower and upper triangles)
125  for(i = 0; i < p; i++) {
126  for(j = 0; j < i; j++) {
127  if(rtrans) {
128  gsl_matrix_set(R, j, i, gsl_matrix_get(R, i, j));
129  } else {
130  gsl_matrix_set(R, i, j, gsl_matrix_get(R, j, i));
131  }
132  }
133  }
134 }
135 
136 void cholupdate(yarp::sig::Matrix& R, const yarp::sig::Vector& x, yarp::sig::Vector& c, yarp::sig::Vector& s,
137  yarp::sig::Matrix& Z, const yarp::sig::Vector& y, yarp::sig::Vector& rho, bool rtrans, bool ztrans) {
138  yarp::gsl::GslMatrix RGslMat(R), ZGslMat(Z);
139  yarp::gsl::GslVector xGslVec(x), yGslVec(y), rhoGslVec(rho), cGslVec(c), sGslVec(s);
140 
141  gsl_matrix* Rgsl = (gsl_matrix*) RGslMat.getGslMatrix();
142  gsl_vector* xgsl = (gsl_vector*) xGslVec.getGslVector();
143  gsl_matrix* Zgsl = (gsl_matrix*) ZGslMat.getGslMatrix();
144  gsl_vector* ygsl = (gsl_vector*) yGslVec.getGslVector();
145  gsl_vector* rhogsl = (gsl_vector*) rhoGslVec.getGslVector();
146  gsl_vector* cgsl = (gsl_vector*) cGslVec.getGslVector();
147  gsl_vector* sgsl = (gsl_vector*) sGslVec.getGslVector();
148 
149  gsl_linalg_cholesky_update(Rgsl, xgsl, cgsl, sgsl, Zgsl, ygsl, rhogsl, (unsigned char) rtrans, (unsigned char) ztrans);
150 }
151 
152 void cholupdate(yarp::sig::Matrix& R, const yarp::sig::Vector& x, bool rtrans) {
153  yarp::sig::Vector c(R.cols());
154  yarp::sig::Vector s(R.cols());
155 
156  yarp::gsl::GslMatrix RGslMat(R);
157  yarp::gsl::GslVector xGslVec(x), cGslVec(c), sGslVec(s);
158 
159  gsl_matrix* Rgsl = (gsl_matrix*) RGslMat.getGslMatrix();
160  gsl_vector* xgsl = (gsl_vector*) xGslVec.getGslVector();
161  gsl_vector* cgsl = (gsl_vector*) cGslVec.getGslVector();
162  gsl_vector* sgsl = (gsl_vector*) sGslVec.getGslVector();
163 
164  gsl_linalg_cholesky_update(Rgsl, xgsl, cgsl, sgsl, NULL, NULL, NULL, (unsigned char) rtrans, 0);
165 }
166 
167 void cholsolve(const yarp::sig::Matrix& R, const yarp::sig::Matrix& B, yarp::sig::Matrix& X) {
168  assert(B.rows() == X.rows());
169  assert(B.cols() == X.cols());
170  assert(R.rows() == R.cols());
171  assert(R.cols() == B.cols());
172 
173  yarp::sig::Vector x;
174  yarp::sig::Vector b;
175  for(int r = 0; r < B.rows(); r++) {
176  // we're forced to waste memory and computation here :(
177  b = B.getRow(r);
178  x = X.getRow(r);
179  cholsolve(R, b, x);
180  // copy back the solution
181  X.setRow(r, x);
182  }
183 }
184 
185 yarp::sig::Matrix cholsolve(const yarp::sig::Matrix& R, const yarp::sig::Matrix& B) {
186  yarp::sig::Matrix X(B.rows(), B.cols());
187  cholsolve(R, B, X);
188  return X;
189 }
190 
191 void cholsolve(const yarp::sig::Matrix& R, const yarp::sig::Vector& b, yarp::sig::Vector& x) {
192  int info;
193 
194  yarp::gsl::GslMatrix RGslMat(R);
195  yarp::gsl::GslVector bGslVec(b), xGslVec(x);
196 
197  gsl_matrix* Rgsl = (gsl_matrix*) RGslMat.getGslMatrix();
198  gsl_vector* bgsl = (gsl_vector*) bGslVec.getGslVector();
199  gsl_vector* xgsl = (gsl_vector*) xGslVec.getGslVector();
200 
201  info = gsl_linalg_cholesky_solve(Rgsl, bgsl, xgsl);
202  if(info) {
203  throw std::runtime_error(gsl_strerror(info));
204  }
205 }
206 
207 yarp::sig::Vector cholsolve(const yarp::sig::Matrix& R, const yarp::sig::Vector& b) {
208  yarp::sig::Vector x(b.size());
209  cholsolve(R, b, x);
210  return x;
211 }
212 
213 yarp::sig::Matrix outerprod(const yarp::sig::Vector& v1, const yarp::sig::Vector& v2) {
214  yarp::sig::Matrix out(v1.size(), v2.size());
215  for(int r = 0; r < out.rows(); r++) {
216  for(int c = 0; c < out.cols(); c++) {
217  out(r, c) = v1(r) * v2(c);
218  }
219  }
220  return out;
221 }
222 
223 yarp::sig::Vector& addvec(yarp::sig::Vector& v, double val) {
224  for(size_t i = 0; i < v.size(); i++) {
225  v(i) += val;
226  }
227  return v;
228 }
229 
230 void trsolve(const yarp::sig::Matrix& A, const yarp::sig::Vector& b, yarp::sig::Vector& x, bool transa) {
231  yarp::gsl::GslMatrix AGslMat(A);
232  yarp::gsl::GslVector bGslVec(b), xGslVec(x);
233 
234  gsl_matrix* Agsl = (gsl_matrix*) AGslMat.getGslMatrix();
235  gsl_vector* bgsl = (gsl_vector*) bGslVec.getGslVector();
236  gsl_vector* xgsl = (gsl_vector*) xGslVec.getGslVector();
237 
238  gsl_vector_memcpy(xgsl, bgsl);
239  CBLAS_TRANSPOSE trans = transa ? CblasTrans : CblasNoTrans;
240  gsl_blas_dtrsv(CblasUpper, trans, CblasNonUnit, Agsl, xgsl);
241 }
242 
243 yarp::sig::Vector trsolve(const yarp::sig::Matrix& A, const yarp::sig::Vector& b, bool transa) {
244  yarp::sig::Vector x(b.size());
245  trsolve(A, b, x, transa);
246  return x;
247 }
248 
249 void fillrandom(yarp::sig::Vector& v, yarp::math::RandScalar& prng) {
250  size_t i;
251  for(i = 0; i < v.size(); i++) {
252  v(i) = prng.get();
253  }
254 }
255 
256 void fillrandom(yarp::sig::Matrix& M, yarp::math::RandScalar& prng) {
257  int r, c;
258  for(r = 0; r < M.rows(); r++) {
259  for(c = 0; c < M.cols(); c++) {
260  M(r, c) = prng.get();
261  }
262  }
263 }
264 
265 void fillrandom(yarp::sig::Vector& v, yarp::math::RandnScalar& prng) {
266  size_t i;
267  for(i = 0; i < v.size(); i++) {
268  v(i) = prng.get();
269  }
270 }
271 
272 void fillrandom(yarp::sig::Matrix& M, yarp::math::RandnScalar& prng) {
273  int r, c;
274  for(r = 0; r < M.rows(); r++) {
275  for(c = 0; c < M.cols(); c++) {
276  M(r, c) = prng.get();
277  }
278  }
279 }
280 
281 yarp::sig::Vector random(int length, yarp::math::RandScalar& prng) {
282  yarp::sig::Vector v(length);
283  fillrandom(v, prng);
284  return v;
285 }
286 
287 yarp::sig::Matrix random(int rows, int columns, yarp::math::RandScalar& prng) {
288  yarp::sig::Matrix M(rows, columns);
289  fillrandom(M, prng);
290  return M;
291 }
292 
293 yarp::sig::Vector random(int length, yarp::math::RandnScalar& prng) {
294  yarp::sig::Vector v(length);
295  fillrandom(v, prng);
296  return v;
297 }
298 
299 yarp::sig::Matrix random(int rows, int columns, yarp::math::RandnScalar& prng) {
300  yarp::sig::Matrix M(rows, columns);
301  fillrandom(M, prng);
302  return M;
303 }
304 
305 yarp::sig::Vector& map(yarp::sig::Vector& v, double (op)(double)) {
306  for(size_t i = 0; i < v.size(); i++) {
307  v(i) = op(v(i));
308  }
309  return v;
310 }
311 
312 yarp::sig::Matrix& map(yarp::sig::Matrix& M, double (op)(double)) {
313  for(int r = 0; r < M.rows(); r++) {
314  for(int c = 0; c < M.cols(); c++) {
315  M(r, c) = op(M(r, c));
316  }
317  }
318  return M;
319 }
320 
321 yarp::sig::Vector map(const yarp::sig::Vector& v, double (op)(double)) {
322  yarp::sig::Vector out(v.size());
323  for(size_t i = 0; i < out.size(); i++) {
324  out(i) = op(v(i));
325  }
326  return out;
327 }
328 
329 yarp::sig::Matrix map(const yarp::sig::Matrix& M, double (op)(double)) {
330  yarp::sig::Matrix out(M.rows(), M.cols());
331  for(int r = 0; r < out.rows(); r++) {
332  for(int c = 0; c < out.cols(); c++) {
333  out(r, c) = op(M(r, c));
334  }
335  }
336  return out;
337 }
338 
339 yarp::sig::Vector& cosvec(yarp::sig::Vector& v){
340  return map(v, std::cos);
341 }
342 
343 yarp::sig::Vector& sinvec(yarp::sig::Vector& v){
344  return map(v, std::sin);
345 }
346 
347 yarp::sig::Matrix& cosmat(yarp::sig::Matrix& M) {
348  return map(M, std::cos);
349 }
350 
351 yarp::sig::Matrix& sinmat(yarp::sig::Matrix& M) {
352  return map(M, std::sin);
353 }
354 
355 yarp::sig::Vector cosvec(const yarp::sig::Vector& v){
356  return map(v, std::cos);
357 }
358 
359 yarp::sig::Vector sinvec(const yarp::sig::Vector& v){
360  return map(v, std::sin);
361 }
362 
363 yarp::sig::Matrix cosmat(const yarp::sig::Matrix& M) {
364  return map(M, std::cos);
365 }
366 
367 yarp::sig::Matrix sinmat(const yarp::sig::Matrix& M) {
368  return map(M, std::sin);
369 }
370 
371 } // math
372 } // learningmachine
373 } // iCub
374 
iCub
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.
Definition: emotionInterface.h:17
iCub::learningmachine::math::fillrandom
void fillrandom(yarp::sig::Vector &v, yarp::math::RandScalar &prng)
Fills an entire vector using the provided pseudo random number generator.
Definition: Math.cpp:249
iCub::learningmachine::math::trsolve
void trsolve(const yarp::sig::Matrix &A, const yarp::sig::Vector &b, yarp::sig::Vector &x, bool transa=false)
Solves a triangular linear system Ax=b where A is triangular.
Definition: Math.cpp:230
iCub::action::log::info
@ info
Definition: actionPrimitives.cpp:64
iCub::learningmachine::math::dchud
void dchud(double *r, int ldr, int p, double *x, double *z, int ldz, int nz, double *y, double *rho, double *c, double *s, unsigned char rtrans=0, unsigned char ztrans=0)
Definition: Math.cpp:33
z
z
Definition: show_eyes_axes.m:22
out
out
Definition: sine.m:8
iCub::learningmachine::math::cholupdate
void cholupdate(yarp::sig::Matrix &R, const yarp::sig::Vector &x, yarp::sig::Vector &c, yarp::sig::Vector &s, yarp::sig::Matrix &Z, const yarp::sig::Vector &y, yarp::sig::Vector &rho, bool rtrans=0, bool ztrans=0)
Perform a rank-1 update to a Cholesky factor, while updating additional vectors using the used Given'...
Definition: Math.cpp:136
p
p
Definition: show_eyes_axes.m:23
iCub::learningmachine::math::random
yarp::sig::Vector random(int length, yarp::math::RandScalar &prng)
Returns a random vector with given dimensionality.
Definition: Math.cpp:281
Math.h
iCub::learningmachine::math::addvec
yarp::sig::Vector & addvec(yarp::sig::Vector &v, double val)
Adds a scalar to a vector inplace.
Definition: Math.cpp:223
iCub::learningmachine::math::sinmat
yarp::sig::Matrix & sinmat(yarp::sig::Matrix &M)
Computes the sine of a matrix element-wise inplace.
Definition: Math.cpp:351
y
y
Definition: show_eyes_axes.m:21
x
x
Definition: compute_ekf_sym.m:21
A
A
Definition: sine.m:16
iCub::learningmachine::math::cholsolve
void cholsolve(const yarp::sig::Matrix &R, const yarp::sig::Matrix &B, yarp::sig::Matrix &X)
Solves a system A*x=b for multiple row vectors in B using a precomputed Cholesky factor R.
Definition: Math.cpp:167
iCub::learningmachine::math::outerprod
yarp::sig::Matrix outerprod(const yarp::sig::Vector &v1, const yarp::sig::Vector &v2)
Computes the outer product of two vectors.
Definition: Math.cpp:213
iCub::learningmachine::math::gsl_linalg_cholesky_update
void gsl_linalg_cholesky_update(gsl_matrix *R, gsl_vector *x, gsl_vector *c, gsl_vector *s, gsl_matrix *Z=NULL, gsl_vector *y=NULL, gsl_vector *rho=NULL, unsigned char rtrans=0, unsigned char ztrans=0)
Definition: Math.cpp:99
v
static int v
Definition: iCub_Sim.cpp:42
iCub::learningmachine::math::cosvec
yarp::sig::Vector & cosvec(yarp::sig::Vector &v)
Computes the cosine of a vector element-wise inplace.
Definition: Math.cpp:339
iCub::learningmachine::math::cosmat
yarp::sig::Matrix & cosmat(yarp::sig::Matrix &M)
Computes the cosine of a matrix element-wise inplace.
Definition: Math.cpp:347
iCub::learningmachine::math::map
yarp::sig::Vector & map(yarp::sig::Vector &v, double(op)(double))
Performs a unary operator inplace on each element of a vector.
Definition: Math.cpp:305
iCub::learningmachine::math::sinvec
yarp::sig::Vector & sinvec(yarp::sig::Vector &v)
Computes the sine of a vector element-wise inplace.
Definition: Math.cpp:343