Visual Servoing Platform  version 3.4.0
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 <cmath> // For std::abs() on iOS
41 #include <cstdlib> // For std::abs() on iOS
42 #include <algorithm> // for (std::min) and (std::max)
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_math.h>
61 # include <gsl/gsl_vector.h>
62 # include <gsl/gsl_matrix.h>
63 # include <gsl/gsl_blas.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,
92  double *tau, double *work, 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)
123 
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", rowNum,
222  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  }
502  else {
503  gsl_Q = gsl_matrix_alloc(rowNum, rowNum); // M by M
504 
505  gsl_linalg_QR_unpack(gsl_A, gsl_tau, gsl_Q, gsl_R);
506 // std::cout << "gsl_Q:\n "; display_gsl(gsl_Q);
507 // std::cout << "gsl_R:\n "; display_gsl(gsl_R);
508 
509  unsigned int Qtda = static_cast<unsigned int>(gsl_Q->tda);
510  size_t len = sizeof(double) * Q.colNum;
511  for(unsigned int i = 0; i < Q.rowNum; i++) {
512  unsigned int k = i * Qtda;
513  memcpy(Q[i], &gsl_Q->data[k], len);
514 // for(unsigned int j = 0; j < Q.colNum; j++) {
515 // Q[i][j] = gsl_Q->data[k + j];
516 // }
517  }
518  gsl_matrix_free(gsl_Q);
519  }
520 
521  // copy useful part of R and update rank
522  na = std::min(m, n);
523  unsigned int Rtda = static_cast<unsigned int>(gsl_R->tda);
524  size_t Rlen = sizeof(double) * R.colNum;
525  for(unsigned int i = 0; i < na; i++) {
526  unsigned int k = i * Rtda;
527  memcpy(R[i], &gsl_R->data[k], Rlen);
528 // for(unsigned int j = i; j < na; j++)
529 // R[i][j] = gsl_R->data[k + j];
530  if(std::abs(gsl_R->data[k + i]) < tol)
531  r--;
532  }
533 
534  gsl_matrix_free(gsl_A);
535  gsl_matrix_free(gsl_R);
536  gsl_vector_free(gsl_tau);
537 
538  return r;
539 #else
540  integer m = (integer)rowNum; // also rows of Q
541  integer n = (integer)colNum; // also columns of R
542  integer r = std::min(n, m); // a priori non-null rows of R = rank of R
543  integer q = r; // columns of Q and rows of R
544  integer na = n; // columns of A
545 
546  // cannot be full decomposition if m < n
547  if(full && m > n) {
548  q = m; // Q is square
549  na = m; // A is square
550  }
551 
552  // prepare matrices and deal with r = 0
553  Q.resize(static_cast<unsigned int>(m), static_cast<unsigned int>(q));
554  if(squareR)
555  R.resize(static_cast<unsigned int>(r), static_cast<unsigned int>(r));
556  else
557  R.resize(static_cast<unsigned int>(r), static_cast<unsigned int>(n));
558  if(r == 0)
559  return 0;
560 
561  integer dimWork = -1;
562  double * qrdata = new double[m*na];
563  double *tau = new double[std::min(m,q)];
564  double *work = new double[1];
565  integer info;
566 
567  // copy this to qrdata in Lapack convention
568  for (integer i = 0; i < m; ++i)
569  {
570  for (integer j = 0; j < n; ++j)
571  qrdata[i + m * j] = data[j + n * i];
572  for (integer j = n; j < na; ++j)
573  qrdata[i + m * j] = 0;
574  }
575 
576  // work = new double[1];
577  //1) Extract householder reflections (useful to compute Q) and R
578  dgeqrf_(
579  &m, //The number of rows of the matrix A. M >= 0.
580  &na, //The number of columns of the matrix A. N >= 0.
581  qrdata,
582  &m,
583  tau,
584  work, //Internal working array. dimension (MAX(1,LWORK))
585  &dimWork, //The dimension of the array WORK. LWORK >= max(1,N).
586  &info //status
587  );
588 
589  if(info != 0){
590  std::cout << "dgeqrf_:Preparation:" << -info << "th element had an illegal value" << std::endl;
591  delete[] qrdata;
592  delete[] work;
593  delete[] tau;
595  }
596  dimWork = allocate_work(&work);
597 
598  dgeqrf_(
599  &m, //The number of rows of the matrix A. M >= 0.
600  &na, //The number of columns of the matrix A. N >= 0.
601  qrdata,
602  &m, //The leading dimension of the array A. LDA >= max(1,M).
603  tau,
604  work, //Internal working array. dimension (MAX(1,LWORK))
605  &dimWork, //The dimension of the array WORK. LWORK >= max(1,N).
606  &info //status
607  );
608 
609  if(info != 0){
610  std::cout << "dgeqrf_:" << -info << "th element had an illegal value" << std::endl;
611  delete[] qrdata;
612  delete[] work;
613  delete[] tau;
615  }
616 
617  // data now contains the R matrix in its upper triangular (in lapack convention)
618 
619  // copy useful part of R from Q and update rank
620  na = std::min(m,n);
621  if(squareR)
622  {
623  for(int i=0;i<na;i++)
624  {
625  for(int j=i;j<na;j++)
626  R[i][j] = qrdata[i+m*j];
627  if(std::abs(qrdata[i+m*i]) < tol)
628  r--;
629  }
630  }
631  else
632  {
633  for(int i=0;i<na;i++)
634  {
635  for(int j=i;j<n;j++)
636  R[i][j] = qrdata[i+m*j];
637  if(std::abs(qrdata[i+m*i]) < tol)
638  r--;
639  }
640  }
641 
642  // extract Q
643  dorgqr_(&m, // The number of rows of the matrix Q. M >= 0.
644  &q, // The number of columns of the matrix Q. M >= N >= 0.
645  &q,
646  qrdata,
647  &m, //The leading dimension of the array A. LDA >= max(1,M).
648  tau,
649  work, //Internal working array. dimension (MAX(1,LWORK))
650  &dimWork, //The dimension of the array WORK. LWORK >= max(1,N).
651  &info //status
652  );
653 
654  // write qrdata into Q
655  for(int i = 0; i < m; ++i)
656  for(int j = 0; j < q; ++j)
657  Q[i][j] = qrdata[i+m*j];
658 
659  delete[] qrdata;
660  delete[] work;
661  delete[] tau;
662  return (unsigned int) r;
663 #endif // defined(VISP_HAVE_GSL)
664 #else
665  (void)Q;
666  (void)R;
667  (void)full;
668  (void)squareR;
669  (void)tol;
670  throw(vpException(vpException::fatalError, "Cannot perform QR decomposition. Install Lapack 3rd party"));
671 #endif
672 }
673 
674 
737 unsigned int vpMatrix::qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full, bool squareR, double tol) const
738 {
739 #if defined(VISP_HAVE_LAPACK)
740 #if defined(VISP_HAVE_GSL)
741  unsigned int m = rowNum; // also rows of Q
742  unsigned int n = colNum; // also columns of R
743  unsigned int r = std::min(n, m); // a priori non-null rows of R = rank of R
744  unsigned int q = r; // columns of Q and rows of R
745  unsigned int na = n; // columns of A
746 
747  // cannot be full decomposition if m < n
748  if(full && m > n) {
749  q = m; // Q is square
750  na = m; // A is square
751  }
752 
753  // prepare Q and deal with r = 0
754  Q.resize(m, q, false);
755  if(r == 0) {
756  if(squareR) {
757  R.resize(0, 0, false);
758  P.resize(0, n, false);
759  }
760  else {
761  R.resize(r, n, false);
762  P.resize(n, n);
763  }
764  return 0;
765  }
766 
767  gsl_matrix gsl_A, *gsl_Q, *gsl_R, gsl_Qfull;
768  gsl_vector *gsl_tau;
769  gsl_permutation *gsl_p;
770  gsl_vector *gsl_norm;
771  int gsl_signum;
772 
773  gsl_A.size1 = rowNum;
774  gsl_A.size2 = colNum;
775  gsl_A.tda = gsl_A.size2;
776  gsl_A.data = this->data;
777  gsl_A.owner = 0;
778  gsl_A.block = 0;
779 
780  gsl_R = gsl_matrix_alloc(rowNum, colNum); // M by N
781  gsl_tau = gsl_vector_alloc(std::min(rowNum, colNum));
782  gsl_norm = gsl_vector_alloc(colNum);
783  gsl_p = gsl_permutation_alloc(n);
784 
785  if (full) {
786  // Q is M by M as expected by gsl_linalg_QR_unpack()
787  // for performance purpose dont allocate gsl_Q, but rather use gsl_Qfull = Q
788  gsl_Qfull.size1 = Q.rowNum;
789  gsl_Qfull.size2 = Q.colNum;
790  gsl_Qfull.tda = gsl_Qfull.size2;
791  gsl_Qfull.data = Q.data;
792  gsl_Qfull.owner = 0;
793  gsl_Qfull.block = 0;
794 
795  gsl_linalg_QRPT_decomp2(&gsl_A, &gsl_Qfull, gsl_R, gsl_tau, gsl_p, &gsl_signum, gsl_norm);
796 // std::cout << "gsl_Qfull:\n "; display_gsl(&gsl_Qfull);
797 // std::cout << "gsl_R:\n "; display_gsl(gsl_R);
798  }
799  else {
800  gsl_Q = gsl_matrix_alloc(rowNum, rowNum); // M by M
801 
802  gsl_linalg_QRPT_decomp2(&gsl_A, gsl_Q, gsl_R, gsl_tau, gsl_p, &gsl_signum, gsl_norm);
803 // std::cout << "gsl_Q:\n "; display_gsl(gsl_Q);
804 // std::cout << "gsl_R:\n "; display_gsl(gsl_R);
805 
806  unsigned int Qtda = static_cast<unsigned int>(gsl_Q->tda);
807  size_t len = sizeof(double) * Q.colNum;
808  for(unsigned int i = 0; i < Q.rowNum; i++) {
809  unsigned int k = i * Qtda;
810  memcpy(Q[i], &gsl_Q->data[k], len);
811 // for(unsigned int j = 0; j < Q.colNum; j++) {
812 // Q[i][j] = gsl_Q->data[k + j];
813 // }
814  }
815  gsl_matrix_free(gsl_Q);
816  }
817 
818  // update rank
819  na = std::min(m, n);
820  unsigned int Rtda = static_cast<unsigned int>(gsl_R->tda);
821  for(unsigned int i = 0; i < na; i++) {
822  unsigned int k = i * Rtda;
823  if(std::abs(gsl_R->data[k + i]) < tol)
824  r--;
825  }
826 
827  if(squareR) {
828  R.resize(r, r, false); // R r x r
829  P.resize(r, n);
830  for(unsigned int i = 0; i < r; ++i)
831  P[i][gsl_p->data[i]] = 1;
832  }
833  else {
834  R.resize(na, n, false); // R is min(m,n) x n of rank r
835  P.resize(n, n);
836  for(unsigned int i = 0; i < n; ++i)
837  P[i][gsl_p->data[i]] = 1;
838  }
839 
840  // copy useful part of R
841  size_t Rlen = sizeof(double) * R.colNum;
842  for(unsigned int i = 0; i < na; i++) {
843  unsigned int k = i * Rtda;
844  memcpy(R[i], &gsl_R->data[k], Rlen);
845  }
846 
847  gsl_matrix_free(gsl_R);
848  gsl_vector_free(gsl_tau);
849  gsl_vector_free(gsl_norm);
850  gsl_permutation_free(gsl_p);
851 
852  return r;
853 #else
854  integer m = (integer)rowNum; // also rows of Q
855  integer n = (integer)colNum; // also columns of R
856  integer r = std::min(n, m); // a priori non-null rows of R = rank of R
857  integer q = r; // columns of Q and rows of R
858  integer na = n; // columns of A
859 
860  // cannot be full decomposition if m < n
861  // cannot be full decomposition if m < n
862  if(full && m > n) {
863  q = m; // Q is square
864  na = m; // A is square
865  }
866 
867  // prepare Q and deal with r = 0
868  Q.resize(static_cast<unsigned int>(m), static_cast<unsigned int>(q));
869  if(r == 0) {
870  if(squareR) {
871  R.resize(0, 0);
872  P.resize(0, static_cast<unsigned int>(n));
873  }
874  else {
875  R.resize(static_cast<unsigned int>(r), static_cast<unsigned int>(n));
876  P.resize(static_cast<unsigned int>(n), static_cast<unsigned int>(n));
877  }
878  return 0;
879  }
880 
881  integer dimWork = -1;
882  double* qrdata = new double[m*na];
883  double* tau = new double[std::min(q,m)];
884  double* work = new double[1];
885  integer* p = new integer[na];
886  for(int i = 0; i < na; ++i)
887  p[i] = 0;
888 
889  integer info;
890 
891  // copy this to qrdata in Lapack convention
892  for(integer i = 0; i < m; ++i)
893  {
894  for(integer j = 0; j < n; ++j)
895  qrdata[i+m*j] = data[j + n*i];
896  for(integer j = n; j < na; ++j)
897  qrdata[i+m*j] = 0;
898  }
899 
900  //1) Extract householder reflections (useful to compute Q) and R
901  dgeqp3_(
902  &m, //The number of rows of the matrix A. M >= 0.
903  &na, //The number of columns of the matrix A. N >= 0.
904  qrdata, /*On entry, the M-by-N matrix A. */
905  &m, //The leading dimension of the array A. LDA >= max(1,M).
906  p, // Dimension N
907  tau, /*Dimension (min(M,N)) */
908  work, //Internal working array. dimension (3*N)
909 
910  &dimWork,
911  &info //status
912  );
913 
914  if(info != 0){
915  std::cout << "dgeqp3_:Preparation:" << -info << "th element had an illegal value" << std::endl;
916  delete[] qrdata;
917  delete[] work;
918  delete[] tau;
919  delete[] p;
921  }
922 
923  dimWork = allocate_work(&work);
924 
925  dgeqp3_(
926  &m, //The number of rows of the matrix A. M >= 0.
927  &na, //The number of columns of the matrix A. N >= 0.
928  qrdata, /*On entry, the M-by-N matrix A. */
929  &m, //The leading dimension of the array A. LDA >= max(1,M).
930  p, // Dimension N
931  tau, /*Dimension (min(M,N)) */
932  work, //Internal working array. dimension (3*N)
933 
934  &dimWork,
935  &info //status
936  );
937 
938  if(info != 0){
939  std::cout << "dgeqp3_:" << -info << " th element had an illegal value" << std::endl;
940  delete[] qrdata;
941  delete[] work;
942  delete[] tau;
943  delete[] p;
945  }
946 
947  // data now contains the R matrix in its upper triangular (in lapack convention)
948  // get rank of R in r
949  na = std::min(n,m);
950  for(int i = 0; i < na; ++i)
951  if(std::abs(qrdata[i+m*i]) < tol)
952  r--;
953 
954  // write R
955  if(squareR) // R r x r
956  {
957  R.resize(static_cast<unsigned int>(r), static_cast<unsigned int>(r));
958  for(int i=0;i<r;i++)
959  for(int j=i;j<r;j++)
960  R[i][j] = qrdata[i+m*j];
961 
962  // write P
963  P.resize(static_cast<unsigned int>(r), static_cast<unsigned int>(n));
964  for(int i = 0; i < r; ++i)
965  P[i][p[i]-1] = 1;
966  }
967  else // R is min(m,n) x n of rank r
968  {
969  R.resize(static_cast<unsigned int>(na), static_cast<unsigned int>(n));
970  for(int i=0;i<na;i++)
971  for(int j=i;j<n;j++)
972  R[i][j] = qrdata[i+m*j];
973  // write P
974  P.resize(static_cast<unsigned int>(n), static_cast<unsigned int>(n));
975  for(int i = 0; i < n; ++i)
976  P[i][p[i]-1] = 1;
977  }
978 
979  // extract Q
980  dorgqr_(&m, // The number of rows of the matrix Q. M >= 0.
981  &q, // The number of columns of the matrix Q. M >= N >= 0.
982  &q,
983  qrdata,
984  &m, //The leading dimension of the array A. LDA >= max(1,M).
985  tau,
986  work, //Internal working array. dimension (MAX(1,LWORK))
987  &dimWork, //The dimension of the array WORK. LWORK >= max(1,N).
988  &info //status
989  );
990 
991  // write qrdata into Q
992  for(int i = 0; i < m; ++i)
993  for(int j = 0; j < q; ++j)
994  Q[i][j] = qrdata[i+m*j];
995 
996  delete[] qrdata;
997  delete[] work;
998  delete[] tau;
999  delete[] p;
1000  return (unsigned int) r;
1001 #endif
1002 #else
1003  (void)Q;
1004  (void)R;
1005  (void)P;
1006  (void)full;
1007  (void)squareR;
1008  (void)tol;
1009  throw(vpException(vpException::fatalError, "Cannot perform QR decomposition. Install Lapack 3rd party"));
1010 #endif
1011 }
1012 
1025 {
1026  if(rowNum != colNum || rowNum == 0) {
1028  "Cannot inverse a triangular matrix (%d, %d) that is not square", rowNum, colNum));
1029  }
1030 #if defined(VISP_HAVE_LAPACK)
1031 #if defined(VISP_HAVE_GSL)
1032  vpMatrix inv;
1033  inv.resize(colNum, rowNum, false);
1034  gsl_matrix gsl_inv;
1035 
1036  gsl_inv.size1 = inv.rowNum;
1037  gsl_inv.size2 = inv.colNum;
1038  gsl_inv.tda = gsl_inv.size2;
1039  gsl_inv.data = inv.data;
1040  gsl_inv.owner = 0;
1041  gsl_inv.block = 0;
1042 
1043  unsigned int tda = static_cast<unsigned int>(gsl_inv.tda);
1044  size_t len = sizeof(double) * inv.colNum;
1045  for (unsigned int i = 0; i < rowNum; i++) {
1046  unsigned int k = i * tda;
1047  memcpy(&gsl_inv.data[k], (*this)[i], len);
1048  }
1049 
1050  if (upper) {
1051 #if (GSL_MAJOR_VERSION >= 2 && GSL_MINOR_VERSION >= 2)
1052  gsl_linalg_tri_upper_invert(&gsl_inv);
1053 #else
1054  {
1055  gsl_matrix_view m;
1056  gsl_vector_view v;
1057  for (unsigned int i = 0; i < rowNum; i++) {
1058  double *Tii = gsl_matrix_ptr(&gsl_inv, i, i);
1059  *Tii = 1.0 / *Tii;
1060  double aii = -(*Tii);
1061  if (i > 0) {
1062  m = gsl_matrix_submatrix(&gsl_inv, 0, 0, i, i);
1063  v = gsl_matrix_subcolumn(&gsl_inv, i, 0, i);
1064  gsl_blas_dtrmv(CblasUpper, CblasNoTrans, CblasNonUnit, &m.matrix, &v.vector);
1065  gsl_blas_dscal(aii, &v.vector);
1066  }
1067  }
1068  }
1069 #endif
1070  }
1071  else {
1072 #if (GSL_MAJOR_VERSION >= 2 && GSL_MINOR_VERSION >= 2)
1073  gsl_linalg_tri_lower_invert(&gsl_inv);
1074 #else
1075  {
1076  gsl_matrix_view m;
1077  gsl_vector_view v;
1078  for (unsigned int i = 0; i < rowNum; i++) {
1079  size_t j = rowNum - i - 1;
1080  double *Tjj = gsl_matrix_ptr(&gsl_inv, j, j);
1081  *Tjj = 1.0 / (*Tjj);
1082  double ajj = -(*Tjj);
1083  if (j < rowNum - 1) {
1084  m = gsl_matrix_submatrix(&gsl_inv, j + 1, j + 1, rowNum - j - 1, rowNum - j - 1);
1085  v = gsl_matrix_subcolumn(&gsl_inv, j, j + 1, rowNum - j - 1);
1086  gsl_blas_dtrmv(CblasLower, CblasNoTrans, CblasNonUnit, &m.matrix, &v.vector);
1087  gsl_blas_dscal(ajj, &v.vector);
1088  }
1089  }
1090  }
1091 #endif
1092  }
1093 
1094  return inv;
1095 #else
1096  integer n = (integer) rowNum; // lda is the number of rows because we don't use a submatrix
1097 
1098  vpMatrix R = *this;
1099  integer info;
1100 
1101  // we just use the other (upper / lower) method from Lapack
1102  if(rowNum > 1 && upper) // upper triangular
1103  dtrtri_((char *)"L", (char *)"N", &n, R.data, &n, &info);
1104  else
1105  dtrtri_((char *)"U", (char *)"N", &n, R.data, &n, &info);
1106 
1107  if (info != 0) {
1108  if (info < 0)
1109  std::cout << "dtrtri_:" << -info << "th element had an illegal value" << std::endl;
1110  else if (info > 0) {
1111  std::cout << "dtrtri_:R(" << info << "," << info << ")"
1112  << " is exactly zero. The triangular matrix is singular "
1113  "and its inverse can not be computed."
1114  << std::endl;
1116  }
1118  }
1119  return R;
1120 #endif
1121 #else
1122  (void)upper;
1123  throw(vpException(vpException::fatalError, "Cannot perform triangular inverse. Install Lapack 3rd party"));
1124 #endif
1125 }
1126 
1127 
1171 {
1172  vpMatrix Q, R, P;
1173  unsigned int r = t().qrPivot(Q, R, P, false, true);
1174  x = Q.extract(0, 0, colNum, r)
1175  * R.inverseTriangular().t()
1176  * P * b;
1177 }
1178 
1223 {
1224  vpColVector x(colNum);
1225  solveByQR(b, x);
1226  return x;
1227 }
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:153
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
vpMatrix inverseTriangular(bool upper=true) const
error that can be emited by ViSP classes.
Definition: vpException.h:71
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
unsigned int getCols() const
Definition: vpArray2D.h:279
unsigned int qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full=false, bool squareR=false, double tol=1e-6) const
unsigned int qr(vpMatrix &Q, vpMatrix &R, bool full=false, bool squareR=false, double tol=1e-6) const
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
Definition: vpMatrix.cpp:407
unsigned int getRows() const
Definition: vpArray2D.h:289
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:137
void solveByQR(const vpColVector &b, vpColVector &x) const
vpMatrix inverseByQR() const
vpMatrix t() const
Definition: vpMatrix.cpp:464
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
error that can be emited by the vpMatrix class and its derivates
vpMatrix inverseByQRLapack() const