Visual Servoing Platform version 3.5.0
vpMatrix_svd.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Matrix SVD decomposition.
33 *
34 * Authors:
35 * Eric Marchand
36 *
37 *****************************************************************************/
38
39#include <visp3/core/vpColVector.h>
40#include <visp3/core/vpConfig.h>
41#include <visp3/core/vpDebug.h>
42#include <visp3/core/vpException.h>
43#include <visp3/core/vpMath.h>
44#include <visp3/core/vpMatrix.h>
45#include <visp3/core/vpMatrixException.h>
46
47#include <cmath> // std::fabs
48#include <iostream>
49#include <limits> // numeric_limits
50
51#ifdef VISP_HAVE_EIGEN3
52#include <Eigen/SVD>
53#endif
54
55#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
56#include <opencv2/core/core.hpp>
57#endif
58
59#ifdef VISP_HAVE_LAPACK
60# ifdef VISP_HAVE_GSL
61# include <gsl/gsl_linalg.h>
62# endif
63# ifdef VISP_HAVE_MKL
64#include <mkl.h>
65typedef MKL_INT integer;
66# else
67# if defined(VISP_HAVE_LAPACK_BUILT_IN)
68typedef long int integer;
69# else
70typedef int integer;
71# endif
72extern "C" int dgesdd_(char *jobz, integer *m, integer *n, double *a, integer *lda, double *s, double *u, integer *ldu,
73 double *vt, integer *ldvt, double *work, integer *lwork, integer *iwork, integer *info);
74
75#include <stdio.h>
76#include <string.h>
77# endif
78#endif
79/*---------------------------------------------------------------------
80
81SVD related functions
82
83---------------------------------------------------------------------*/
84
85#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
86
154{
155 int rows = (int)this->getRows();
156 int cols = (int)this->getCols();
157 cv::Mat m(rows, cols, CV_64F, this->data);
158 cv::SVD opencvSVD(m);
159 cv::Mat opencvV = opencvSVD.vt;
160 cv::Mat opencvW = opencvSVD.w;
161 V.resize((unsigned int)opencvV.rows, (unsigned int)opencvV.cols);
162 w.resize((unsigned int)(opencvW.rows * opencvW.cols));
163
164 memcpy(V.data, opencvV.data, (size_t)(8 * opencvV.rows * opencvV.cols));
165 V = V.transpose();
166 memcpy(w.data, opencvW.data, (size_t)(8 * opencvW.rows * opencvW.cols));
167 this->resize((unsigned int)opencvSVD.u.rows, (unsigned int)opencvSVD.u.cols);
168 memcpy(this->data, opencvSVD.u.data, (size_t)(8 * opencvSVD.u.rows * opencvSVD.u.cols));
169}
170
171#endif
172
173#if defined(VISP_HAVE_LAPACK)
241{
242#ifdef VISP_HAVE_GSL
243 {
244 // GSL cannot consider M < N. In that case we transpose input matrix
245 vpMatrix U;
246 unsigned int nc = getCols();
247 unsigned int nr = getRows();
248
249 if(rowNum < colNum) {
250 U = this->transpose();
251 nc = getRows();
252 nr = getCols();
253 }
254 else {
255 nc = getCols();
256 nr = getRows();
257 }
258
259 w.resize(nc);
260 V.resize(nc, nc);
261
262 gsl_vector *work = gsl_vector_alloc(nc);
263
264 gsl_matrix A;
265 A.size1 = nr;
266 A.size2 = nc;
267 A.tda = A.size2;
268 if(rowNum < colNum) {
269 A.data = U.data;
270 }
271 else {
272 A.data = this->data;
273 }
274 A.owner = 0;
275 A.block = 0;
276
277 gsl_matrix V_;
278 V_.size1 = nc;
279 V_.size2 = nc;
280 V_.tda = V_.size2;
281 V_.data = V.data;
282 V_.owner = 0;
283 V_.block = 0;
284
285 gsl_vector S;
286 S.size = nc;
287 S.stride = 1;
288 S.data = w.data;
289 S.owner = 0;
290 S.block = 0;
291
292 gsl_linalg_SV_decomp(&A, &V_, &S, work);
293
294 if(rowNum < colNum) {
295 (*this) = V.transpose();
296 V = U;
297 }
298
299 gsl_vector_free(work);
300 }
301#else
302 {
303 w.resize(this->getCols());
304 V.resize(this->getCols(), this->getCols());
305
306 integer m = (integer)(this->getCols());
307 integer n = (integer)(this->getRows());
308 integer lda = m;
309 integer ldu = m;
310 integer ldvt = (std::min)(m, n);
311 integer info, lwork;
312
313 double wkopt;
314 double *work;
315
316 integer *iwork = new integer[8 * static_cast<integer>((std::min)(n, m))];
317
318 double *s = w.data;
319 double *a = new double[static_cast<unsigned int>(lda * n)];
320 memcpy(a, this->data, this->getRows() * this->getCols() * sizeof(double));
321 double *u = V.data;
322 double *vt = this->data;
323
324 lwork = -1;
325 dgesdd_((char *)"S", &m, &n, a, &lda, s, u, &ldu, vt, &ldvt, &wkopt, &lwork, iwork, &info);
326 lwork = (int)wkopt;
327 work = new double[static_cast<unsigned int>(lwork)];
328
329 dgesdd_((char *)"S", &m, &n, a, &lda, s, u, &ldu, vt, &ldvt, work, &lwork, iwork, &info);
330
331 if (info > 0) {
332 throw(vpMatrixException(vpMatrixException::fatalError, "The algorithm computing SVD failed to converge."));
333 }
334
335 V = V.transpose();
336 delete[] work;
337 delete[] iwork;
338 delete[] a;
339 }
340#endif
341}
342#endif
343
344#ifdef VISP_HAVE_EIGEN3
412{
413 w.resize(this->getCols());
414 V.resize(this->getCols(), this->getCols());
415
416 Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > M(this->data, this->getRows(),
417 this->getCols());
418
419 Eigen::JacobiSVD<Eigen::MatrixXd> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
420
421 Eigen::Map<Eigen::VectorXd> w_(w.data, w.size());
422 Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > V_(V.data, V.getRows(),
423 V.getCols());
424 Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > U_(this->data, this->getRows(),
425 this->getCols());
426 w_ = svd.singularValues();
427 V_ = svd.matrixV();
428 U_ = svd.matrixU();
429}
430#endif
unsigned int getCols() const
Definition: vpArray2D.h:279
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
unsigned int getRows() const
Definition: vpArray2D.h:289
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:137
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
@ fatalError
Fatal error.
Definition: vpException.h:96
error that can be emited by the vpMatrix class and its derivates
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void svdLapack(vpColVector &w, vpMatrix &V)
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2030
void svdOpenCV(vpColVector &w, vpMatrix &V)
void svdEigen3(vpColVector &w, vpMatrix &V)
vpMatrix transpose() const
Definition: vpMatrix.cpp:474