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