Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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>
69typedef MKL_INT integer;
70
71integer 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
80typedef long int integer;
81#else
82typedef int integer;
83#endif
84extern "C" int dgeqrf_(integer *m, integer *n, double *a, integer *lda, double *tau, double *work, integer *lwork,
85 integer *info);
86extern "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);
88extern "C" int dorgqr_(integer *, integer *, integer *, double *, integer *, double *, double *, integer *, integer *);
89extern "C" int dtrtri_(char *uplo, char *diag, integer *n, double *a, integer *lda, integer *info);
90extern "C" int dgeqp3_(integer *m, integer *n, double *a, integer *lda, integer *p, double *tau, double *work,
91 integer *lwork, integer *info);
92
93int allocate_work(double **work);
94
95int 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
107void 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
443unsigned 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
723unsigned 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{
1198 solveByQR(b, x);
1199 return x;
1200}
unsigned int getCols() const
Definition vpArray2D.h:280
Type * data
Address of the first element of the data array.
Definition vpArray2D.h:144
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition vpArray2D.h:305
unsigned int rowNum
Number of rows in the array.
Definition vpArray2D.h:134
unsigned int getRows() const
Definition vpArray2D.h:290
unsigned int colNum
Number of columns in the array.
Definition vpArray2D.h:136
Implementation of column vector and the associated operations.
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:152
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:461
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:404