YARP Yet Another Robot Platform
Searching...
No Matches
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
5 */
6
8#include <yarp/math/Math.h>
9#include <yarp/math/SVD.h>
11
12#include <yarp/eigen/Eigen.h>
13
14#include <Eigen/Eigenvalues>
15
16#include <cmath>
17#include <cassert>
18
19using namespace yarp::eigen;
20using namespace yarp::sig;
21using namespace yarp::math;
22
23namespace {
24YARP_LOG_COMPONENT(MATH, "yarp.math")
25}
26
27Vector operator+(const Vector &a, const double &s)
28{
29 Vector ret(a);
30 return ret+=s;
31}
32
33Vector operator+(const double &s, const Vector &a)
34{
35 return a+s;
36}
37
38Vector& 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
47Vector 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
63Matrix 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
82Vector operator-(const Vector &a, const double &s)
83{
84 Vector ret(a);
85 return ret-=s;
86}
87
88Vector 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
98Vector& 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
107Vector 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
123Matrix 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
143Vector operator*(double k, const Vector &b)
144{
145 return operator*(b,k);
146}
147
148Vector operator*(const Vector &a, double k)
149{
150 Vector ret(a);
151 return ret*=k;
152}
153
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
163Vector 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
184Vector 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
194Matrix 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
215Matrix operator*(const double k, const Matrix &M)
216{
217 Matrix res(M);
218 return res*=k;
219}
220
221Matrix operator*(const Matrix &M, const double k)
222{
223 return operator*(k,M);
224}
225
226Matrix& 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
236Vector 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
260Vector 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
276Vector operator/(const Vector &b, double k)
277{
278 Vector res(b);
279 return res/=k;
280}
281
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
292Matrix operator/(const Matrix &M, const double k)
293{
294 Matrix res(M);
295 return res/=k;
296}
297
298Matrix& 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
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
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
363Matrix 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
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
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
403Vector 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
415Vector 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
426Vector 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
437Vector 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
445Vector 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
454Vector 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
464Vector 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
475double yarp::math::dot(const Vector &a, const Vector &b)
476{
477 yCAssert(MATH, a.size()==b.size());
478
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
538double yarp::math::norm(const Vector &v)
539{
541}
542
543double yarp::math::norm2(const Vector &v)
544{
545 return dot(v,v);
546}
547
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
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
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
607double yarp::math::det(const Matrix& in)
608{
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
624bool 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/*
649Matrix yarp::math::chinv(const Matrix& in)
650{
651 Matrix ret(in);
652
653 return ret;
654}
655*/
656
657double 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}
#define M_PI
bool ret
A class for a Matrix.
Definition Matrix.h:39
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:189
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:94
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:88
void resize(size_t size) override
Resize the vector.
Definition Vector.h:221
size_t size() const
Definition Vector.h:322
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:401
size_t length() const
Get the length of the vector.
Definition Vector.h:330
#define yCAssert(component, x)
#define yCWarning(component,...)
#define YARP_LOG_COMPONENT(name,...)
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, const double &s)
Subtraction operator between a vector and a scalar (defined in Math.h).
Definition math.cpp:98
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/=(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, double k)
Vector-scalar product operator (defined in Math.h).
Definition math.cpp:154
Eigen::Map< Eigen::VectorXd > toEigen(yarp::sig::Vector &yarpVector)
Convert a yarp::sig::Vector to a Eigen::Map<Eigen::VectorXd> object.
Definition Eigen.h:21
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
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
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
VectorOf< double > Vector
Definition Vector.h:36