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 getRows() const
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