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);
502 gsl_Q = gsl_matrix_alloc(rowNum, rowNum);
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;
651 #endif // defined(VISP_HAVE_GSL) 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);
1060 if (j < rowNum - 1) {
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;
1079 if (rowNum > 1 && upper)
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);
vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
Used to indicate that a value is not in the allowed range.
Implementation of a matrix and operations on matrices.
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
vpMatrix inverseByQR() const
unsigned int qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full=false, bool squareR=false, double tol=1e-6) const
error that can be emited by ViSP classes.
unsigned int getRows() const
Type * data
Address of the first element of the data array.
vpMatrix inverseTriangular(bool upper=true) const
vpMatrix inverseByQRLapack() const
unsigned int getCols() const
unsigned int rowNum
Number of rows in the array.
unsigned int qr(vpMatrix &Q, vpMatrix &R, bool full=false, bool squareR=false, double tol=1e-6) const
void solveByQR(const vpColVector &b, vpColVector &x) const
unsigned int colNum
Number of columns in the array.
Implementation of column vector and the associated operations.
error that can be emited by the vpMatrix class and its derivates