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