42#include <visp3/core/vpConfig.h>
44#include <visp3/core/vpColVector.h>
45#include <visp3/core/vpMath.h>
46#include <visp3/core/vpMatrix.h>
49#include <visp3/core/vpException.h>
50#include <visp3/core/vpMatrixException.h>
53#include <visp3/core/vpDebug.h>
55#ifdef VISP_HAVE_LAPACK
57#if !(GSL_MAJOR_VERSION >= 2 && GSL_MINOR_VERSION >= 2)
59#include <gsl/gsl_blas.h>
60#include <gsl/gsl_math.h>
61#include <gsl/gsl_matrix.h>
62#include <gsl/gsl_vector.h>
64#include <gsl/gsl_linalg.h>
65#include <gsl/gsl_permutation.h>
69typedef MKL_INT integer;
71integer allocate_work(
double **work)
73 integer dimWork = (integer)((*work)[0]);
75 *work =
new double[dimWork];
76 return (integer)dimWork;
78#elif !defined(VISP_HAVE_GSL)
79#ifdef VISP_HAVE_LAPACK_BUILT_IN
80typedef long int integer;
84extern "C" int dgeqrf_(integer *m, integer *n,
double *a, integer *lda,
double *tau,
double *work, integer *lwork,
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);
93int allocate_work(
double **work);
95int allocate_work(
double **work)
97 unsigned int dimWork = (
unsigned int)((*work)[0]);
99 *work =
new double[dimWork];
105#if defined(VISP_HAVE_GSL)
106#ifndef DOXYGEN_SHOULD_SKIP_THIS
107void display_gsl(gsl_matrix *M)
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] <<
" ";
115 std::cout << std::endl;
121#if defined(VISP_HAVE_LAPACK)
153#if defined(VISP_HAVE_GSL)
157 gsl_matrix *gsl_A, *gsl_Q, *gsl_R, gsl_inv;
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;
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);
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);
187 for (
unsigned int i = 0; i <
rowNum; i++) {
188 double *Tii = gsl_matrix_ptr(gsl_R, i, i);
190 double aii = -(*Tii);
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);
200 gsl_matrix_transpose(gsl_Q);
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);
210 gsl_matrix_free(gsl_A);
211 gsl_matrix_free(gsl_Q);
212 gsl_matrix_free(gsl_R);
213 gsl_vector_free(gsl_tau);
224 integer rowNum_ = (integer)this->
getRows();
225 integer colNum_ = (integer)this->
getCols();
226 integer lda = (integer)rowNum_;
227 integer dimTau = (std::min)(rowNum_, colNum_);
228 integer dimWork = -1;
229 double *tau =
new double[dimTau];
230 double *work =
new double[1];
257 std::cout <<
"dgeqrf_:Preparation:" << -info <<
"th element had an illegal value" << std::endl;
260 dimWork = allocate_work(&work);
282 std::cout <<
"dgeqrf_:" << -info <<
"th element had an illegal value" << std::endl;
291 dtrtri_((
char *)
"U", (
char *)
"N", &dimTau, C.
data, &lda, &info);
294 std::cout <<
"dtrtri_:" << -info <<
"th element had an illegal value" << std::endl;
296 std::cout <<
"dtrtri_:R(" << info <<
"," << info <<
")"
297 <<
" is exactly zero. The triangular matrix is singular "
298 "and its inverse can not be computed."
300 std::cout <<
"R=" << std::endl << C << std::endl;
309 for (
unsigned int i = 0; i < C.
getRows(); i++)
310 for (
unsigned int j = 0; j < C.
getRows(); j++)
321 dormqr_((
char *)
"R", (
char *)
"T", &rowNum_, &colNum_, &dimTau, A.
data, &lda, tau, C.
data, &ldc, work, &dimWork,
324 std::cout <<
"dormqr_:Preparation" << -info <<
"th element had an illegal value" << std::endl;
327 dimWork = allocate_work(&work);
329 dormqr_((
char *)
"R", (
char *)
"T", &rowNum_, &colNum_, &dimTau, A.
data, &lda, tau, C.
data, &ldc, work, &dimWork,
333 std::cout <<
"dormqr_:" << -info <<
"th element had an illegal value" << std::endl;
382#if defined(VISP_HAVE_LAPACK)
445#if defined(VISP_HAVE_LAPACK)
446#if defined(VISP_HAVE_GSL)
449 unsigned int r = std::min(n, m);
468 gsl_matrix *gsl_A, *gsl_Q, *gsl_R, gsl_Qfull;
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);
485 gsl_linalg_QR_decomp(gsl_A, gsl_tau);
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;
497 gsl_linalg_QR_unpack(gsl_A, gsl_tau, &gsl_Qfull, gsl_R);
503 gsl_linalg_QR_unpack(gsl_A, gsl_tau, gsl_Q, gsl_R);
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);
516 gsl_matrix_free(gsl_Q);
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);
528 if (std::abs(gsl_R->data[k + i]) < tol)
532 gsl_matrix_free(gsl_A);
533 gsl_matrix_free(gsl_R);
534 gsl_vector_free(gsl_tau);
538 integer m = (integer)
rowNum;
539 integer n = (integer)
colNum;
540 integer r = std::min(n, m);
551 Q.
resize(
static_cast<unsigned int>(m),
static_cast<unsigned int>(q));
553 R.
resize(
static_cast<unsigned int>(r),
static_cast<unsigned int>(r));
555 R.
resize(
static_cast<unsigned int>(r),
static_cast<unsigned int>(n));
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];
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;
584 std::cout <<
"dgeqrf_:Preparation:" << -info <<
"th element had an illegal value" << std::endl;
590 dimWork = allocate_work(&work);
603 std::cout <<
"dgeqrf_:" << -info <<
"th element had an illegal value" << std::endl;
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)
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)
642 for (
int i = 0; i < m; ++i)
643 for (
int j = 0; j < q; ++j)
644 Q[i][j] = qrdata[i + m * j];
649 return (
unsigned int)r;
725#if defined(VISP_HAVE_LAPACK)
726#if defined(VISP_HAVE_GSL)
729 unsigned int r = std::min(n, m);
752 gsl_matrix gsl_A, *gsl_Q, *gsl_R, gsl_Qfull;
754 gsl_permutation *gsl_p;
755 gsl_vector *gsl_norm;
760 gsl_A.tda = gsl_A.size2;
761 gsl_A.data = this->
data;
767 gsl_norm = gsl_vector_alloc(
colNum);
768 gsl_p = gsl_permutation_alloc(n);
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;
780 gsl_linalg_QRPT_decomp2(&gsl_A, &gsl_Qfull, gsl_R, gsl_tau, gsl_p, &gsl_signum, gsl_norm);
786 gsl_linalg_QRPT_decomp2(&gsl_A, gsl_Q, gsl_R, gsl_tau, gsl_p, &gsl_signum, gsl_norm);
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);
799 gsl_matrix_free(gsl_Q);
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)
814 for (
unsigned int i = 0; i < r; ++i)
815 P[i][gsl_p->data[i]] = 1;
819 for (
unsigned int i = 0; i < n; ++i)
820 P[i][gsl_p->data[i]] = 1;
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);
830 gsl_matrix_free(gsl_R);
831 gsl_vector_free(gsl_tau);
832 gsl_vector_free(gsl_norm);
833 gsl_permutation_free(gsl_p);
837 integer m = (integer)
rowNum;
838 integer n = (integer)
colNum;
839 integer r = std::min(n, m);
851 Q.
resize(
static_cast<unsigned int>(m),
static_cast<unsigned int>(q));
855 P.
resize(0,
static_cast<unsigned int>(n));
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));
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)
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;
895 std::cout <<
"dgeqp3_:Preparation:" << -info <<
"th element had an illegal value" << std::endl;
903 dimWork = allocate_work(&work);
918 std::cout <<
"dgeqp3_:" << -info <<
" th element had an illegal value" << std::endl;
929 for (
int i = 0; i < na; ++i)
930 if (std::abs(qrdata[i + m * i]) < tol)
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];
942 P.
resize(
static_cast<unsigned int>(r),
static_cast<unsigned int>(n));
943 for (
int i = 0; i < r; ++i)
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];
952 P.
resize(
static_cast<unsigned int>(n),
static_cast<unsigned int>(n));
953 for (
int i = 0; i < n; ++i)
969 for (
int i = 0; i < m; ++i)
970 for (
int j = 0; j < q; ++j)
971 Q[i][j] = qrdata[i + m * j];
977 return (
unsigned int)r;
1007#if defined(VISP_HAVE_LAPACK)
1008#if defined(VISP_HAVE_GSL)
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;
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);
1028#if (GSL_MAJOR_VERSION >= 2 && GSL_MINOR_VERSION >= 2)
1029 gsl_linalg_tri_upper_invert(&gsl_inv);
1034 for (
unsigned int i = 0; i <
rowNum; i++) {
1035 double *Tii = gsl_matrix_ptr(&gsl_inv, i, i);
1037 double aii = -(*Tii);
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);
1048#if (GSL_MAJOR_VERSION >= 2 && GSL_MINOR_VERSION >= 2)
1049 gsl_linalg_tri_lower_invert(&gsl_inv);
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);
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);
1072 integer n = (integer)
rowNum;
1079 dtrtri_((
char *)
"L", (
char *)
"N", &n, R.
data, &n, &info);
1081 dtrtri_((
char *)
"U", (
char *)
"N", &n, R.
data, &n, &info);
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."
1148 unsigned int r =
t().
qrPivot(Q, R, P,
false,
true);
unsigned int getCols() const
Type * data
Address of the first element of the data array.
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
unsigned int rowNum
Number of rows in the array.
unsigned int getRows() const
unsigned int colNum
Number of columns in the array.
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
@ badValue
Used to indicate that a value is not in the allowed range.
@ dimensionError
Bad dimension.
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.
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
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