43 #include <visp3/core/vpConfig.h>
45 #include <visp3/core/vpColVector.h>
46 #include <visp3/core/vpMath.h>
47 #include <visp3/core/vpMatrix.h>
50 #include <visp3/core/vpException.h>
51 #include <visp3/core/vpMatrixException.h>
54 #include <visp3/core/vpDebug.h>
56 #ifdef VISP_HAVE_LAPACK
58 #if !(GSL_MAJOR_VERSION >= 2 && GSL_MINOR_VERSION >= 2)
60 #include <gsl/gsl_blas.h>
61 #include <gsl/gsl_math.h>
62 #include <gsl/gsl_matrix.h>
63 #include <gsl/gsl_vector.h>
65 #include <gsl/gsl_linalg.h>
66 #include <gsl/gsl_permutation.h>
70 typedef MKL_INT integer;
72 integer allocate_work(
double **work)
74 integer dimWork = (integer)((*work)[0]);
76 *work =
new double[dimWork];
77 return (integer)dimWork;
79 #elif !defined(VISP_HAVE_GSL)
80 #ifdef VISP_HAVE_LAPACK_BUILT_IN
81 typedef long int integer;
85 extern "C" int dgeqrf_(integer *m, integer *n,
double *a, integer *lda,
double *tau,
double *work, integer *lwork,
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,
double *tau,
double *work,
92 integer *lwork, integer *info);
94 int allocate_work(
double **work);
96 int allocate_work(
double **work)
98 unsigned int dimWork = (
unsigned int)((*work)[0]);
100 *work =
new double[dimWork];
106 #if defined(VISP_HAVE_GSL)
107 #ifndef DOXYGEN_SHOULD_SKIP_THIS
108 void display_gsl(gsl_matrix *M)
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] <<
" ";
116 std::cout << std::endl;
122 #if defined(VISP_HAVE_LAPACK)
154 #if defined(VISP_HAVE_GSL)
158 gsl_matrix *gsl_A, *gsl_Q, *gsl_R, gsl_inv;
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;
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);
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);
188 for (
unsigned int i = 0; i <
rowNum; i++) {
189 double *Tii = gsl_matrix_ptr(gsl_R, i, i);
191 double aii = -(*Tii);
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);
201 gsl_matrix_transpose(gsl_Q);
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);
211 gsl_matrix_free(gsl_A);
212 gsl_matrix_free(gsl_Q);
213 gsl_matrix_free(gsl_R);
214 gsl_vector_free(gsl_tau);
225 integer rowNum_ = (integer)this->
getRows();
226 integer colNum_ = (integer)this->
getCols();
227 integer lda = (integer)rowNum_;
228 integer dimTau = (std::min)(rowNum_, colNum_);
229 integer dimWork = -1;
230 double *tau =
new double[dimTau];
231 double *work =
new double[1];
258 std::cout <<
"dgeqrf_:Preparation:" << -info <<
"th element had an illegal value" << std::endl;
261 dimWork = allocate_work(&work);
283 std::cout <<
"dgeqrf_:" << -info <<
"th element had an illegal value" << std::endl;
292 dtrtri_((
char *)
"U", (
char *)
"N", &dimTau, C.
data, &lda, &info);
295 std::cout <<
"dtrtri_:" << -info <<
"th element had an illegal value" << std::endl;
297 std::cout <<
"dtrtri_:R(" << info <<
"," << info <<
")"
298 <<
" is exactly zero. The triangular matrix is singular "
299 "and its inverse can not be computed."
301 std::cout <<
"R=" << std::endl << C << std::endl;
310 for (
unsigned int i = 0; i < C.
getRows(); i++)
311 for (
unsigned int j = 0; j < C.
getRows(); j++)
322 dormqr_((
char *)
"R", (
char *)
"T", &rowNum_, &colNum_, &dimTau, A.
data, &lda, tau, C.
data, &ldc, work, &dimWork,
325 std::cout <<
"dormqr_:Preparation" << -info <<
"th element had an illegal value" << std::endl;
328 dimWork = allocate_work(&work);
330 dormqr_((
char *)
"R", (
char *)
"T", &rowNum_, &colNum_, &dimTau, A.
data, &lda, tau, C.
data, &ldc, work, &dimWork,
334 std::cout <<
"dormqr_:" << -info <<
"th element had an illegal value" << std::endl;
383 #if defined(VISP_HAVE_LAPACK)
446 #if defined(VISP_HAVE_LAPACK)
447 #if defined(VISP_HAVE_GSL)
450 unsigned int r = std::min(n, m);
469 gsl_matrix *gsl_A, *gsl_Q, *gsl_R, gsl_Qfull;
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);
486 gsl_linalg_QR_decomp(gsl_A, gsl_tau);
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;
498 gsl_linalg_QR_unpack(gsl_A, gsl_tau, &gsl_Qfull, gsl_R);
504 gsl_linalg_QR_unpack(gsl_A, gsl_tau, gsl_Q, gsl_R);
508 unsigned int Qtda =
static_cast<unsigned int>(gsl_Q->tda);
509 size_t len =
sizeof(double) * Q.
colNum;
510 for (
unsigned int i = 0; i < Q.
rowNum; i++) {
511 unsigned int k = i * Qtda;
512 memcpy(Q[i], &gsl_Q->
data[k], len);
517 gsl_matrix_free(gsl_Q);
522 unsigned int Rtda =
static_cast<unsigned int>(gsl_R->tda);
523 size_t Rlen =
sizeof(double) * R.
colNum;
524 for (
unsigned int i = 0; i < na; i++) {
525 unsigned int k = i * Rtda;
526 memcpy(R[i], &gsl_R->
data[k], Rlen);
529 if (std::abs(gsl_R->data[k + i]) < tol)
533 gsl_matrix_free(gsl_A);
534 gsl_matrix_free(gsl_R);
535 gsl_vector_free(gsl_tau);
539 integer m = (integer)
rowNum;
540 integer n = (integer)
colNum;
541 integer r = std::min(n, m);
552 Q.
resize(
static_cast<unsigned int>(m),
static_cast<unsigned int>(q));
554 R.
resize(
static_cast<unsigned int>(r),
static_cast<unsigned int>(r));
556 R.
resize(
static_cast<unsigned int>(r),
static_cast<unsigned int>(n));
560 integer dimWork = -1;
561 double *qrdata =
new double[m * na];
562 double *tau =
new double[std::min(m, q)];
563 double *work =
new double[1];
567 for (integer i = 0; i < m; ++i) {
568 for (integer j = 0; j < n; ++j)
569 qrdata[i + m * j] =
data[j + n * i];
570 for (integer j = n; j < na; ++j)
571 qrdata[i + m * j] = 0;
585 std::cout <<
"dgeqrf_:Preparation:" << -info <<
"th element had an illegal value" << std::endl;
591 dimWork = allocate_work(&work);
604 std::cout <<
"dgeqrf_:" << -info <<
"th element had an illegal value" << std::endl;
616 for (
int i = 0; i < na; i++) {
617 for (
int j = i; j < na; j++)
618 R[i][j] = qrdata[i + m * j];
619 if (std::abs(qrdata[i + m * i]) < tol)
623 for (
int i = 0; i < na; i++) {
624 for (
int j = i; j < n; j++)
625 R[i][j] = qrdata[i + m * j];
626 if (std::abs(qrdata[i + m * i]) < tol)
643 for (
int i = 0; i < m; ++i)
644 for (
int j = 0; j < q; ++j)
645 Q[i][j] = qrdata[i + m * j];
650 return (
unsigned int)r;
726 #if defined(VISP_HAVE_LAPACK)
727 #if defined(VISP_HAVE_GSL)
730 unsigned int r = std::min(n, m);
753 gsl_matrix gsl_A, *gsl_Q, *gsl_R, gsl_Qfull;
755 gsl_permutation *gsl_p;
756 gsl_vector *gsl_norm;
761 gsl_A.tda = gsl_A.size2;
762 gsl_A.data = this->
data;
768 gsl_norm = gsl_vector_alloc(
colNum);
769 gsl_p = gsl_permutation_alloc(n);
774 gsl_Qfull.size1 = Q.
rowNum;
775 gsl_Qfull.size2 = Q.
colNum;
776 gsl_Qfull.tda = gsl_Qfull.size2;
777 gsl_Qfull.data = Q.
data;
781 gsl_linalg_QRPT_decomp2(&gsl_A, &gsl_Qfull, gsl_R, gsl_tau, gsl_p, &gsl_signum, gsl_norm);
787 gsl_linalg_QRPT_decomp2(&gsl_A, gsl_Q, gsl_R, gsl_tau, gsl_p, &gsl_signum, gsl_norm);
791 unsigned int Qtda =
static_cast<unsigned int>(gsl_Q->tda);
792 size_t len =
sizeof(double) * Q.
colNum;
793 for (
unsigned int i = 0; i < Q.
rowNum; i++) {
794 unsigned int k = i * Qtda;
795 memcpy(Q[i], &gsl_Q->
data[k], len);
800 gsl_matrix_free(gsl_Q);
805 unsigned int Rtda =
static_cast<unsigned int>(gsl_R->tda);
806 for (
unsigned int i = 0; i < na; i++) {
807 unsigned int k = i * Rtda;
808 if (std::abs(gsl_R->data[k + i]) < tol)
815 for (
unsigned int i = 0; i < r; ++i)
816 P[i][gsl_p->data[i]] = 1;
820 for (
unsigned int i = 0; i < n; ++i)
821 P[i][gsl_p->data[i]] = 1;
825 size_t Rlen =
sizeof(double) * R.
colNum;
826 for (
unsigned int i = 0; i < na; i++) {
827 unsigned int k = i * Rtda;
828 memcpy(R[i], &gsl_R->
data[k], Rlen);
831 gsl_matrix_free(gsl_R);
832 gsl_vector_free(gsl_tau);
833 gsl_vector_free(gsl_norm);
834 gsl_permutation_free(gsl_p);
838 integer m = (integer)
rowNum;
839 integer n = (integer)
colNum;
840 integer r = std::min(n, m);
852 Q.
resize(
static_cast<unsigned int>(m),
static_cast<unsigned int>(q));
856 P.
resize(0,
static_cast<unsigned int>(n));
858 R.
resize(
static_cast<unsigned int>(r),
static_cast<unsigned int>(n));
859 P.
resize(
static_cast<unsigned int>(n),
static_cast<unsigned int>(n));
864 integer dimWork = -1;
865 double *qrdata =
new double[m * na];
866 double *tau =
new double[std::min(q, m)];
867 double *work =
new double[1];
868 integer *p =
new integer[na];
869 for (
int i = 0; i < na; ++i)
875 for (integer i = 0; i < m; ++i) {
876 for (integer j = 0; j < n; ++j)
877 qrdata[i + m * j] =
data[j + n * i];
878 for (integer j = n; j < na; ++j)
879 qrdata[i + m * j] = 0;
896 std::cout <<
"dgeqp3_:Preparation:" << -info <<
"th element had an illegal value" << std::endl;
904 dimWork = allocate_work(&work);
919 std::cout <<
"dgeqp3_:" << -info <<
" th element had an illegal value" << std::endl;
930 for (
int i = 0; i < na; ++i)
931 if (std::abs(qrdata[i + m * i]) < tol)
937 R.
resize(
static_cast<unsigned int>(r),
static_cast<unsigned int>(r));
938 for (
int i = 0; i < r; i++)
939 for (
int j = i; j < r; j++)
940 R[i][j] = qrdata[i + m * j];
943 P.
resize(
static_cast<unsigned int>(r),
static_cast<unsigned int>(n));
944 for (
int i = 0; i < r; ++i)
948 R.
resize(
static_cast<unsigned int>(na),
static_cast<unsigned int>(n));
949 for (
int i = 0; i < na; i++)
950 for (
int j = i; j < n; j++)
951 R[i][j] = qrdata[i + m * j];
953 P.
resize(
static_cast<unsigned int>(n),
static_cast<unsigned int>(n));
954 for (
int i = 0; i < n; ++i)
970 for (
int i = 0; i < m; ++i)
971 for (
int j = 0; j < q; ++j)
972 Q[i][j] = qrdata[i + m * j];
978 return (
unsigned int)r;
1008 #if defined(VISP_HAVE_LAPACK)
1009 #if defined(VISP_HAVE_GSL)
1014 gsl_inv.size1 = inv.
rowNum;
1015 gsl_inv.size2 = inv.
colNum;
1016 gsl_inv.tda = gsl_inv.size2;
1017 gsl_inv.data = inv.
data;
1021 unsigned int tda =
static_cast<unsigned int>(gsl_inv.tda);
1022 size_t len =
sizeof(double) * inv.
colNum;
1023 for (
unsigned int i = 0; i <
rowNum; i++) {
1024 unsigned int k = i * tda;
1025 memcpy(&gsl_inv.data[k], (*
this)[i], len);
1029 #if (GSL_MAJOR_VERSION >= 2 && GSL_MINOR_VERSION >= 2)
1030 gsl_linalg_tri_upper_invert(&gsl_inv);
1035 for (
unsigned int i = 0; i <
rowNum; i++) {
1036 double *Tii = gsl_matrix_ptr(&gsl_inv, i, i);
1038 double aii = -(*Tii);
1040 m = gsl_matrix_submatrix(&gsl_inv, 0, 0, i, i);
1041 v = gsl_matrix_subcolumn(&gsl_inv, i, 0, i);
1042 gsl_blas_dtrmv(CblasUpper, CblasNoTrans, CblasNonUnit, &m.matrix, &v.vector);
1043 gsl_blas_dscal(aii, &v.vector);
1049 #if (GSL_MAJOR_VERSION >= 2 && GSL_MINOR_VERSION >= 2)
1050 gsl_linalg_tri_lower_invert(&gsl_inv);
1055 for (
unsigned int i = 0; i <
rowNum; i++) {
1056 size_t j =
rowNum - i - 1;
1057 double *Tjj = gsl_matrix_ptr(&gsl_inv, j, j);
1058 *Tjj = 1.0 / (*Tjj);
1059 double ajj = -(*Tjj);
1061 m = gsl_matrix_submatrix(&gsl_inv, j + 1, j + 1,
rowNum - j - 1,
rowNum - j - 1);
1062 v = gsl_matrix_subcolumn(&gsl_inv, j, j + 1,
rowNum - j - 1);
1063 gsl_blas_dtrmv(CblasLower, CblasNoTrans, CblasNonUnit, &m.matrix, &v.vector);
1064 gsl_blas_dscal(ajj, &v.vector);
1073 integer n = (integer)
rowNum;
1080 dtrtri_((
char *)
"L", (
char *)
"N", &n, R.
data, &n, &info);
1082 dtrtri_((
char *)
"U", (
char *)
"N", &n, R.
data, &n, &info);
1086 std::cout <<
"dtrtri_:" << -info <<
"th element had an illegal value" << std::endl;
1087 else if (info > 0) {
1088 std::cout <<
"dtrtri_:R(" << info <<
"," << info <<
")"
1089 <<
" is exactly zero. The triangular matrix is singular "
1090 "and its inverse can not be computed."
1149 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 emited by ViSP classes.
@ badValue
Used to indicate that a value is not in the allowed range.
@ dimensionError
Bad dimension.
error that can be emited by the vpMatrix class and its derivates
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