Visual Servoing Platform  version 3.5.1 under development (2023-05-30)
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  * Fabien Spindler
37  *
38  *****************************************************************************/
39 
40 #include <algorithm> // for (std::min) and (std::max)
41 #include <cmath> // For std::abs() on iOS
42 #include <cstdlib> // For std::abs() on iOS
43 #include <visp3/core/vpConfig.h>
44 
45 #include <visp3/core/vpColVector.h>
46 #include <visp3/core/vpMath.h>
47 #include <visp3/core/vpMatrix.h>
48 
49 // Exception
50 #include <visp3/core/vpException.h>
51 #include <visp3/core/vpMatrixException.h>
52 
53 // Debug trace
54 #include <visp3/core/vpDebug.h>
55 
56 #ifdef VISP_HAVE_LAPACK
57 #ifdef VISP_HAVE_GSL
58 #if !(GSL_MAJOR_VERSION >= 2 && GSL_MINOR_VERSION >= 2)
59 // Needed for GSL_VERSION < 2.2 where gsl_linalg_tri_*_invert() not present
60 #include <gsl/gsl_blas.h>
61 #include <gsl/gsl_math.h>
62 #include <gsl/gsl_matrix.h>
63 #include <gsl/gsl_vector.h>
64 #endif
65 #include <gsl/gsl_linalg.h>
66 #include <gsl/gsl_permutation.h>
67 #endif
68 #ifdef VISP_HAVE_MKL
69 #include <mkl.h>
70 typedef MKL_INT integer;
71 
72 integer allocate_work(double **work)
73 {
74  integer dimWork = (integer)((*work)[0]);
75  delete[] * work;
76  *work = new double[dimWork];
77  return (integer)dimWork;
78 }
79 #elif !defined(VISP_HAVE_GSL)
80 #ifdef VISP_HAVE_LAPACK_BUILT_IN
81 typedef long int integer;
82 #else
83 typedef int integer;
84 #endif
85 extern "C" int dgeqrf_(integer *m, integer *n, double *a, integer *lda, double *tau, double *work, integer *lwork,
86  integer *info);
87 extern "C" int dormqr_(char *side, char *trans, integer *m, integer *n, integer *k, double *a, integer *lda,
88  double *tau, double *c__, integer *ldc, double *work, integer *lwork, integer *info);
89 extern "C" int dorgqr_(integer *, integer *, integer *, double *, integer *, double *, double *, integer *, integer *);
90 extern "C" int dtrtri_(char *uplo, char *diag, integer *n, double *a, integer *lda, integer *info);
91 extern "C" int dgeqp3_(integer *m, integer *n, double *a, integer *lda, integer *p, double *tau, double *work,
92  integer *lwork, integer *info);
93 
94 int allocate_work(double **work);
95 
96 int allocate_work(double **work)
97 {
98  unsigned int dimWork = (unsigned int)((*work)[0]);
99  delete[] * work;
100  *work = new double[dimWork];
101  return (int)dimWork;
102 }
103 #endif
104 #endif
105 
106 #if defined(VISP_HAVE_GSL)
107 #ifndef DOXYGEN_SHOULD_SKIP_THIS
108 void display_gsl(gsl_matrix *M)
109 {
110  // display
111  for (unsigned int i = 0; i < M->size1; i++) {
112  unsigned int k = i * M->tda;
113  for (unsigned int j = 0; j < M->size2; j++) {
114  std::cout << M->data[k + j] << " ";
115  }
116  std::cout << std::endl;
117  }
118 }
119 #endif
120 #endif
121 
122 #if defined(VISP_HAVE_LAPACK)
153 {
154 #if defined(VISP_HAVE_GSL)
155  {
156  vpMatrix inv;
157  inv.resize(colNum, rowNum, false);
158  gsl_matrix *gsl_A, *gsl_Q, *gsl_R, gsl_inv;
159  gsl_vector *gsl_tau;
160 
161  gsl_A = gsl_matrix_alloc(rowNum, colNum);
162  gsl_Q = gsl_matrix_alloc(rowNum, rowNum); // M by M
163  gsl_R = gsl_matrix_alloc(rowNum, colNum); // M by N
164  gsl_tau = gsl_vector_alloc(std::min(rowNum, colNum));
165 
166  gsl_inv.size1 = inv.rowNum;
167  gsl_inv.size2 = inv.colNum;
168  gsl_inv.tda = gsl_inv.size2;
169  gsl_inv.data = inv.data;
170  gsl_inv.owner = 0;
171  gsl_inv.block = 0;
172 
173  // copy input matrix since gsl_linalg_QR_decomp() is destructive
174  unsigned int Atda = static_cast<unsigned int>(gsl_A->tda);
175  size_t len = sizeof(double) * colNum;
176  for (unsigned int i = 0; i < rowNum; i++) {
177  unsigned int k = i * Atda;
178  memcpy(&gsl_A->data[k], (*this)[i], len);
179  }
180  gsl_linalg_QR_decomp(gsl_A, gsl_tau);
181  gsl_linalg_QR_unpack(gsl_A, gsl_tau, gsl_Q, gsl_R);
182 #if (GSL_MAJOR_VERSION >= 2 && GSL_MINOR_VERSION >= 2)
183  gsl_linalg_tri_upper_invert(gsl_R);
184 #else
185  {
186  gsl_matrix_view m;
187  gsl_vector_view v;
188  for (unsigned int i = 0; i < rowNum; i++) {
189  double *Tii = gsl_matrix_ptr(gsl_R, i, i);
190  *Tii = 1.0 / (*Tii);
191  double aii = -(*Tii);
192  if (i > 0) {
193  m = gsl_matrix_submatrix(gsl_R, 0, 0, i, i);
194  v = gsl_matrix_subcolumn(gsl_R, i, 0, i);
195  gsl_blas_dtrmv(CblasUpper, CblasNoTrans, CblasNonUnit, &m.matrix, &v.vector);
196  gsl_blas_dscal(aii, &v.vector);
197  }
198  }
199  }
200 #endif
201  gsl_matrix_transpose(gsl_Q);
202 
203  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, gsl_R, gsl_Q, 0, &gsl_inv);
204  unsigned int gsl_inv_tda = static_cast<unsigned int>(gsl_inv.tda);
205  size_t inv_len = sizeof(double) * inv.colNum;
206  for (unsigned int i = 0; i < inv.rowNum; i++) {
207  unsigned int k = i * gsl_inv_tda;
208  memcpy(inv[i], &gsl_inv.data[k], inv_len);
209  }
210 
211  gsl_matrix_free(gsl_A);
212  gsl_matrix_free(gsl_Q);
213  gsl_matrix_free(gsl_R);
214  gsl_vector_free(gsl_tau);
215 
216  return inv;
217  }
218 #else
219  {
220  if (rowNum != colNum) {
221  throw(vpMatrixException(vpMatrixException::matrixError, "Cannot inverse a non-square matrix (%ux%u) by QR",
222  rowNum, colNum));
223  }
224 
225  integer rowNum_ = (integer)this->getRows();
226  integer colNum_ = (integer)this->getCols();
227  integer lda = (integer)rowNum_; // lda is the number of rows because we don't use a submatrix
228  integer dimTau = (std::min)(rowNum_, colNum_);
229  integer dimWork = -1;
230  double *tau = new double[dimTau];
231  double *work = new double[1];
232  integer info;
233  vpMatrix C;
234  vpMatrix A = *this;
235 
236  try {
237  // 1) Extract householder reflections (useful to compute Q) and R
238  dgeqrf_(&rowNum_, // The number of rows of the matrix A. M >= 0.
239  &colNum_, // The number of columns of the matrix A. N >= 0.
240  A.data, /*On entry, the M-by-N matrix A.
241  On exit, the elements on and above the diagonal of
242  the array contain the min(M,N)-by-N upper trapezoidal
243  matrix R (R is upper triangular if m >= n); the
244  elements below the diagonal, with the array TAU,
245  represent the orthogonal matrix Q as a product of
246  min(m,n) elementary reflectors.
247  */
248  &lda, // The leading dimension of the array A. LDA >= max(1,M).
249  tau, /*Dimension (min(M,N))
250  The scalar factors of the elementary reflectors
251  */
252  work, // Internal working array. dimension (MAX(1,LWORK))
253  &dimWork, // The dimension of the array WORK. LWORK >= max(1,N).
254  &info // status
255  );
256 
257  if (info != 0) {
258  std::cout << "dgeqrf_:Preparation:" << -info << "th element had an illegal value" << std::endl;
260  }
261  dimWork = allocate_work(&work);
262 
263  dgeqrf_(&rowNum_, // The number of rows of the matrix A. M >= 0.
264  &colNum_, // The number of columns of the matrix A. N >= 0.
265  A.data, /*On entry, the M-by-N matrix A.
266  On exit, the elements on and above the diagonal of
267  the array contain the min(M,N)-by-N upper trapezoidal
268  matrix R (R is upper triangular if m >= n); the
269  elements below the diagonal, with the array TAU,
270  represent the orthogonal matrix Q as a product of
271  min(m,n) elementary reflectors.
272  */
273  &lda, // The leading dimension of the array A. LDA >= max(1,M).
274  tau, /*Dimension (min(M,N))
275  The scalar factors of the elementary reflectors
276  */
277  work, // Internal working array. dimension (MAX(1,LWORK))
278  &dimWork, // The dimension of the array WORK. LWORK >= max(1,N).
279  &info // status
280  );
281 
282  if (info != 0) {
283  std::cout << "dgeqrf_:" << -info << "th element had an illegal value" << std::endl;
285  }
286 
287  // A now contains the R matrix in its upper triangular (in lapack
288  // convention)
289  C = A;
290 
291  // 2) Invert R
292  dtrtri_((char *)"U", (char *)"N", &dimTau, C.data, &lda, &info);
293  if (info != 0) {
294  if (info < 0)
295  std::cout << "dtrtri_:" << -info << "th element had an illegal value" << std::endl;
296  else if (info > 0) {
297  std::cout << "dtrtri_:R(" << info << "," << info << ")"
298  << " is exactly zero. The triangular matrix is singular "
299  "and its inverse can not be computed."
300  << std::endl;
301  std::cout << "R=" << std::endl << C << std::endl;
302  }
304  }
305 
306  // 3) Zero-fill R^-1
307  // the matrix is upper triangular for lapack but lower triangular for visp
308  // we fill it with zeros above the diagonal (where we don't need the
309  // values)
310  for (unsigned int i = 0; i < C.getRows(); i++)
311  for (unsigned int j = 0; j < C.getRows(); j++)
312  if (j > i)
313  C[i][j] = 0.;
314 
315  dimWork = -1;
316  integer ldc = lda;
317 
318  // 4) Transpose Q and left-multiply it by R^-1
319  // get R^-1*tQ
320  // C contains R^-1
321  // A contains Q
322  dormqr_((char *)"R", (char *)"T", &rowNum_, &colNum_, &dimTau, A.data, &lda, tau, C.data, &ldc, work, &dimWork,
323  &info);
324  if (info != 0) {
325  std::cout << "dormqr_:Preparation" << -info << "th element had an illegal value" << std::endl;
327  }
328  dimWork = allocate_work(&work);
329 
330  dormqr_((char *)"R", (char *)"T", &rowNum_, &colNum_, &dimTau, A.data, &lda, tau, C.data, &ldc, work, &dimWork,
331  &info);
332 
333  if (info != 0) {
334  std::cout << "dormqr_:" << -info << "th element had an illegal value" << std::endl;
336  }
337  delete[] tau;
338  delete[] work;
339  } catch (vpMatrixException &) {
340  delete[] tau;
341  delete[] work;
342  throw;
343  }
344 
345  return C;
346  }
347 #endif
348 }
349 #endif
350 
382 {
383 #if defined(VISP_HAVE_LAPACK)
384  return inverseByQRLapack();
385 #else
386  throw(vpException(vpException::fatalError, "Cannot inverse matrix by QR. Install Lapack 3rd party"));
387 #endif
388 }
389 
444 unsigned int vpMatrix::qr(vpMatrix &Q, vpMatrix &R, bool full, bool squareR, double tol) const
445 {
446 #if defined(VISP_HAVE_LAPACK)
447 #if defined(VISP_HAVE_GSL)
448  unsigned int m = rowNum; // also rows of Q
449  unsigned int n = colNum; // also columns of R
450  unsigned int r = std::min(n, m); // a priori non-null rows of R = rank of R
451  unsigned int q = r; // columns of Q and rows of R
452  unsigned int na = n; // columns of A
453 
454  // cannot be full decomposition if m < n
455  if (full && m > n) {
456  q = m; // Q is square
457  na = m; // A is square
458  }
459 
460  // prepare matrices and deal with r = 0
461  Q.resize(m, q, false);
462  if (squareR)
463  R.resize(r, r, false);
464  else
465  R.resize(r, n, false);
466  if (r == 0)
467  return 0;
468 
469  gsl_matrix *gsl_A, *gsl_Q, *gsl_R, gsl_Qfull;
470  gsl_vector *gsl_tau;
471 
472  gsl_A = gsl_matrix_alloc(rowNum, colNum);
473  gsl_R = gsl_matrix_alloc(rowNum, colNum); // M by N
474  gsl_tau = gsl_vector_alloc(std::min(rowNum, colNum));
475 
476  // copy input matrix since gsl_linalg_QR_decomp() is destructive
477  unsigned int Atda = static_cast<unsigned int>(gsl_A->tda);
478  size_t len = sizeof(double) * colNum;
479  for (unsigned int i = 0; i < rowNum; i++) {
480  unsigned int k = i * Atda;
481  memcpy(&gsl_A->data[k], (*this)[i], len);
482  // for (unsigned int j = 0; j < colNum; j++)
483  // gsl_A->data[k + j] = (*this)[i][j];
484  }
485 
486  gsl_linalg_QR_decomp(gsl_A, gsl_tau);
487 
488  if (full) {
489  // Q is M by M as expected by gsl_linalg_QR_unpack()
490  // for performance purpose dont allocate gsl_Q, but rather use gsl_Qfull = Q
491  gsl_Qfull.size1 = Q.rowNum;
492  gsl_Qfull.size2 = Q.colNum;
493  gsl_Qfull.tda = gsl_Qfull.size2;
494  gsl_Qfull.data = Q.data;
495  gsl_Qfull.owner = 0;
496  gsl_Qfull.block = 0;
497 
498  gsl_linalg_QR_unpack(gsl_A, gsl_tau, &gsl_Qfull, gsl_R);
499  // std::cout << "gsl_Qfull:\n "; display_gsl(&gsl_Qfull);
500  // std::cout << "gsl_R:\n "; display_gsl(gsl_R);
501  } else {
502  gsl_Q = gsl_matrix_alloc(rowNum, rowNum); // M by M
503 
504  gsl_linalg_QR_unpack(gsl_A, gsl_tau, gsl_Q, gsl_R);
505  // std::cout << "gsl_Q:\n "; display_gsl(gsl_Q);
506  // std::cout << "gsl_R:\n "; display_gsl(gsl_R);
507 
508  unsigned int Qtda = static_cast<unsigned int>(gsl_Q->tda);
509  size_t len = sizeof(double) * Q.colNum;
510  for (unsigned int i = 0; i < Q.rowNum; i++) {
511  unsigned int k = i * Qtda;
512  memcpy(Q[i], &gsl_Q->data[k], len);
513  // for(unsigned int j = 0; j < Q.colNum; j++) {
514  // Q[i][j] = gsl_Q->data[k + j];
515  // }
516  }
517  gsl_matrix_free(gsl_Q);
518  }
519 
520  // copy useful part of R and update rank
521  na = std::min(m, n);
522  unsigned int Rtda = static_cast<unsigned int>(gsl_R->tda);
523  size_t Rlen = sizeof(double) * R.colNum;
524  for (unsigned int i = 0; i < na; i++) {
525  unsigned int k = i * Rtda;
526  memcpy(R[i], &gsl_R->data[k], Rlen);
527  // for(unsigned int j = i; j < na; j++)
528  // R[i][j] = gsl_R->data[k + j];
529  if (std::abs(gsl_R->data[k + i]) < tol)
530  r--;
531  }
532 
533  gsl_matrix_free(gsl_A);
534  gsl_matrix_free(gsl_R);
535  gsl_vector_free(gsl_tau);
536 
537  return r;
538 #else
539  integer m = (integer)rowNum; // also rows of Q
540  integer n = (integer)colNum; // also columns of R
541  integer r = std::min(n, m); // a priori non-null rows of R = rank of R
542  integer q = r; // columns of Q and rows of R
543  integer na = n; // columns of A
544 
545  // cannot be full decomposition if m < n
546  if (full && m > n) {
547  q = m; // Q is square
548  na = m; // A is square
549  }
550 
551  // prepare matrices and deal with r = 0
552  Q.resize(static_cast<unsigned int>(m), static_cast<unsigned int>(q));
553  if (squareR)
554  R.resize(static_cast<unsigned int>(r), static_cast<unsigned int>(r));
555  else
556  R.resize(static_cast<unsigned int>(r), static_cast<unsigned int>(n));
557  if (r == 0)
558  return 0;
559 
560  integer dimWork = -1;
561  double *qrdata = new double[m * na];
562  double *tau = new double[std::min(m, q)];
563  double *work = new double[1];
564  integer info;
565 
566  // copy this to qrdata in Lapack convention
567  for (integer i = 0; i < m; ++i) {
568  for (integer j = 0; j < n; ++j)
569  qrdata[i + m * j] = data[j + n * i];
570  for (integer j = n; j < na; ++j)
571  qrdata[i + m * j] = 0;
572  }
573 
574  // work = new double[1];
575  // 1) Extract householder reflections (useful to compute Q) and R
576  dgeqrf_(&m, // The number of rows of the matrix A. M >= 0.
577  &na, // The number of columns of the matrix A. N >= 0.
578  qrdata, &m, tau,
579  work, // Internal working array. dimension (MAX(1,LWORK))
580  &dimWork, // The dimension of the array WORK. LWORK >= max(1,N).
581  &info // status
582  );
583 
584  if (info != 0) {
585  std::cout << "dgeqrf_:Preparation:" << -info << "th element had an illegal value" << std::endl;
586  delete[] qrdata;
587  delete[] work;
588  delete[] tau;
590  }
591  dimWork = allocate_work(&work);
592 
593  dgeqrf_(&m, // The number of rows of the matrix A. M >= 0.
594  &na, // The number of columns of the matrix A. N >= 0.
595  qrdata,
596  &m, // The leading dimension of the array A. LDA >= max(1,M).
597  tau,
598  work, // Internal working array. dimension (MAX(1,LWORK))
599  &dimWork, // The dimension of the array WORK. LWORK >= max(1,N).
600  &info // status
601  );
602 
603  if (info != 0) {
604  std::cout << "dgeqrf_:" << -info << "th element had an illegal value" << std::endl;
605  delete[] qrdata;
606  delete[] work;
607  delete[] tau;
609  }
610 
611  // data now contains the R matrix in its upper triangular (in lapack convention)
612 
613  // copy useful part of R from Q and update rank
614  na = std::min(m, n);
615  if (squareR) {
616  for (int i = 0; i < na; i++) {
617  for (int j = i; j < na; j++)
618  R[i][j] = qrdata[i + m * j];
619  if (std::abs(qrdata[i + m * i]) < tol)
620  r--;
621  }
622  } else {
623  for (int i = 0; i < na; i++) {
624  for (int j = i; j < n; j++)
625  R[i][j] = qrdata[i + m * j];
626  if (std::abs(qrdata[i + m * i]) < tol)
627  r--;
628  }
629  }
630 
631  // extract Q
632  dorgqr_(&m, // The number of rows of the matrix Q. M >= 0.
633  &q, // The number of columns of the matrix Q. M >= N >= 0.
634  &q, qrdata,
635  &m, // The leading dimension of the array A. LDA >= max(1,M).
636  tau,
637  work, // Internal working array. dimension (MAX(1,LWORK))
638  &dimWork, // The dimension of the array WORK. LWORK >= max(1,N).
639  &info // status
640  );
641 
642  // write qrdata into Q
643  for (int i = 0; i < m; ++i)
644  for (int j = 0; j < q; ++j)
645  Q[i][j] = qrdata[i + m * j];
646 
647  delete[] qrdata;
648  delete[] work;
649  delete[] tau;
650  return (unsigned int)r;
651 #endif // defined(VISP_HAVE_GSL)
652 #else
653  (void)Q;
654  (void)R;
655  (void)full;
656  (void)squareR;
657  (void)tol;
658  throw(vpException(vpException::fatalError, "Cannot perform QR decomposition. Install Lapack 3rd party"));
659 #endif
660 }
661 
724 unsigned int vpMatrix::qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full, bool squareR, double tol) const
725 {
726 #if defined(VISP_HAVE_LAPACK)
727 #if defined(VISP_HAVE_GSL)
728  unsigned int m = rowNum; // also rows of Q
729  unsigned int n = colNum; // also columns of R
730  unsigned int r = std::min(n, m); // a priori non-null rows of R = rank of R
731  unsigned int q = r; // columns of Q and rows of R
732  unsigned int na = n; // columns of A
733 
734  // cannot be full decomposition if m < n
735  if (full && m > n) {
736  q = m; // Q is square
737  na = m; // A is square
738  }
739 
740  // prepare Q and deal with r = 0
741  Q.resize(m, q, false);
742  if (r == 0) {
743  if (squareR) {
744  R.resize(0, 0, false);
745  P.resize(0, n, false);
746  } else {
747  R.resize(r, n, false);
748  P.resize(n, n);
749  }
750  return 0;
751  }
752 
753  gsl_matrix gsl_A, *gsl_Q, *gsl_R, gsl_Qfull;
754  gsl_vector *gsl_tau;
755  gsl_permutation *gsl_p;
756  gsl_vector *gsl_norm;
757  int gsl_signum;
758 
759  gsl_A.size1 = rowNum;
760  gsl_A.size2 = colNum;
761  gsl_A.tda = gsl_A.size2;
762  gsl_A.data = this->data;
763  gsl_A.owner = 0;
764  gsl_A.block = 0;
765 
766  gsl_R = gsl_matrix_alloc(rowNum, colNum); // M by N
767  gsl_tau = gsl_vector_alloc(std::min(rowNum, colNum));
768  gsl_norm = gsl_vector_alloc(colNum);
769  gsl_p = gsl_permutation_alloc(n);
770 
771  if (full) {
772  // Q is M by M as expected by gsl_linalg_QR_unpack()
773  // for performance purpose dont allocate gsl_Q, but rather use gsl_Qfull = Q
774  gsl_Qfull.size1 = Q.rowNum;
775  gsl_Qfull.size2 = Q.colNum;
776  gsl_Qfull.tda = gsl_Qfull.size2;
777  gsl_Qfull.data = Q.data;
778  gsl_Qfull.owner = 0;
779  gsl_Qfull.block = 0;
780 
781  gsl_linalg_QRPT_decomp2(&gsl_A, &gsl_Qfull, gsl_R, gsl_tau, gsl_p, &gsl_signum, gsl_norm);
782  // std::cout << "gsl_Qfull:\n "; display_gsl(&gsl_Qfull);
783  // std::cout << "gsl_R:\n "; display_gsl(gsl_R);
784  } else {
785  gsl_Q = gsl_matrix_alloc(rowNum, rowNum); // M by M
786 
787  gsl_linalg_QRPT_decomp2(&gsl_A, gsl_Q, gsl_R, gsl_tau, gsl_p, &gsl_signum, gsl_norm);
788  // std::cout << "gsl_Q:\n "; display_gsl(gsl_Q);
789  // std::cout << "gsl_R:\n "; display_gsl(gsl_R);
790 
791  unsigned int Qtda = static_cast<unsigned int>(gsl_Q->tda);
792  size_t len = sizeof(double) * Q.colNum;
793  for (unsigned int i = 0; i < Q.rowNum; i++) {
794  unsigned int k = i * Qtda;
795  memcpy(Q[i], &gsl_Q->data[k], len);
796  // for(unsigned int j = 0; j < Q.colNum; j++) {
797  // Q[i][j] = gsl_Q->data[k + j];
798  // }
799  }
800  gsl_matrix_free(gsl_Q);
801  }
802 
803  // update rank
804  na = std::min(m, n);
805  unsigned int Rtda = static_cast<unsigned int>(gsl_R->tda);
806  for (unsigned int i = 0; i < na; i++) {
807  unsigned int k = i * Rtda;
808  if (std::abs(gsl_R->data[k + i]) < tol)
809  r--;
810  }
811 
812  if (squareR) {
813  R.resize(r, r, false); // R r x r
814  P.resize(r, n);
815  for (unsigned int i = 0; i < r; ++i)
816  P[i][gsl_p->data[i]] = 1;
817  } else {
818  R.resize(na, n, false); // R is min(m,n) x n of rank r
819  P.resize(n, n);
820  for (unsigned int i = 0; i < n; ++i)
821  P[i][gsl_p->data[i]] = 1;
822  }
823 
824  // copy useful part of R
825  size_t Rlen = sizeof(double) * R.colNum;
826  for (unsigned int i = 0; i < na; i++) {
827  unsigned int k = i * Rtda;
828  memcpy(R[i], &gsl_R->data[k], Rlen);
829  }
830 
831  gsl_matrix_free(gsl_R);
832  gsl_vector_free(gsl_tau);
833  gsl_vector_free(gsl_norm);
834  gsl_permutation_free(gsl_p);
835 
836  return r;
837 #else
838  integer m = (integer)rowNum; // also rows of Q
839  integer n = (integer)colNum; // also columns of R
840  integer r = std::min(n, m); // a priori non-null rows of R = rank of R
841  integer q = r; // columns of Q and rows of R
842  integer na = n; // columns of A
843 
844  // cannot be full decomposition if m < n
845  // cannot be full decomposition if m < n
846  if (full && m > n) {
847  q = m; // Q is square
848  na = m; // A is square
849  }
850 
851  // prepare Q and deal with r = 0
852  Q.resize(static_cast<unsigned int>(m), static_cast<unsigned int>(q));
853  if (r == 0) {
854  if (squareR) {
855  R.resize(0, 0);
856  P.resize(0, static_cast<unsigned int>(n));
857  } else {
858  R.resize(static_cast<unsigned int>(r), static_cast<unsigned int>(n));
859  P.resize(static_cast<unsigned int>(n), static_cast<unsigned int>(n));
860  }
861  return 0;
862  }
863 
864  integer dimWork = -1;
865  double *qrdata = new double[m * na];
866  double *tau = new double[std::min(q, m)];
867  double *work = new double[1];
868  integer *p = new integer[na];
869  for (int i = 0; i < na; ++i)
870  p[i] = 0;
871 
872  integer info;
873 
874  // copy this to qrdata in Lapack convention
875  for (integer i = 0; i < m; ++i) {
876  for (integer j = 0; j < n; ++j)
877  qrdata[i + m * j] = data[j + n * i];
878  for (integer j = n; j < na; ++j)
879  qrdata[i + m * j] = 0;
880  }
881 
882  // 1) Extract householder reflections (useful to compute Q) and R
883  dgeqp3_(&m, // The number of rows of the matrix A. M >= 0.
884  &na, // The number of columns of the matrix A. N >= 0.
885  qrdata, /*On entry, the M-by-N matrix A. */
886  &m, // The leading dimension of the array A. LDA >= max(1,M).
887  p, // Dimension N
888  tau, /*Dimension (min(M,N)) */
889  work, // Internal working array. dimension (3*N)
890 
891  &dimWork,
892  &info // status
893  );
894 
895  if (info != 0) {
896  std::cout << "dgeqp3_:Preparation:" << -info << "th element had an illegal value" << std::endl;
897  delete[] qrdata;
898  delete[] work;
899  delete[] tau;
900  delete[] p;
902  }
903 
904  dimWork = allocate_work(&work);
905 
906  dgeqp3_(&m, // The number of rows of the matrix A. M >= 0.
907  &na, // The number of columns of the matrix A. N >= 0.
908  qrdata, /*On entry, the M-by-N matrix A. */
909  &m, // The leading dimension of the array A. LDA >= max(1,M).
910  p, // Dimension N
911  tau, /*Dimension (min(M,N)) */
912  work, // Internal working array. dimension (3*N)
913 
914  &dimWork,
915  &info // status
916  );
917 
918  if (info != 0) {
919  std::cout << "dgeqp3_:" << -info << " th element had an illegal value" << std::endl;
920  delete[] qrdata;
921  delete[] work;
922  delete[] tau;
923  delete[] p;
925  }
926 
927  // data now contains the R matrix in its upper triangular (in lapack convention)
928  // get rank of R in r
929  na = std::min(n, m);
930  for (int i = 0; i < na; ++i)
931  if (std::abs(qrdata[i + m * i]) < tol)
932  r--;
933 
934  // write R
935  if (squareR) // R r x r
936  {
937  R.resize(static_cast<unsigned int>(r), static_cast<unsigned int>(r));
938  for (int i = 0; i < r; i++)
939  for (int j = i; j < r; j++)
940  R[i][j] = qrdata[i + m * j];
941 
942  // write P
943  P.resize(static_cast<unsigned int>(r), static_cast<unsigned int>(n));
944  for (int i = 0; i < r; ++i)
945  P[i][p[i] - 1] = 1;
946  } else // R is min(m,n) x n of rank r
947  {
948  R.resize(static_cast<unsigned int>(na), static_cast<unsigned int>(n));
949  for (int i = 0; i < na; i++)
950  for (int j = i; j < n; j++)
951  R[i][j] = qrdata[i + m * j];
952  // write P
953  P.resize(static_cast<unsigned int>(n), static_cast<unsigned int>(n));
954  for (int i = 0; i < n; ++i)
955  P[i][p[i] - 1] = 1;
956  }
957 
958  // extract Q
959  dorgqr_(&m, // The number of rows of the matrix Q. M >= 0.
960  &q, // The number of columns of the matrix Q. M >= N >= 0.
961  &q, qrdata,
962  &m, // The leading dimension of the array A. LDA >= max(1,M).
963  tau,
964  work, // Internal working array. dimension (MAX(1,LWORK))
965  &dimWork, // The dimension of the array WORK. LWORK >= max(1,N).
966  &info // status
967  );
968 
969  // write qrdata into Q
970  for (int i = 0; i < m; ++i)
971  for (int j = 0; j < q; ++j)
972  Q[i][j] = qrdata[i + m * j];
973 
974  delete[] qrdata;
975  delete[] work;
976  delete[] tau;
977  delete[] p;
978  return (unsigned int)r;
979 #endif
980 #else
981  (void)Q;
982  (void)R;
983  (void)P;
984  (void)full;
985  (void)squareR;
986  (void)tol;
987  throw(vpException(vpException::fatalError, "Cannot perform QR decomposition. Install Lapack 3rd party"));
988 #endif
989 }
990 
1003 {
1004  if (rowNum != colNum || rowNum == 0) {
1005  throw(vpException(vpException::dimensionError, "Cannot inverse a triangular matrix (%d, %d) that is not square",
1006  rowNum, colNum));
1007  }
1008 #if defined(VISP_HAVE_LAPACK)
1009 #if defined(VISP_HAVE_GSL)
1010  vpMatrix inv;
1011  inv.resize(colNum, rowNum, false);
1012  gsl_matrix gsl_inv;
1013 
1014  gsl_inv.size1 = inv.rowNum;
1015  gsl_inv.size2 = inv.colNum;
1016  gsl_inv.tda = gsl_inv.size2;
1017  gsl_inv.data = inv.data;
1018  gsl_inv.owner = 0;
1019  gsl_inv.block = 0;
1020 
1021  unsigned int tda = static_cast<unsigned int>(gsl_inv.tda);
1022  size_t len = sizeof(double) * inv.colNum;
1023  for (unsigned int i = 0; i < rowNum; i++) {
1024  unsigned int k = i * tda;
1025  memcpy(&gsl_inv.data[k], (*this)[i], len);
1026  }
1027 
1028  if (upper) {
1029 #if (GSL_MAJOR_VERSION >= 2 && GSL_MINOR_VERSION >= 2)
1030  gsl_linalg_tri_upper_invert(&gsl_inv);
1031 #else
1032  {
1033  gsl_matrix_view m;
1034  gsl_vector_view v;
1035  for (unsigned int i = 0; i < rowNum; i++) {
1036  double *Tii = gsl_matrix_ptr(&gsl_inv, i, i);
1037  *Tii = 1.0 / *Tii;
1038  double aii = -(*Tii);
1039  if (i > 0) {
1040  m = gsl_matrix_submatrix(&gsl_inv, 0, 0, i, i);
1041  v = gsl_matrix_subcolumn(&gsl_inv, i, 0, i);
1042  gsl_blas_dtrmv(CblasUpper, CblasNoTrans, CblasNonUnit, &m.matrix, &v.vector);
1043  gsl_blas_dscal(aii, &v.vector);
1044  }
1045  }
1046  }
1047 #endif
1048  } else {
1049 #if (GSL_MAJOR_VERSION >= 2 && GSL_MINOR_VERSION >= 2)
1050  gsl_linalg_tri_lower_invert(&gsl_inv);
1051 #else
1052  {
1053  gsl_matrix_view m;
1054  gsl_vector_view v;
1055  for (unsigned int i = 0; i < rowNum; i++) {
1056  size_t j = rowNum - i - 1;
1057  double *Tjj = gsl_matrix_ptr(&gsl_inv, j, j);
1058  *Tjj = 1.0 / (*Tjj);
1059  double ajj = -(*Tjj);
1060  if (j < rowNum - 1) {
1061  m = gsl_matrix_submatrix(&gsl_inv, j + 1, j + 1, rowNum - j - 1, rowNum - j - 1);
1062  v = gsl_matrix_subcolumn(&gsl_inv, j, j + 1, rowNum - j - 1);
1063  gsl_blas_dtrmv(CblasLower, CblasNoTrans, CblasNonUnit, &m.matrix, &v.vector);
1064  gsl_blas_dscal(ajj, &v.vector);
1065  }
1066  }
1067  }
1068 #endif
1069  }
1070 
1071  return inv;
1072 #else
1073  integer n = (integer)rowNum; // lda is the number of rows because we don't use a submatrix
1074 
1075  vpMatrix R = *this;
1076  integer info;
1077 
1078  // we just use the other (upper / lower) method from Lapack
1079  if (rowNum > 1 && upper) // upper triangular
1080  dtrtri_((char *)"L", (char *)"N", &n, R.data, &n, &info);
1081  else
1082  dtrtri_((char *)"U", (char *)"N", &n, R.data, &n, &info);
1083 
1084  if (info != 0) {
1085  if (info < 0)
1086  std::cout << "dtrtri_:" << -info << "th element had an illegal value" << std::endl;
1087  else if (info > 0) {
1088  std::cout << "dtrtri_:R(" << info << "," << info << ")"
1089  << " is exactly zero. The triangular matrix is singular "
1090  "and its inverse can not be computed."
1091  << std::endl;
1093  }
1095  }
1096  return R;
1097 #endif
1098 #else
1099  (void)upper;
1100  throw(vpException(vpException::fatalError, "Cannot perform triangular inverse. Install Lapack 3rd party"));
1101 #endif
1102 }
1103 
1147 {
1148  vpMatrix Q, R, P;
1149  unsigned int r = t().qrPivot(Q, R, P, false, true);
1150  x = Q.extract(0, 0, colNum, r) * R.inverseTriangular().t() * P * b;
1151 }
1152 
1197 {
1198  vpColVector x(colNum);
1199  solveByQR(b, x);
1200  return x;
1201 }
unsigned int getCols() const
Definition: vpArray2D.h:282
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:146
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:307
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:136
unsigned int getRows() const
Definition: vpArray2D.h:292
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:138
Implementation of column vector and the associated operations.
Definition: vpColVector.h:172
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
@ dimensionError
Bad dimension.
Definition: vpException.h:95
@ 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
unsigned int qr(vpMatrix &Q, vpMatrix &R, bool full=false, bool squareR=false, double tol=1e-6) const
vpMatrix inverseTriangular(bool upper=true) const
unsigned int qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full=false, bool squareR=false, double tol=1e-6) const
vpMatrix t() const
Definition: vpMatrix.cpp:462
void solveByQR(const vpColVector &b, vpColVector &x) const
vpMatrix inverseByQR() const
vpMatrix inverseByQRLapack() const
vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
Definition: vpMatrix.cpp:405