Visual Servoing Platform  version 3.2.1 under development (2019-10-23) under development (2019-10-23)
vpMatrix_qr.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 QR decomposition.
33  *
34  * Authors:
35  * Filip Novotny
36  *
37  *****************************************************************************/
38 
39 #include <cmath> // For std::abs() on iOS
40 #include <cstdlib> // For std::abs() on iOS
41 #include <algorithm> // for (std::min) and (std::max)
42 #include <visp3/core/vpConfig.h>
43 
44 #include <visp3/core/vpColVector.h>
45 #include <visp3/core/vpMath.h>
46 #include <visp3/core/vpMatrix.h>
47 
48 // Exception
49 #include <visp3/core/vpException.h>
50 #include <visp3/core/vpMatrixException.h>
51 
52 // Debug trace
53 #include <visp3/core/vpDebug.h>
54 
55 #ifdef VISP_HAVE_LAPACK
56 # ifdef VISP_HAVE_MKL
57 #include <mkl.h>
58 typedef MKL_INT integer;
59 
60 integer allocate_work(double **work)
61 {
62  integer dimWork = (integer)((*work)[0]);
63  delete[] * work;
64  *work = new double[dimWork];
65  return (integer)dimWork;
66 }
67 # else
68 # ifdef VISP_HAVE_LAPACK_BUILT_IN
69 typedef long int integer;
70 # else
71 typedef int integer;
72 # endif
73 extern "C" int dgeqrf_(integer *m, integer *n, double *a, integer *lda, double *tau, double *work, integer *lwork,
74  integer *info);
75 extern "C" int dormqr_(char *side, char *trans, integer *m, integer *n, integer *k, double *a, integer *lda,
76  double *tau, double *c__, integer *ldc, double *work, integer *lwork, integer *info);
77 extern "C" int dorgqr_(integer *, integer *, integer *, double *, integer *, double *, double *, integer *, integer *);
78 extern "C" int dtrtri_(char *uplo, char *diag, integer *n, double *a, integer *lda, integer *info);
79 extern "C" int dgeqp3_(integer *m, integer *n, double*a, integer *lda, integer *p,
80  double *tau, double *work, integer* lwork, integer *info);
81 
82 int allocate_work(double **work);
83 
84 int allocate_work(double **work)
85 {
86  unsigned int dimWork = (unsigned int)((*work)[0]);
87  delete[] * work;
88  *work = new double[dimWork];
89  return (int)dimWork;
90 }
91 # endif
92 #endif
93 
94 #if defined(VISP_HAVE_LAPACK)
95 
125 {
126  if (rowNum != colNum) {
127  throw(vpMatrixException(vpMatrixException::matrixError, "Cannot inverse a non-square matrix (%ux%u) by QR", rowNum,
128  colNum));
129  }
130 
131  integer rowNum_ = (integer)this->getRows();
132  integer colNum_ = (integer)this->getCols();
133  integer lda = (integer)rowNum_; // lda is the number of rows because we don't use a submatrix
134  integer dimTau = (std::min)(rowNum_, colNum_);
135  integer dimWork = -1;
136  double *tau = new double[dimTau];
137  double *work = new double[1];
138  integer info;
139  vpMatrix C;
140  vpMatrix A = *this;
141 
142  try {
143  // 1) Extract householder reflections (useful to compute Q) and R
144  dgeqrf_(&rowNum_, // The number of rows of the matrix A. M >= 0.
145  &colNum_, // The number of columns of the matrix A. N >= 0.
146  A.data, /*On entry, the M-by-N matrix A.
147  On exit, the elements on and above the diagonal of
148  the array contain the min(M,N)-by-N upper trapezoidal
149  matrix R (R is upper triangular if m >= n); the
150  elements below the diagonal, with the array TAU,
151  represent the orthogonal matrix Q as a product of
152  min(m,n) elementary reflectors.
153  */
154  &lda, // The leading dimension of the array A. LDA >= max(1,M).
155  tau, /*Dimension (min(M,N))
156  The scalar factors of the elementary reflectors
157  */
158  work, // Internal working array. dimension (MAX(1,LWORK))
159  &dimWork, // The dimension of the array WORK. LWORK >= max(1,N).
160  &info // status
161  );
162 
163  if (info != 0) {
164  std::cout << "dgeqrf_:Preparation:" << -info << "th element had an illegal value" << std::endl;
166  }
167  dimWork = allocate_work(&work);
168 
169  dgeqrf_(&rowNum_, // The number of rows of the matrix A. M >= 0.
170  &colNum_, // The number of columns of the matrix A. N >= 0.
171  A.data, /*On entry, the M-by-N matrix A.
172  On exit, the elements on and above the diagonal of
173  the array contain the min(M,N)-by-N upper trapezoidal
174  matrix R (R is upper triangular if m >= n); the
175  elements below the diagonal, with the array TAU,
176  represent the orthogonal matrix Q as a product of
177  min(m,n) elementary reflectors.
178  */
179  &lda, // The leading dimension of the array A. LDA >= max(1,M).
180  tau, /*Dimension (min(M,N))
181  The scalar factors of the elementary reflectors
182  */
183  work, // Internal working array. dimension (MAX(1,LWORK))
184  &dimWork, // The dimension of the array WORK. LWORK >= max(1,N).
185  &info // status
186  );
187 
188  if (info != 0) {
189  std::cout << "dgeqrf_:" << -info << "th element had an illegal value" << std::endl;
191  }
192 
193  // A now contains the R matrix in its upper triangular (in lapack
194  // convention)
195  C = A;
196 
197  // 2) Invert R
198  dtrtri_((char *)"U", (char *)"N", &dimTau, C.data, &lda, &info);
199  if (info != 0) {
200  if (info < 0)
201  std::cout << "dtrtri_:" << -info << "th element had an illegal value" << std::endl;
202  else if (info > 0) {
203  std::cout << "dtrtri_:R(" << info << "," << info << ")"
204  << " is exactly zero. The triangular matrix is singular "
205  "and its inverse can not be computed."
206  << std::endl;
207  std::cout << "R=" << std::endl << C << std::endl;
208  }
210  }
211 
212  // 3) Zero-fill R^-1
213  // the matrix is upper triangular for lapack but lower triangular for visp
214  // we fill it with zeros above the diagonal (where we don't need the
215  // values)
216  for (unsigned int i = 0; i < C.getRows(); i++)
217  for (unsigned int j = 0; j < C.getRows(); j++)
218  if (j > i)
219  C[i][j] = 0.;
220 
221  dimWork = -1;
222  integer ldc = lda;
223 
224  // 4) Transpose Q and left-multiply it by R^-1
225  // get R^-1*tQ
226  // C contains R^-1
227  // A contains Q
228  dormqr_((char *)"R", (char *)"T", &rowNum_, &colNum_, &dimTau, A.data, &lda, tau, C.data, &ldc, work, &dimWork,
229  &info);
230  if (info != 0) {
231  std::cout << "dormqr_:Preparation" << -info << "th element had an illegal value" << std::endl;
233  }
234  dimWork = allocate_work(&work);
235 
236  dormqr_((char *)"R", (char *)"T", &rowNum_, &colNum_, &dimTau, A.data, &lda, tau, C.data, &ldc, work, &dimWork,
237  &info);
238 
239  if (info != 0) {
240  std::cout << "dormqr_:" << -info << "th element had an illegal value" << std::endl;
242  }
243  delete[] tau;
244  delete[] work;
245  } catch (vpMatrixException &) {
246  delete[] tau;
247  delete[] work;
248  throw;
249  }
250 
251  return C;
252 }
253 #endif
254 
286 {
287 #if defined(VISP_HAVE_LAPACK)
288  return inverseByQRLapack();
289 #else
290  throw(vpException(vpException::fatalError, "Cannot inverse matrix by QR. Install Lapack 3rd party"));
291 #endif
292 }
293 
294 
349 unsigned int vpMatrix::qr(vpMatrix &Q, vpMatrix &R, bool full, bool squareR, double tol) const
350 {
351 #if defined(VISP_HAVE_LAPACK)
352  integer m = (integer)rowNum; // also rows of Q
353  integer n = (integer)colNum; // also columns of R
354  integer r = std::min(n, m); // a priori non-null rows of R = rank of R
355  integer q = r; // columns of Q and rows of R
356  integer na = n; // columns of A
357 
358  // cannot be full decomposition if m < n
359  if(full && m > n)
360  {
361  q = m; // Q is square
362  na = m; // A is square
363  }
364 
365  // prepare matrices and deal with r = 0
366  Q.resize(static_cast<unsigned int>(m), static_cast<unsigned int>(q));
367  if(squareR)
368  R.resize(static_cast<unsigned int>(r), static_cast<unsigned int>(r));
369  else
370  R.resize(static_cast<unsigned int>(r), static_cast<unsigned int>(n));
371  if(r == 0)
372  return 0;
373 
374  integer dimWork = -1;
375  double * qrdata = new double[m*na];
376  double *tau = new double[std::min(m,q)];
377  double *work = new double[1];
378  integer info;
379 
380  // copy this to qrdata in Lapack convention
381  for (integer i = 0; i < m; ++i)
382  {
383  for (integer j = 0; j < n; ++j)
384  qrdata[i + m * j] = data[j + n * i];
385  for (integer j = n; j < na; ++j)
386  qrdata[i + m * j] = 0;
387  }
388 
389  // work = new double[1];
390  //1) Extract householder reflections (useful to compute Q) and R
391  dgeqrf_(
392  &m, //The number of rows of the matrix A. M >= 0.
393  &na, //The number of columns of the matrix A. N >= 0.
394  qrdata,
395  &m,
396  tau,
397  work, //Internal working array. dimension (MAX(1,LWORK))
398  &dimWork, //The dimension of the array WORK. LWORK >= max(1,N).
399  &info //status
400  );
401 
402  if(info != 0){
403  std::cout << "dgeqrf_:Preparation:" << -info << "th element had an illegal value" << std::endl;
404  delete[] qrdata;
405  delete[] work;
406  delete[] tau;
408  }
409  dimWork = allocate_work(&work);
410 
411  dgeqrf_(
412  &m, //The number of rows of the matrix A. M >= 0.
413  &na, //The number of columns of the matrix A. N >= 0.
414  qrdata,
415  &m, //The leading dimension of the array A. LDA >= max(1,M).
416  tau,
417  work, //Internal working array. dimension (MAX(1,LWORK))
418  &dimWork, //The dimension of the array WORK. LWORK >= max(1,N).
419  &info //status
420  );
421 
422  if(info != 0){
423  std::cout << "dgeqrf_:" << -info << "th element had an illegal value" << std::endl;
424  delete[] qrdata;
425  delete[] work;
426  delete[] tau;
428  }
429 
430  // data now contains the R matrix in its upper triangular (in lapack convention)
431 
432  // copy useful part of R from Q and update rank
433  na = std::min(m,n);
434  if(squareR)
435  {
436  for(int i=0;i<na;i++)
437  {
438  for(int j=i;j<na;j++)
439  R[i][j] = qrdata[i+m*j];
440  if(std::abs(qrdata[i+m*i]) < tol)
441  r--;
442  }
443  }
444  else
445  {
446  for(int i=0;i<na;i++)
447  {
448  for(int j=i;j<n;j++)
449  R[i][j] = qrdata[i+m*j];
450  if(std::abs(qrdata[i+m*i]) < tol)
451  r--;
452  }
453  }
454 
455  // extract Q
456  dorgqr_(&m, // The number of rows of the matrix Q. M >= 0.
457  &q, // The number of columns of the matrix Q. M >= N >= 0.
458  &q,
459  qrdata,
460  &m, //The leading dimension of the array A. LDA >= max(1,M).
461  tau,
462  work, //Internal working array. dimension (MAX(1,LWORK))
463  &dimWork, //The dimension of the array WORK. LWORK >= max(1,N).
464  &info //status
465  );
466 
467  // write qrdata into Q
468  for(int i = 0; i < m; ++i)
469  for(int j = 0; j < q; ++j)
470  Q[i][j] = qrdata[i+m*j];
471 
472  delete[] qrdata;
473  delete[] work;
474  delete[] tau;
475  return (unsigned int) r;
476 #else
477  (void)Q;
478  (void)R;
479  (void)full;
480  (void)squareR;
481  (void)tol;
482  throw(vpException(vpException::fatalError, "Cannot perform QR decomposition. Install Lapack 3rd party"));
483 #endif
484 }
485 
486 
549 unsigned int vpMatrix::qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full, bool squareR, double tol) const
550 {
551 #if defined(VISP_HAVE_LAPACK)
552  integer m = (integer) rowNum; // also rows of Q
553  integer n = (integer) colNum; // also columns of R
554  integer r = std::min(n,m); // a priori non-null rows of R = rank of R
555  integer q = r; // columns of Q and rows of R
556  integer na = n;
557 
558  // cannot be full decomposition if m < n
559  if(full && m > n)
560  {
561  q = m; // Q is square
562  na = m;
563  }
564 
565  // prepare Q and deal with r = 0
566  Q.resize(static_cast<unsigned int>(m), static_cast<unsigned int>(q));
567  if(r == 0)
568  {
569  if(squareR)
570  {
571  R.resize(0, 0);
572  P.resize(0, static_cast<unsigned int>(n));
573  }
574  else
575  {
576  R.resize(static_cast<unsigned int>(r), static_cast<unsigned int>(n));
577  P.resize(static_cast<unsigned int>(n), static_cast<unsigned int>(n));
578  }
579  return 0;
580  }
581 
582  integer dimWork = -1;
583  double* qrdata = new double[m*na];
584  double* tau = new double[std::min(q,m)];
585  double* work = new double[1];
586  integer* p = new integer[na];
587  for(int i = 0; i < na; ++i)
588  p[i] = 0;
589 
590  integer info;
591 
592  // copy this to qrdata in Lapack convention
593  for(integer i = 0; i < m; ++i)
594  {
595  for(integer j = 0; j < n; ++j)
596  qrdata[i+m*j] = data[j + n*i];
597  for(integer j = n; j < na; ++j)
598  qrdata[i+m*j] = 0;
599  }
600 
601  //1) Extract householder reflections (useful to compute Q) and R
602  dgeqp3_(
603  &m, //The number of rows of the matrix A. M >= 0.
604  &na, //The number of columns of the matrix A. N >= 0.
605  qrdata, /*On entry, the M-by-N matrix A. */
606  &m, //The leading dimension of the array A. LDA >= max(1,M).
607  p, // Dimension N
608  tau, /*Dimension (min(M,N)) */
609  work, //Internal working array. dimension (3*N)
610 
611  &dimWork,
612  &info //status
613  );
614 
615  if(info != 0){
616  std::cout << "dgeqp3_:Preparation:" << -info << "th element had an illegal value" << std::endl;
617  delete[] qrdata;
618  delete[] work;
619  delete[] tau;
620  delete[] p;
622  }
623 
624  dimWork = allocate_work(&work);
625 
626  dgeqp3_(
627  &m, //The number of rows of the matrix A. M >= 0.
628  &na, //The number of columns of the matrix A. N >= 0.
629  qrdata, /*On entry, the M-by-N matrix A. */
630  &m, //The leading dimension of the array A. LDA >= max(1,M).
631  p, // Dimension N
632  tau, /*Dimension (min(M,N)) */
633  work, //Internal working array. dimension (3*N)
634 
635  &dimWork,
636  &info //status
637  );
638 
639  if(info != 0){
640  std::cout << "dgeqp3_:" << -info << " th element had an illegal value" << std::endl;
641  delete[] qrdata;
642  delete[] work;
643  delete[] tau;
644  delete[] p;
646  }
647 
648 
649  // data now contains the R matrix in its upper triangular (in lapack convention)
650  // get rank of R in r
651  na = std::min(n,m);
652  for(int i = 0; i < na; ++i)
653  if(std::abs(qrdata[i+m*i]) < tol)
654  r--;
655 
656  // write R
657  if(squareR) // R r x r
658  {
659  R.resize(static_cast<unsigned int>(r), static_cast<unsigned int>(r));
660  for(int i=0;i<r;i++)
661  for(int j=i;j<r;j++)
662  R[i][j] = qrdata[i+m*j];
663 
664  // write P
665  P.resize(static_cast<unsigned int>(r), static_cast<unsigned int>(n));
666  for(int i = 0; i < r; ++i)
667  P[i][p[i]-1] = 1;
668  }
669  else // R is min(m,n) x n of rank r
670  {
671  R.resize(static_cast<unsigned int>(na), static_cast<unsigned int>(n));
672  for(int i=0;i<na;i++)
673  for(int j=i;j<n;j++)
674  R[i][j] = qrdata[i+m*j];
675  // write P
676  P.resize(static_cast<unsigned int>(n), static_cast<unsigned int>(n));
677  for(int i = 0; i < n; ++i)
678  P[i][p[i]-1] = 1;
679  }
680 
681  // extract Q
682  dorgqr_(&m, // The number of rows of the matrix Q. M >= 0.
683  &q, // The number of columns of the matrix Q. M >= N >= 0.
684  &q,
685  qrdata,
686  &m, //The leading dimension of the array A. LDA >= max(1,M).
687  tau,
688  work, //Internal working array. dimension (MAX(1,LWORK))
689  &dimWork, //The dimension of the array WORK. LWORK >= max(1,N).
690  &info //status
691  );
692 
693  // write qrdata into Q
694  for(int i = 0; i < m; ++i)
695  for(int j = 0; j < q; ++j)
696  Q[i][j] = qrdata[i+m*j];
697 
698  delete[] qrdata;
699  delete[] work;
700  delete[] tau;
701  delete[] p;
702  return (unsigned int) r;
703 #else
704  (void)Q;
705  (void)R;
706  (void)P;
707  (void)full;
708  (void)squareR;
709  (void)tol;
710  throw(vpException(vpException::fatalError, "Cannot perform QR decomposition. Install Lapack 3rd party"));
711 #endif
712 }
713 
726 {
727 #if defined(VISP_HAVE_LAPACK)
728  if(rowNum != colNum || rowNum == 0)
730 
731  integer n = (integer) rowNum; // lda is the number of rows because we don't use a submatrix
732 
733  vpMatrix R = *this;
734  integer info;
735 
736  // we just use the other (upper / lower) method from Lapack
737  if(rowNum > 1 && upper) // upper triangular
738  dtrtri_((char *)"L", (char *)"N", &n, R.data, &n, &info);
739  else
740  dtrtri_((char *)"U", (char *)"N", &n, R.data, &n, &info);
741 
742  if (info != 0) {
743  if (info < 0)
744  std::cout << "dtrtri_:" << -info << "th element had an illegal value" << std::endl;
745  else if (info > 0) {
746  std::cout << "dtrtri_:R(" << info << "," << info << ")"
747  << " is exactly zero. The triangular matrix is singular "
748  "and its inverse can not be computed."
749  << std::endl;
751  }
753  }
754  return R;
755 #else
756  (void)upper;
757  throw(vpException(vpException::fatalError, "Cannot perform triangular inverse. Install Lapack 3rd party"));
758 #endif
759 }
760 
761 
805 {
806  vpMatrix Q, R, P;
807  unsigned int r = t().qrPivot(Q, R, P, false, true);
808  x = Q.extract(0, 0, colNum, r)
809  * R.inverseTriangular().t()
810  * P * b;
811 }
812 
857 {
858  vpColVector x(colNum);
859  solveByQR(b, x);
860  return x;
861 }
vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
Definition: vpMatrix.cpp:450
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
vpMatrix inverseByQR() const
unsigned int qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full=false, bool squareR=false, double tol=1e-6) const
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
error that can be emited by ViSP classes.
Definition: vpException.h:71
unsigned int getRows() const
Definition: vpArray2D.h:289
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
vpMatrix inverseTriangular(bool upper=true) const
vpMatrix inverseByQRLapack() const
unsigned int getCols() const
Definition: vpArray2D.h:279
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
vpMatrix t() const
Definition: vpMatrix.cpp:507
unsigned int qr(vpMatrix &Q, vpMatrix &R, bool full=false, bool squareR=false, double tol=1e-6) const
void solveByQR(const vpColVector &b, vpColVector &x) const
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:137
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
error that can be emited by the vpMatrix class and its derivates