YARP
Yet Another Robot Platform
SVD.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 
14 #include <yarp/math/SVD.h>
15 #include <yarp/math/Math.h>
16 
17 #include <yarp/eigen/Eigen.h>
18 
19 #include <Eigen/SVD>
20 
21 
22 using namespace yarp::sig;
23 using namespace yarp::eigen;
24 
25 void yarp::math::SVD(const Matrix &in, Matrix &U, Vector &S, Matrix &V)
26 {
27  SVDJacobi(in,U,S,V);
28 }
29 
30 void yarp::math::SVDMod(const Matrix &in, Matrix &U, Vector &S, Matrix &V)
31 {
32  SVDJacobi(in,U,S,V);
33 }
34 
35 void yarp::math::SVDJacobi(const Matrix &in, Matrix &U, Vector &S, Matrix &V)
36 {
37  Eigen::JacobiSVD< Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> > svd(toEigen(in), Eigen::ComputeThinU | Eigen::ComputeThinV);
38 
39  U.resize(svd.matrixU().rows(),svd.matrixU().cols());
40  toEigen(U) = svd.matrixU();
41 
42  S.resize(svd.singularValues().size());
43  toEigen(S) = svd.singularValues();
44 
45  V.resize(svd.matrixV().rows(),svd.matrixV().cols());
46  toEigen(V) = svd.matrixV();
47 }
48 
49 Matrix yarp::math::pinv(const Matrix &in, double tol)
50 {
51  int m = in.rows(), n = in.cols(), k = m<n?m:n;
52  Matrix U(m,k), V(n,k);
53  Vector Sdiag(k);
54 
55  yarp::math::SVD(in, U, Sdiag, V);
56 
57  Matrix Spinv = zeros(k,k);
58  for (int c=0;c<k; c++)
59  if ( Sdiag(c)> tol)
60  Spinv(c,c) = 1/Sdiag(c);
61  return V*Spinv*U.transposed();
62 }
63 
64 void yarp::math::pinv(const Matrix &in, Matrix &out, double tol)
65 {
66  int m = in.rows(), n = in.cols(), k = m<n?m:n;
67  Matrix U(m,k), V(n,k);
68  Vector Sdiag(k);
69 
70  yarp::math::SVD(in, U, Sdiag, V);
71 
72  Matrix Spinv = zeros(k,k);
73  for (int c=0;c<k; c++)
74  if ( Sdiag(c)> tol)
75  Spinv(c,c) = 1/Sdiag(c);
76  out = V*Spinv*U.transposed();
77 }
78 
79 Matrix yarp::math::pinv(const Matrix &in, Vector &sv, double tol)
80 {
81  int m = in.rows(), n = in.cols(), k = m<n?m:n;
82  Matrix U(m,k), V(n,k);
83  if((int)sv.size()!=k)
84  sv.resize(k);
85 
86  yarp::math::SVD(in, U, sv, V);
87 
88  Matrix Spinv = zeros(k,k);
89  for (int c=0;c<k; c++)
90  if ( sv(c)> tol)
91  Spinv(c,c) = 1/sv(c);
92 
93  return V*Spinv*U.transposed();
94 }
95 
96 void yarp::math::pinv(const Matrix &in, Matrix &out, Vector &sv, double tol)
97 {
98  int m = in.rows(), n = in.cols(), k = m<n?m:n;
99  Matrix U(m,k), V(n,k);
100  if((int)sv.size()!=k)
101  sv.resize(k);
102 
103  yarp::math::SVD(in, U, sv, V);
104 
105  Matrix Spinv = zeros(k,k);
106  for (int c=0;c<k; c++)
107  if ( sv(c)> tol)
108  Spinv(c,c) = 1/sv(c);
109 
110  out = V*Spinv*U.transposed();
111 }
112 
113 Matrix yarp::math::pinvDamped(const Matrix &in, Vector &sv, double damp)
114 {
115  Matrix out(in.cols(), in.rows());
116  pinvDamped(in, out, sv, damp);
117  return out;
118 }
119 
120 Matrix yarp::math::pinvDamped(const Matrix &in, double damp)
121 {
122  int k = in.rows()<in.cols() ? in.rows() : in.cols();
123  Vector sv(k);
124  Matrix out(in.cols(), in.rows());
125  pinvDamped(in, out, sv, damp);
126  return out;
127 }
128 
129 void yarp::math::pinvDamped(const Matrix &in, Matrix &out, double damp)
130 {
131  int k = in.rows()<in.cols() ? in.rows() : in.cols();
132  Vector sv(k);
133  pinvDamped(in, out, sv, damp);
134 }
135 
136 void yarp::math::pinvDamped(const Matrix &in, Matrix &out, Vector &sv, double damp)
137 {
138  int m = in.rows(), n = in.cols(), k = m<n?m:n;
139  Matrix U(m,k), V(n,k);
140  if((int)sv.size()!=k)
141  sv.resize(k);
142 
143  yarp::math::SVD(in, U, sv, V);
144 
145  Matrix Spinv = zeros(k,k);
146  double damp2 = damp*damp;
147  for (int c=0;c<k; c++)
148  Spinv(c,c) = sv(c) / (sv(c)*sv(c) + damp2);
149 
150  out = V*Spinv*U.transposed();
151 }
152 
154 {
155  Matrix out(A.rows(),A.rows());
156  projectionMatrix(A, out, tol);
157  return out;
158 }
159 
160 void yarp::math::projectionMatrix(const Matrix &A, Matrix &out, double tol)
161 {
162  int m = A.rows();
163  int n = A.cols();
164  int k = std::min(m, n);
165  Matrix U(m, k);
166  Matrix V(n, k);
167  Vector Sdiag(k);
168  yarp::math::SVD(A, U, Sdiag, V);
169  Matrix UT = U.transposed();
170  for(int c = 0; c < k; c++) {
171  if(Sdiag(c) <= tol) {
172  UT.setRow(c, zeros(m));
173  }
174  }
175  out = U*UT;
176 }
177 
179 {
180  Matrix out(A.cols(),A.cols());
181  nullspaceProjection(A, out, tol);
182  return out;
183 }
184 
185 void yarp::math::nullspaceProjection(const Matrix &A, Matrix &out, double tol)
186 {
187  int m = A.rows();
188  int n = A.cols();
189  int k = std::min(m, n);
190  Matrix U(m,k);
191  Matrix V(n,k);
192  Vector Sdiag(k);
193  yarp::math::SVD(A, U, Sdiag, V);
194  Matrix VT = V.transposed();
195  for (int c = 0; c < k; c++) {
196  if (Sdiag(c) <= tol) {
197  VT.setRow(c, zeros(n));
198  }
199  }
200  out = eye(n) - V*VT;
201 }
A class for a Matrix.
Definition: Matrix.h:46
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
bool setRow(size_t row, const Vector &r)
Set a row of the matrix copying the values from a vector: the vector length must be equal to the numb...
Definition: Matrix.cpp:479
size_t cols() const
Return number of columns.
Definition: Matrix.h:101
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
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
yarp::sig::Matrix projectionMatrix(const yarp::sig::Matrix &A, double tol=0.0)
Compute the projection matrix of A, that is defined as A times its pseudoinverse: A*pinv(A) (defined ...
Definition: SVD.cpp:153
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
void SVDMod(const yarp::sig::Matrix &in, yarp::sig::Matrix &U, yarp::sig::Vector &S, yarp::sig::Matrix &V)
Perform SVD decomposition on a MxN matrix (for M >= N) (defined in SVD.h).
Definition: SVD.cpp:30
yarp::sig::Matrix pinv(const yarp::sig::Matrix &in, double tol=0.0)
Perform the moore-penrose pseudo-inverse of a matrix (defined in SVD.h).
Definition: SVD.cpp:49
yarp::sig::Matrix pinvDamped(const yarp::sig::Matrix &in, yarp::sig::Vector &sv, double damp)
Perform the damped pseudo-inverse of a matrix (defined in SVD.h).
Definition: SVD.cpp:113
yarp::sig::Vector zeros(int s)
Creates a vector of zeros (defined in Math.h).
Definition: math.cpp:552
yarp::sig::Matrix nullspaceProjection(const yarp::sig::Matrix &A, double tol=0.0)
Compute the nullspace projection matrix of A, that is defined as the difference between the identity ...
Definition: SVD.cpp:178
void SVDJacobi(const yarp::sig::Matrix &in, yarp::sig::Matrix &U, yarp::sig::Vector &S, yarp::sig::Matrix &V)
Perform SVD decomposition on a matrix using the Jacobi method (defined in SVD.h).
Definition: SVD.cpp:35
Signal processing.
Definition: Image.h:25