YARP
Yet Another Robot Platform
math.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2021 Istituto Italiano di Tecnologia (IIT)
3  * Copyright (C) 2006-2010 RobotCub Consortium
4  * All rights reserved.
5  *
6  * This software may be modified and distributed under the terms of the
7  * BSD-3-Clause license. See the accompanying LICENSE file for details.
8  */
9 
10 #include <yarp/os/LogComponent.h>
11 #include <yarp/math/Math.h>
12 #include <yarp/math/SVD.h>
13 #include <yarp/math/Quaternion.h>
14 
15 #include <yarp/eigen/Eigen.h>
16 
17 #include <Eigen/Eigenvalues>
18 
19 #include <cmath>
20 #include <cassert>
21 
22 using namespace yarp::eigen;
23 using namespace yarp::sig;
24 using namespace yarp::math;
25 
26 namespace {
27 YARP_LOG_COMPONENT(MATH, "yarp.math")
28 }
29 
30 Vector operator+(const Vector &a, const double &s)
31 {
32  Vector ret(a);
33  return ret+=s;
34 }
35 
36 Vector operator+(const double &s, const Vector &a)
37 {
38  return a+s;
39 }
40 
41 Vector& operator+=(Vector &a, const double &s)
42 {
43  size_t l = a.size();
44  for(size_t i=0; i<l; i++)
45  a[i] += s;
46  return a;
47 }
48 
49 Vector operator+(const Vector &a, const Vector &b)
50 {
51  Vector ret(a);
52  return ret+=b;
53 }
54 
56 {
57  size_t s=a.size();
58  yCAssert(MATH, s==b.size());
59  for (size_t k=0; k<s;k++)
60  a[k]+=b[k];
61  return a;
62 }
63 
64 Matrix operator+(const Matrix &a, const Matrix &b)
65 {
66  Matrix ret(a);
67  return ret+=b;
68 }
69 
71 {
72  size_t n=a.cols();
73  size_t m=a.rows();
74  yCAssert(MATH, m==b.rows() && n==b.cols());
75  for (size_t r=0; r<m;r++)
76  for (size_t c=0; c<n;c++)
77  a(r,c)+=b(r,c);
78  return a;
79 }
80 
81 Vector operator-(const Vector &a, const double &s)
82 {
83  Vector ret(a);
84  return ret-=s;
85 }
86 
87 Vector operator-(const double &s, const Vector &a)
88 {
89  size_t l = a.size();
90  Vector ret(l);
91  for(size_t i=0; i<l; i++)
92  ret[i] = s-a[i];
93  return ret;
94 }
95 
96 Vector& operator-=(Vector &a, const double &s)
97 {
98  size_t l = a.size();
99  for(size_t i=0; i<l; i++)
100  a[i] -= s;
101  return a;
102 }
103 
104 Vector operator-(const Vector &a, const Vector &b)
105 {
106  Vector ret(a);
107  return ret-=b;
108 }
109 
111 {
112  size_t s=a.size();
113  yCAssert(MATH, s==b.size());
114  for (size_t k=0; k<s;k++)
115  a[k]-=b[k];
116  return a;
117 }
118 
119 Matrix operator-(const Matrix &a, const Matrix &b)
120 {
121  Matrix ret(a);
122  return ret-=b;
123 }
124 
126 {
127  size_t n=a.cols();
128  size_t m=a.rows();
129  yCAssert(MATH, m==b.rows());
130  yCAssert(MATH, n==b.cols());
131  for (size_t r=0; r<m;r++)
132  for (size_t c=0; c<n;c++)
133  a(r,c)-=b(r,c);
134  return a;
135 }
136 
137 Vector operator*(double k, const Vector &b)
138 {
139  return operator*(b,k);
140 }
141 
142 Vector operator*(const Vector &a, double k)
143 {
144  Vector ret(a);
145  return ret*=k;
146 }
147 
148 Vector& operator*=(Vector &a, double k)
149 {
150  size_t size=a.size();
151  for (size_t i = 0; i < size; i++)
152  a[i]*=k;
153  return a;
154 }
155 
156 Vector operator*(const Vector &a, const Matrix &m)
157 {
158  yCAssert(MATH, a.size()==(size_t)m.rows());
159  Vector ret((size_t)m.cols());
160 
161  toEigen(ret) = toEigen(m).transpose()*toEigen(a);
162 
163  return ret;
164 }
165 
167 {
168  yCAssert(MATH, a.size()==(size_t)m.rows());
169  Vector a2(a);
170  a.resize(m.cols());
171 
172  toEigen(a) = toEigen(m).transpose()*toEigen(a2);
173 
174  return a;
175 }
176 
177 Vector operator*(const Matrix &m, const Vector &a)
178 {
179  yCAssert(MATH, (size_t)m.cols()==a.size());
180  Vector ret((size_t)m.rows(),0.0);
181 
182  toEigen(ret) = toEigen(m)*toEigen(a);
183 
184  return ret;
185 }
186 
187 Matrix operator*(const Matrix &a, const Matrix &b)
188 {
189  yCAssert(MATH, a.cols()==b.rows());
190  Matrix c(a.rows(), b.cols());
191 
192  toEigen(c) = toEigen(a)*toEigen(b);
193 
194  return c;
195 }
196 
198 {
199  yCAssert(MATH, a.cols()==b.rows());
200  Matrix a2(a); // a copy of a
201  a.resize(a.rows(), b.cols());
202 
203  toEigen(a) = toEigen(a2)*toEigen(b);
204 
205  return a;
206 }
207 
208 Matrix operator*(const double k, const Matrix &M)
209 {
210  Matrix res(M);
211  return res*=k;
212 }
213 
214 Matrix operator*(const Matrix &M, const double k)
215 {
216  return operator*(k,M);
217 }
218 
219 Matrix& operator*=(Matrix &M, const double k)
220 {
221  for (size_t r=0; r<M.rows(); r++)
222  for (size_t c=0; c<M.cols(); c++)
223  M(r,c)*=k;
224  return M;
225 }
226 
227 Vector operator*(const Vector &a, const Vector &b)
228 {
229  Vector res(a);
230  return res*=b;
231 }
232 
234 {
235  size_t n =a.length();
236  yCAssert(MATH, n==b.length());
237  for (size_t i=0; i<n; i++)
238  a[i]*=b[i];
239  return a;
240 }
241 
243 {
244  return Quaternion(a.w()*b.x() + a.x()*b.w() + a.y()*b.z() - a.z()*b.y(),
245  a.w()*b.y() + a.y()*b.w() + a.z()*b.x() - a.x()*b.z(),
246  a.w()*b.z() + a.z()*b.w() + a.x()*b.y() - a.y()*b.x(),
247  a.w()*b.w() - a.x()*b.x() - a.y()*b.y() - a.z()*b.z());
248 }
249 
250 Vector operator/(const Vector &a, const Vector &b)
251 {
252  Vector res(a);
253  return res/=b;
254 }
255 
257 {
258  size_t n =a.length();
259  yCAssert(MATH, n==b.length());
260  for (size_t i=0; i<n; i++)
261  a[i]/=b[i];
262  return a;
263 }
264 
265 Vector operator/(const Vector &b, double k)
266 {
267  Vector res(b);
268  return res/=k;
269 }
270 
271 Vector& operator/=(Vector &b, double k)
272 {
273  size_t n=b.length();
274  yCAssert(MATH, k!=0.0);
275  for (size_t i = 0; i < n; i++)
276  b[i]/=k;
277  return b;
278 }
279 
280 Matrix operator/(const Matrix &M, const double k)
281 {
282  Matrix res(M);
283  return res/=k;
284 }
285 
286 Matrix& operator/=(Matrix &M, const double k)
287 {
288  yCAssert(MATH, k!=0.0);
289  int rows=M.rows();
290  int cols=M.cols();
291  for (int r=0; r<rows; r++)
292  for (int c=0; c<cols; c++)
293  M(r,c)/=k;
294  return M;
295 }
296 
297 Matrix yarp::math::pile(const Matrix &m1, const Matrix &m2)
298 {
299  size_t c = m1.cols();
300  yCAssert(MATH, c==m2.cols());
301  size_t r1 = m1.rows();
302  size_t r2 = m2.rows();
303  Matrix res(r1+r2, c);
304 
305  toEigen(res).block(0,0,r1,c) = toEigen(m1);
306  toEigen(res).block(r1,0,r2,c) = toEigen(m2);
307 
308  return res;
309 }
310 
312 {
313  int c = m.cols();
314  yCAssert(MATH, (size_t)c==v.size());
315  int r = m.rows();
316  Matrix res(r+1, c);
317 
318  toEigen(res).block(0,0,r,c) = toEigen(m);
319  toEigen(res).block(r,0,1,c) = toEigen(v).transpose();
320 
321  return res;
322 }
323 
325 {
326  int c = m.cols();
327  yCAssert(MATH, (size_t)c==v.size());
328  int r = m.rows();
329  Matrix res(r+1, c);
330 
331  toEigen(res).block(0,0,1,c) = toEigen(v).transpose();
332  toEigen(res).block(1,0,r,c) = toEigen(m);
333 
334  return res;
335 }
336 
337 Matrix yarp::math::pile(const Vector &v1, const Vector &v2)
338 {
339  size_t n = v1.size();
340  yCAssert(MATH, n==v2.size());
341  Matrix res(2, (int)n);
342 
343  toEigen(res).block(0,0,1,n) = toEigen(v1);
344  toEigen(res).block(1,0,1,n) = toEigen(v2);
345 
346  return res;
347 }
348 
349 Matrix yarp::math::cat(const Matrix &m1, const Matrix &m2)
350 {
351  size_t r = m1.rows();
352  yCAssert(MATH, r==m2.rows());
353  size_t c1 = m1.cols();
354  size_t c2 = m2.cols();
355  Matrix res(r, c1+c2);
356 
357  toEigen(res).block(0,0,r,c1) = toEigen(m1);
358  toEigen(res).block(0,c1,r,c2) = toEigen(m2);
359 
360  return res;
361 }
362 
363 Matrix yarp::math::cat(const Matrix &m, const Vector &v)
364 {
365  int r = m.rows();
366  yCAssert(MATH, (size_t)r==v.size());
367  int c = m.cols();
368  Matrix res(r, c+1);
369 
370  toEigen(res).block(0,0,r,c) = toEigen(m);
371  toEigen(res).block(0,c,r,1) = toEigen(v);
372 
373  return res;
374 }
375 
376 Matrix yarp::math::cat(const Vector &v, const Matrix &m)
377 {
378  int r = m.rows();
379  yCAssert(MATH, (size_t)r==v.size());
380  int c = m.cols();
381  Matrix res(r, c+1);
382 
383  toEigen(res).block(0,0,r,1) = toEigen(v);
384  toEigen(res).block(0,1,r,c) = toEigen(m);
385 
386  return res;
387 }
388 
389 Vector yarp::math::cat(const Vector &v1, const Vector &v2)
390 {
391  size_t n1 = v1.size();
392  size_t n2 = v2.size();
393  Vector res(n1+n2);
394 
395  toEigen(res).segment(0,n1) = toEigen(v1);
396  toEigen(res).segment(n1,n2) = toEigen(v2);
397 
398  return res;
399 }
400 
401 Vector yarp::math::cat(const Vector &v, double s)
402 {
403  size_t n = v.size();
404  Vector res(n+1);
405 
406  toEigen(res).segment(0,n) = toEigen(v);
407  res(n) = s;
408 
409  return res;
410 }
411 
412 Vector yarp::math::cat(double s, const Vector &v)
413 {
414  size_t n = v.size();
415  Vector res(n+1);
416 
417  res(0) = s;
418  toEigen(res).segment(1,n) = toEigen(v);
419 
420  return res;
421 }
422 
423 Vector yarp::math::cat(double s1, double s2)
424 {
425  Vector res(2);
426  res(0) = s1;
427  res(1) = s2;
428  return res;
429 }
430 
431 Vector yarp::math::cat(double s1, double s2, double s3)
432 {
433  Vector res(3);
434  res(0) = s1;
435  res(1) = s2;
436  res(2) = s3;
437  return res;
438 }
439 
440 Vector yarp::math::cat(double s1, double s2, double s3, double s4)
441 {
442  Vector res(4);
443  res(0) = s1;
444  res(1) = s2;
445  res(2) = s3;
446  res(3) = s4;
447  return res;
448 }
449 
450 Vector yarp::math::cat(double s1, double s2, double s3, double s4, double s5)
451 {
452  Vector res(5);
453  res(0) = s1;
454  res(1) = s2;
455  res(2) = s3;
456  res(3) = s4;
457  res(4) = s5;
458  return res;
459 }
460 
461 double yarp::math::dot(const Vector &a, const Vector &b)
462 {
463  yCAssert(MATH, a.size()==b.size());
464 
465  return toEigen(a).dot(toEigen(b));
466 }
467 
468 
470 {
471  size_t s = a.size();
472  yCAssert(MATH, s==b.size());
473  Matrix res(s, s);
474  for(size_t i=0;i<s;i++)
475  for(size_t j=0;j<s;j++)
476  res(i,j) = a(i) * b(j);
477  return res;
478 }
479 
481 {
482  yCAssert(MATH, a.size()==3);
483  yCAssert(MATH, b.size()==3);
484  Vector v(3);
485  v[0]=a[1]*b[2]-a[2]*b[1];
486  v[1]=a[2]*b[0]-a[0]*b[2];
487  v[2]=a[0]*b[1]-a[1]*b[0];
488  return v;
489 }
490 
492 {
493  yCAssert(MATH, v.size()==3);
494  Matrix res = zeros(3,3);
495  res(1,0) = v(2);
496  res(0,1) = -v(2);
497  res(2,0) = -v(1);
498  res(0,2) = v(1);
499  res(2,1) = v(0);
500  res(1,2) = -v(0);
501  return res;
502 }
503 
505 {
506  if(v.size()!=3)
507  return false;
508  if(res.cols()!=3 || res.rows()!=3)
509  res.resize(3,3);
510  res(0,0) = res(1,1) = res(2,2) = 0.0;
511  res(1,0) = v(2);
512  res(0,1) = -v(2);
513  res(2,0) = -v(1);
514  res(0,2) = v(1);
515  res(2,1) = v(0);
516  res(1,2) = -v(0);
517  return true;
518 }
519 
520 double yarp::math::norm(const Vector &v)
521 {
522  return toEigen(v).norm();
523 }
524 
525 double yarp::math::norm2(const Vector &v)
526 {
527  return dot(v,v);
528 }
529 
530 double yarp::math::findMax(const Vector &v)
531 {
532  if (v.length()<=0)
533  return 0.0;
534  double ret=v[0];
535  for (size_t i=1; i<v.size(); i++)
536  if (v[i]>ret)
537  ret=v[i];
538  return ret;
539 }
540 
541 double yarp::math::findMin(const Vector &v)
542 {
543  if (v.length()<=0)
544  return 0.0;
545  double ret=v[0];
546  for (size_t i=1; i<v.length(); i++)
547  if (v[i]<ret)
548  ret=v[i];
549  return ret;
550 }
551 
553 {
554  return Vector(s, 0.0);
555 }
556 
558 {
559  return Vector(s, 1.0);
560 }
561 
562 Matrix yarp::math::eye(int r, int c)
563 {
564  Matrix ret;
565  ret.resize(r,c);
566  ret.eye();
567  return ret;
568 }
569 
571 {
572  return eye(n,n);
573 }
574 
576 {
577  Matrix ret;
578  ret.resize(r,c);
579  ret.zero();
580  return ret;
581 }
582 
583 double yarp::math::det(const Matrix& in)
584 {
585  return toEigen(in).determinant();
586 }
587 
589 {
590  int m = in.rows();
591  int n = in.cols();
592 
593  Matrix ret(m, n);
594 
595  toEigen(ret) = toEigen(in).inverse();
596 
597  return ret;
598 }
599 
600 bool yarp::math::eigenValues(const Matrix& in, Vector &real, Vector &img)
601 {
602  // return error for non-square matrix
603  if(in.cols() != in.rows())
604  return false;
605 
606  int n = in.cols();
607  real.resize(n);
608  img.resize(n);
609 
610  // Get eigenvalues
611  Eigen::EigenSolver< Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> > es(toEigen(in));
612 
613  for(int i=0; i < n; i++)
614  {
615  std::complex<double> lambda = es.eigenvalues()[i];
616  real(i) = lambda.real();
617  img(i) = lambda.imag();
618  }
619 
620  return true;
621 }
622 
623 /*
624 Matrix yarp::math::chinv(const Matrix& in)
625 {
626  Matrix ret(in);
627 
628  return ret;
629 }
630 */
631 
632 double yarp::math::sign(const double &v)
633 {
634  return ((v==0.0)?0.0:((v>0.0)?1.0:-1.0));
635 }
636 
637 
639 {
640  Vector ret(v.length());
641  for (size_t i=0; i<v.length(); i++)
642  ret[i]=sign(v[i]);
643 
644  return ret;
645 }
646 
648 {
649  yCAssert(MATH, (R.rows()>=3) && (R.cols()>=3));
650 
651  Vector v(4);
652  v[0]=R(2,1)-R(1,2);
653  v[1]=R(0,2)-R(2,0);
654  v[2]=R(1,0)-R(0,1);
655  v[3]=0.0;
656  double r=yarp::math::norm(v);
657  double theta=atan2(0.5*r,0.5*(R(0,0)+R(1,1)+R(2,2)-1));
658 
659  if (r<1e-9)
660  {
661  // if we enter here, then
662  // R is symmetric; this can
663  // happen only if the rotation
664  // angle is 0 (R=I) or 180 degrees
665  Matrix A=R.submatrix(0,2,0,2);
666  Matrix U(3,3), V(3,3);
667  Vector S(3);
668 
669  // A=I+sin(theta)*S+(1-cos(theta))*S^2
670  // where S is the skew matrix.
671  // Given a point x, A*x is the rotated one,
672  // hence if Ax=x then x belongs to the rotation
673  // axis. We have therefore to find the kernel of
674  // the linear application (A-I).
675  SVD(A-eye(3,3),U,S,V);
676 
677  v[0]=V(0,2);
678  v[1]=V(1,2);
679  v[2]=V(2,2);
680  r=yarp::math::norm(v);
681  }
682 
683  v=(1.0/r)*v;
684  v[3]=theta;
685 
686  return v;
687 }
688 
690 {
691  yCAssert(MATH, v.length()>=4);
692 
693  Matrix R=eye(4,4);
694 
695  double theta=v[3];
696  if (theta==0.0)
697  return R;
698 
699  double c=cos(theta);
700  double s=sin(theta);
701  double C=1.0-c;
702 
703  double xs =v[0]*s;
704  double ys =v[1]*s;
705  double zs =v[2]*s;
706  double xC =v[0]*C;
707  double yC =v[1]*C;
708  double zC =v[2]*C;
709  double xyC=v[0]*yC;
710  double yzC=v[1]*zC;
711  double zxC=v[2]*xC;
712 
713  R(0,0)=v[0]*xC+c;
714  R(0,1)=xyC-zs;
715  R(0,2)=zxC+ys;
716  R(1,0)=xyC+zs;
717  R(1,1)=v[1]*yC+c;
718  R(1,2)=yzC-xs;
719  R(2,0)=zxC-ys;
720  R(2,1)=yzC+xs;
721  R(2,2)=v[2]*zC+c;
722 
723  return R;
724 }
725 
727 {
728  yCAssert(MATH, (R.rows()>=3) && (R.cols()>=3));
729 
730  Vector v(3);
731  bool singularity=false;
732  if (R(2,2)<1.0)
733  {
734  if (R(2,2)>-1.0)
735  {
736  v[0]=atan2(R(1,2),R(0,2));
737  v[1]=acos(R(2,2));
738  v[2]=atan2(R(2,1),-R(2,0));
739  }
740  else
741  {
742  // Not a unique solution: gamma-alpha=atan2(R10,R11)
743  singularity=true;
744  v[0]=-atan2(R(1,0),R(1,1));
745  v[1]=M_PI;
746  v[2]=0.0;
747  }
748  }
749  else
750  {
751  // Not a unique solution: gamma+alpha=atan2(R10,R11)
752  singularity=true;
753  v[0]=atan2(R(1,0),R(1,1));
754  v[1]=0.0;
755  v[2]=0.0;
756  }
757 
758  if (singularity)
759  yCWarning(MATH, "dcm2euler() in singularity: choosing one solution among multiple");
760 
761  return v;
762 }
763 
765 {
766  yCAssert(MATH, v.length()>=3);
767 
768  Matrix Rza=eye(4,4); Matrix Ryb=eye(4,4); Matrix Rzg=eye(4,4);
769  double alpha=v[0]; double ca=cos(alpha); double sa=sin(alpha);
770  double beta=v[1]; double cb=cos(beta); double sb=sin(beta);
771  double gamma=v[2]; double cg=cos(gamma); double sg=sin(gamma);
772 
773  Rza(0,0)=ca; Rza(1,1)=ca; Rza(1,0)= sa; Rza(0,1)=-sa;
774  Rzg(0,0)=cg; Rzg(1,1)=cg; Rzg(1,0)= sg; Rzg(0,1)=-sg;
775  Ryb(0,0)=cb; Ryb(2,2)=cb; Ryb(2,0)=-sb; Ryb(0,2)= sb;
776 
777  return Rza*Ryb*Rzg;
778 }
779 
781 {
782  yCAssert(MATH, (R.rows()>=3) && (R.cols()>=3));
783 
784  Vector v(3);
785  bool singularity=false;
786  if (R(2,0)<1.0)
787  {
788  if (R(2,0)>-1.0)
789  {
790  v[0]=atan2(R(2,1),R(2,2));
791  v[1]=asin(-R(2,0));
792  v[2]=atan2(R(1,0),R(0,0));
793  }
794  else
795  {
796  // Not a unique solution: psi-phi=atan2(-R12,R11)
797  singularity = true;
798  v[0]=0.0;
799  v[1]=M_PI/2.0;
800  v[2]=-atan2(-R(1,2),R(1,1));
801  }
802  }
803  else
804  {
805  // Not a unique solution: psi+phi=atan2(-R12,R11)
806  singularity = true;
807  v[0]=0.0;
808  v[1]=-M_PI/2.0;
809  v[2]=atan2(-R(1,2),R(1,1));
810  }
811 
812  if (singularity)
813  yCWarning(MATH, "dcm2rpy() in singularity: choosing one solution among multiple");
814 
815  return v;
816 }
817 
819 {
820  yCAssert(MATH, v.length()>=3);
821 
822  Matrix Rz=eye(4,4); Matrix Ry=eye(4,4); Matrix Rx=eye(4,4);
823  double roll=v[0]; double cr=cos(roll); double sr=sin(roll);
824  double pitch=v[1]; double cp=cos(pitch); double sp=sin(pitch);
825  double yaw=v[2]; double cy=cos(yaw); double sy=sin(yaw);
826 
827  Rz(0,0)=cy; Rz(1,1)=cy; Rz(0,1)=-sy; Rz(1,0)= sy; // z-rotation with yaw
828  Ry(0,0)=cp; Ry(2,2)=cp; Ry(0,2)= sp; Ry(2,0)=-sp; // y-rotation with pitch
829  Rx(1,1)=cr; Rx(2,2)=cr; Rx(1,2)=-sr; Rx(2,1)= sr; // x-rotation with roll
830 
831  return Rz*Ry*Rx;
832 }
833 
835 {
836  yCAssert(MATH, (R.rows() >= 3) && (R.cols() >= 3));
837 
838  Vector v(3); // yaw pitch roll
839 
840  if (R(0, 2)<1.0)
841  {
842  if (R(0, 2)>-1.0)
843  {
844  v[0] = atan2(-R(0, 1), R(0, 0));
845  v[1] = asin(R(0, 2));
846  v[2] = atan2(-R(1, 2), R(2, 2));
847  }
848  else // == -1
849  {
850  // Not a unique solution: psi-phi=atan2(-R12,R11)
851  v[0] = 0.0;
852  v[1] = -M_PI / 2.0;
853  v[2] = -atan2(R(1, 0), R(1, 1));
854  }
855  }
856  else // == +1
857  {
858  // Not a unique solution: psi+phi=atan2(-R12,R11)
859  v[0] = 0.0;
860  v[1] = M_PI / 2.0;
861  v[2] = atan2(R(1, 0), R(1, 1));
862  }
863 
864  return v;
865 }
866 
868 {
869  yCAssert(MATH, v.length() >= 3);
870 
871  Matrix Rz = eye(4, 4); Matrix Ry = eye(4, 4); Matrix Rx = eye(4, 4);
872  double roll = v[2]; double cr = cos(roll); double sr = sin(roll);
873  double pitch = v[1]; double cp = cos(pitch); double sp = sin(pitch);
874  double yaw = v[0]; double cy = cos(yaw); double sy = sin(yaw);
875 
876  Rz(0, 0) = cy; Rz(1, 1) = cy; Rz(0, 1) = -sy; Rz(1, 0) = sy; // z-rotation with yaw
877  Ry(0, 0) = cp; Ry(2, 2) = cp; Ry(0, 2) = sp; Ry(2, 0) = -sp; // y-rotation with pitch
878  Rx(1, 1) = cr; Rx(2, 2) = cr; Rx(1, 2) = -sr; Rx(2, 1) = sr; // x-rotation with roll
879 
880  return Rx*Ry*Rz;
881 }
882 
884 {
885  yCAssert(MATH, (H.rows()==4) && (H.cols()==4));
886 
887  Vector p(4);
888  p[0]=H(0,3);
889  p[1]=H(1,3);
890  p[2]=H(2,3);
891  p[3]=1.0;
892 
893  Matrix invH=H.transposed();
894  p=invH*p;
895 
896  invH(0,3)=-p[0];
897  invH(1,3)=-p[1];
898  invH(2,3)=-p[2];
899  invH(3,0)=invH(3,1)=invH(3,2)=0.0;
900 
901  return invH;
902 }
903 
905 {
906  yCAssert(MATH, (H.rows()==4) && (H.cols()==4));
907 
908  // the skew matrix coming from the translational part of H: S(r)
909  Matrix S(3,3);
910  S(0,0)= 0.0; S(0,1)=-H(2,3); S(0,2)= H(1,3);
911  S(1,0)= H(2,3); S(1,1)= 0.0; S(1,2)=-H(0,3);
912  S(2,0)=-H(1,3); S(2,1)= H(0,3); S(2,2)= 0.0;
913 
914  S = S*H.submatrix(0,2,0,2);
915 
916  Matrix A(6,6); A.zero();
917  for (int i=0; i<3; i++)
918  {
919  for (int j=0; j<3; j++)
920  {
921  A(i,j) = H(i,j);
922  A(i+3,j+3) = H(i,j);
923  A(i,j+3) = S(i,j);
924  }
925  }
926 
927  return A;
928 }
929 
931 {
932  yCAssert(MATH, (H.rows()==4) && (H.cols()==4));
933 
934  // R^T
935  Matrix Rt = H.submatrix(0,2,0,2).transposed();
936  // R^T * r
937  Vector Rtp = Rt*H.getCol(3).subVector(0,2);
938 
939  Matrix S(3,3);
940  S(0,0)= 0.0; S(0,1)=-Rtp(2); S(0,2)= Rtp(1);
941  S(1,0)= Rtp(2); S(1,1)= 0.0; S(1,2)=-Rtp(0);
942  S(2,0)=-Rtp(1); S(2,1)= Rtp(0); S(2,2)= 0.0;
943 
944  S = S*Rt;
945 
946  Matrix A(6,6); A.zero();
947  for (int i=0; i<3; i++)
948  {
949  for (int j=0; j<3; j++)
950  {
951  A(i,j) = Rt(i,j);
952  A(i+3,j+3) = Rt(i,j);
953  A(i,j+3) = -S(i,j);
954  }
955  }
956 
957  return A;
958 }
bool ret
double x() const
Definition: Quaternion.cpp:75
double z() const
Definition: Quaternion.cpp:85
double w() const
Definition: Quaternion.cpp:70
double y() const
Definition: Quaternion.cpp:80
A class for a Matrix.
Definition: Matrix.h:46
void zero()
Zero the matrix.
Definition: Matrix.cpp:308
Matrix submatrix(size_t r1, size_t r2, size_t c1, size_t c2) const
Extract a submatrix from (r1,c1) to (r2,c2) (extremes included), as in Matlab B=A(r1:r2,...
Definition: Matrix.h:196
void resize(size_t r, size_t c)
Resize the matrix, if matrix is not empty preserve old content.
Definition: Matrix.cpp:251
Matrix transposed() const
Return the transposed of the matrix.
Definition: Matrix.cpp:369
size_t cols() const
Return number of columns.
Definition: Matrix.h:101
Vector getCol(size_t c) const
Get a columns of the matrix as a vector.
Definition: Matrix.cpp:392
size_t rows() const
Return number of rows.
Definition: Matrix.h:95
void resize(size_t size) override
Resize the vector.
Definition: Vector.h:225
size_t size() const
Definition: Vector.h:326
VectorOf< T > subVector(unsigned int first, unsigned int last) const
Creates and returns a new vector, being the portion of the original vector defined by the first and l...
Definition: Vector.h:404
size_t length() const
Get the length of the vector.
Definition: Vector.h:334
#define M_PI
#define yCAssert(component, x)
Definition: LogComponent.h:172
#define yCWarning(component,...)
Definition: LogComponent.h:146
#define YARP_LOG_COMPONENT(name,...)
Definition: LogComponent.h:80
Vector & operator+=(Vector &a, const double &s)
Addition operator between a scalar and a vector (defined in Math.h).
Definition: math.cpp:41
Vector operator+(const Vector &a, const double &s)
Mathematical operations.
Definition: math.cpp:30
Vector operator-(const Vector &a, const double &s)
Subtraction operator between a vector and a scalar (defined in Math.h).
Definition: math.cpp:81
Vector operator*(double k, const Vector &b)
Scalar-vector product operator (defined in Math.h).
Definition: math.cpp:137
Vector & operator*=(Vector &a, double k)
Vector-scalar product operator (defined in Math.h).
Definition: math.cpp:148
Vector & operator/=(Vector &a, const Vector &b)
Vector-vector element-wise division operator (defined in Math.h).
Definition: math.cpp:256
Vector operator/(const Vector &a, const Vector &b)
Vector-vector element-wise division operator (defined in Math.h).
Definition: math.cpp:250
Vector & operator-=(Vector &a, const double &s)
Subtraction operator between a vector and a scalar (defined in Math.h).
Definition: math.cpp:96
yarp::rosmsg::geometry_msgs::Quaternion Quaternion
Definition: Quaternion.h:24
Eigen::Map< Eigen::VectorXd > toEigen(yarp::sig::Vector &yarpVector)
Convert a yarp::sig::Vector to a Eigen::Map<Eigen::VectorXd> object.
Definition: Eigen.h:25
yarp::sig::Matrix eye(int r, int c)
Build an identity matrix (defined in Math.h).
Definition: math.cpp:562
void SVD(const yarp::sig::Matrix &in, yarp::sig::Matrix &U, yarp::sig::Vector &S, yarp::sig::Matrix &V)
Factorize the M-by-N matrix 'in' into the singular value decomposition in = U S V^T (defined in SVD....
Definition: SVD.cpp:25
yarp::sig::Matrix cat(const yarp::sig::Matrix &m1, const yarp::sig::Matrix &m2)
Matrix-Matrix concatenation by row (defined in Math.h).
Definition: math.cpp:349
double norm(const yarp::sig::Vector &v)
Returns the Euclidean norm of the vector (defined in Math.h).
Definition: math.cpp:520
yarp::sig::Vector cross(const yarp::sig::Vector &a, const yarp::sig::Vector &b)
Compute the cross product between two vectors (defined in Math.h).
Definition: math.cpp:480
yarp::sig::Vector ones(int s)
Creates a vector of ones (defined in Math.h).
Definition: math.cpp:557
yarp::sig::Vector dcm2ypr(const yarp::sig::Matrix &R)
Converts a dcm (direction cosine matrix) rotation matrix to yaw-roll-pitch angles (defined in Math....
Definition: math.cpp:834
bool eigenValues(const yarp::sig::Matrix &in, yarp::sig::Vector &real, yarp::sig::Vector &img)
Computes eigenvalues of the n-by-n real nonsymmetric matrix (defined in Math.h).
Definition: math.cpp:600
yarp::sig::Matrix crossProductMatrix(const yarp::sig::Vector &v)
Compute the cross product matrix, that is a 3-by-3 skew-symmetric matrix (defined in Math....
Definition: math.cpp:491
double norm2(const yarp::sig::Vector &v)
Returns the Euclidean squared norm of the vector (defined in Math.h).
Definition: math.cpp:525
double det(const yarp::sig::Matrix &in)
Computes the determinant of a matrix (defined in Math.h).
Definition: math.cpp:583
yarp::sig::Matrix SE3inv(const yarp::sig::Matrix &H)
Returns the inverse of a 4 by 4 rototranslational matrix (defined in Math.h).
Definition: math.cpp:883
yarp::sig::Matrix adjointInv(const yarp::sig::Matrix &H)
Returns the inverse of the adjoint matrix of a given roto-translational matrix (defined in Math....
Definition: math.cpp:930
yarp::sig::Vector zeros(int s)
Creates a vector of zeros (defined in Math.h).
Definition: math.cpp:552
double findMax(const yarp::sig::Vector &v)
Returns the maximum of the elements of a real vector (defined in Math.h).
Definition: math.cpp:530
yarp::sig::Matrix rpy2dcm(const yarp::sig::Vector &rpy)
Converts roll-pitch-yaw angles in the corresponding dcm (direction cosine matrix) rotation matrix (de...
Definition: math.cpp:818
double sign(const double &v)
Invert a symmetric and positive definite matrix using Cholesky decomposition (defined in Math....
Definition: math.cpp:632
double findMin(const yarp::sig::Vector &v)
Returns the minimum of the elements of a real vector (defined in Math.h).
Definition: math.cpp:541
yarp::sig::Matrix adjoint(const yarp::sig::Matrix &H)
Returns the adjoint matrix of a given roto-translational matrix (defined in Math.h).
Definition: math.cpp:904
yarp::sig::Matrix ypr2dcm(const yarp::sig::Vector &ypr)
Converts yaw-pitch-roll angles in the corresponding dcm (direction cosine matrix) rotation matrix (de...
Definition: math.cpp:867
double dot(const yarp::sig::Vector &a, const yarp::sig::Vector &b)
Scalar product between vectors (defined in Math.h).
Definition: math.cpp:461
yarp::sig::Vector dcm2euler(const yarp::sig::Matrix &R)
Converts a dcm (direction cosine matrix) rotation matrix to euler angles (ZYZ) (defined in Math....
Definition: math.cpp:726
yarp::sig::Matrix axis2dcm(const yarp::sig::Vector &v)
Returns a dcm (direction cosine matrix) rotation matrix R from axis/angle representation (defined in ...
Definition: math.cpp:689
yarp::sig::Matrix luinv(const yarp::sig::Matrix &in)
Invert a square matrix using LU-decomposition (defined in Math.h).
Definition: math.cpp:588
yarp::sig::Matrix euler2dcm(const yarp::sig::Vector &euler)
Converts euler angles (ZYZ) vector in the corresponding dcm (direction cosine matrix) rotation matrix...
Definition: math.cpp:764
yarp::sig::Vector dcm2rpy(const yarp::sig::Matrix &R)
Converts a dcm (direction cosine matrix) rotation matrix to roll-pitch-yaw angles (defined in Math....
Definition: math.cpp:780
yarp::sig::Matrix outerProduct(const yarp::sig::Vector &a, const yarp::sig::Vector &b)
Outer product between vectors (defined in Math.h).
Definition: math.cpp:469
yarp::sig::Matrix pile(const yarp::sig::Matrix &m1, const yarp::sig::Matrix &m2)
Matrix-Matrix concatenation by column (defined in Math.h).
Definition: math.cpp:297
yarp::sig::Vector dcm2axis(const yarp::sig::Matrix &R)
Converts a dcm (direction cosine matrix) rotation matrix R to axis/angle representation (defined in M...
Definition: math.cpp:647
Signal processing.
Definition: Image.h:25
VectorOf< double > Vector
Definition: Vector.h:33