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>
69 typedef MKL_INT integer;
71 integer 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
80 typedef long int integer;
84 extern "C" int dgeqrf_(integer *m, integer *n,
double *a, integer *lda,
double *tau,
double *work, integer *lwork,
86 extern "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);
88 extern "C" int dorgqr_(integer *, integer *, integer *,
double *, integer *,
double *,
double *, integer *, integer *);
89 extern "C" int dtrtri_(
char *uplo,
char *diag, integer *n,
double *a, integer *lda, integer *info);
90 extern "C" int dgeqp3_(integer *m, integer *n,
double *a, integer *lda, integer *p,
double *tau,
double *work,
91 integer *lwork, integer *info);
93 int allocate_work(
double **work);
95 int 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
107 void 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;
163 gsl_tau = gsl_vector_alloc(std::min<unsigned int>(
rowNum,
colNum));
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<integer>(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;
383 #if defined(VISP_HAVE_LAPACK)
446 #if defined(VISP_HAVE_LAPACK)
447 #if defined(VISP_HAVE_GSL)
450 unsigned int r = std::min<unsigned int>(n, m);
469 gsl_matrix *gsl_A, *gsl_Q, *gsl_R, gsl_Qfull;
474 gsl_tau = gsl_vector_alloc(std::min<unsigned int>(
rowNum,
colNum));
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);
505 gsl_linalg_QR_unpack(gsl_A, gsl_tau, gsl_Q, gsl_R);
509 unsigned int Qtda =
static_cast<unsigned int>(gsl_Q->tda);
510 size_t len =
sizeof(double) * Q.
colNum;
511 for (
unsigned int i = 0; i < Q.
rowNum; ++i) {
512 unsigned int k = i * Qtda;
513 memcpy(Q[i], &gsl_Q->
data[k], len);
518 gsl_matrix_free(gsl_Q);
522 na = std::min<unsigned int>(m, n);
523 unsigned int Rtda =
static_cast<unsigned int>(gsl_R->tda);
524 size_t Rlen =
sizeof(double) * R.
colNum;
525 for (
unsigned int i = 0; i < na; ++i) {
526 unsigned int k = i * Rtda;
527 memcpy(R[i], &gsl_R->
data[k], Rlen);
530 if (std::abs(gsl_R->data[k + i]) < tol)
534 gsl_matrix_free(gsl_A);
535 gsl_matrix_free(gsl_R);
536 gsl_vector_free(gsl_tau);
540 integer m = (integer)
rowNum;
541 integer n = (integer)
colNum;
542 integer r = std::min<integer>(n, m);
553 Q.
resize(
static_cast<unsigned int>(m),
static_cast<unsigned int>(q));
555 R.
resize(
static_cast<unsigned int>(r),
static_cast<unsigned int>(r));
557 R.
resize(
static_cast<unsigned int>(r),
static_cast<unsigned int>(n));
561 integer dimWork = -1;
562 double *qrdata =
new double[m * na];
563 double *tau =
new double[std::min<integer>(m, q)];
564 double *work =
new double[1];
568 for (integer i = 0; i < m; ++i) {
569 for (integer j = 0; j < n; ++j)
570 qrdata[i + m * j] =
data[j + n * i];
571 for (integer j = n; j < na; ++j)
572 qrdata[i + m * j] = 0;
586 std::cout <<
"dgeqrf_:Preparation:" << -info <<
"th element had an illegal value" << std::endl;
592 dimWork = allocate_work(&work);
605 std::cout <<
"dgeqrf_:" << -info <<
"th element had an illegal value" << std::endl;
615 na = std::min<integer>(m, n);
617 for (
int i = 0; i < na; ++i) {
618 for (
int j = i; j < na; ++j)
619 R[i][j] = qrdata[i + m * j];
620 if (std::abs(qrdata[i + m * i]) < tol)
625 for (
int i = 0; i < na; ++i) {
626 for (
int j = i; j < n; ++j)
627 R[i][j] = qrdata[i + m * j];
628 if (std::abs(qrdata[i + m * i]) < tol)
645 for (
int i = 0; i < m; ++i)
646 for (
int j = 0; j < q; ++j)
647 Q[i][j] = qrdata[i + m * j];
652 return (
unsigned int)r;
728 #if defined(VISP_HAVE_LAPACK)
729 #if defined(VISP_HAVE_GSL)
732 unsigned int r = std::min<unsigned int>(n, m);
756 gsl_matrix gsl_A, *gsl_Q, *gsl_R, gsl_Qfull;
758 gsl_permutation *gsl_p;
759 gsl_vector *gsl_norm;
764 gsl_A.tda = gsl_A.size2;
765 gsl_A.data = this->
data;
770 gsl_tau = gsl_vector_alloc(std::min<unsigned int>(
rowNum,
colNum));
771 gsl_norm = gsl_vector_alloc(
colNum);
772 gsl_p = gsl_permutation_alloc(n);
777 gsl_Qfull.size1 = Q.
rowNum;
778 gsl_Qfull.size2 = Q.
colNum;
779 gsl_Qfull.tda = gsl_Qfull.size2;
780 gsl_Qfull.data = Q.
data;
784 gsl_linalg_QRPT_decomp2(&gsl_A, &gsl_Qfull, gsl_R, gsl_tau, gsl_p, &gsl_signum, gsl_norm);
791 gsl_linalg_QRPT_decomp2(&gsl_A, gsl_Q, gsl_R, gsl_tau, gsl_p, &gsl_signum, gsl_norm);
795 unsigned int Qtda =
static_cast<unsigned int>(gsl_Q->tda);
796 size_t len =
sizeof(double) * Q.
colNum;
797 for (
unsigned int i = 0; i < Q.
rowNum; ++i) {
798 unsigned int k = i * Qtda;
799 memcpy(Q[i], &gsl_Q->
data[k], len);
804 gsl_matrix_free(gsl_Q);
808 na = std::min<unsigned int>(m, n);
809 unsigned int Rtda =
static_cast<unsigned int>(gsl_R->tda);
810 for (
unsigned int i = 0; i < na; ++i) {
811 unsigned int k = i * Rtda;
812 if (std::abs(gsl_R->data[k + i]) < tol)
819 for (
unsigned int i = 0; i < r; ++i)
820 P[i][gsl_p->data[i]] = 1;
825 for (
unsigned int i = 0; i < n; ++i)
826 P[i][gsl_p->data[i]] = 1;
830 size_t Rlen =
sizeof(double) * R.
colNum;
831 for (
unsigned int i = 0; i < na; ++i) {
832 unsigned int k = i * Rtda;
833 memcpy(R[i], &gsl_R->
data[k], Rlen);
836 gsl_matrix_free(gsl_R);
837 gsl_vector_free(gsl_tau);
838 gsl_vector_free(gsl_norm);
839 gsl_permutation_free(gsl_p);
843 integer m = (integer)
rowNum;
844 integer n = (integer)
colNum;
845 integer r = std::min<integer>(n, m);
857 Q.
resize(
static_cast<unsigned int>(m),
static_cast<unsigned int>(q));
861 P.
resize(0,
static_cast<unsigned int>(n));
864 R.
resize(
static_cast<unsigned int>(r),
static_cast<unsigned int>(n));
865 P.
resize(
static_cast<unsigned int>(n),
static_cast<unsigned int>(n));
870 integer dimWork = -1;
871 integer min_q_m = std::min<integer>(q, m);
872 double *qrdata =
new double[m * na];
873 double *tau =
new double[min_q_m];
874 double *work =
new double[1];
875 integer *p =
new integer[na];
876 for (
int i = 0; i < na; ++i)
882 for (integer i = 0; i < m; ++i) {
883 for (integer j = 0; j < n; ++j)
884 qrdata[i + m * j] =
data[j + n * i];
885 for (integer j = n; j < na; ++j)
886 qrdata[i + m * j] = 0;
903 std::cout <<
"dgeqp3_:Preparation:" << -info <<
"th element had an illegal value" << std::endl;
911 dimWork = allocate_work(&work);
926 std::cout <<
"dgeqp3_:" << -info <<
" th element had an illegal value" << std::endl;
936 na = std::min<integer>(n, m);
937 for (
int i = 0; i < na; ++i)
938 if (std::abs(qrdata[i + m * i]) < tol)
944 R.
resize(
static_cast<unsigned int>(r),
static_cast<unsigned int>(r));
945 for (
int i = 0; i < r; ++i)
946 for (
int j = i; j < r; ++j)
947 R[i][j] = qrdata[i + m * j];
950 P.
resize(
static_cast<unsigned int>(r),
static_cast<unsigned int>(n));
951 for (
int i = 0; i < r; ++i)
956 R.
resize(
static_cast<unsigned int>(na),
static_cast<unsigned int>(n));
957 for (
int i = 0; i < na; ++i)
958 for (
int j = i; j < n; ++j)
959 R[i][j] = qrdata[i + m * j];
961 P.
resize(
static_cast<unsigned int>(n),
static_cast<unsigned int>(n));
962 for (
int i = 0; i < n; ++i)
978 for (
int i = 0; i < m; ++i)
979 for (
int j = 0; j < q; ++j)
980 Q[i][j] = qrdata[i + m * j];
986 return (
unsigned int)r;
1016 #if defined(VISP_HAVE_LAPACK)
1017 #if defined(VISP_HAVE_GSL)
1022 gsl_inv.size1 = inv.
rowNum;
1023 gsl_inv.size2 = inv.
colNum;
1024 gsl_inv.tda = gsl_inv.size2;
1025 gsl_inv.data = inv.
data;
1029 unsigned int tda =
static_cast<unsigned int>(gsl_inv.tda);
1030 size_t len =
sizeof(double) * inv.
colNum;
1031 for (
unsigned int i = 0; i <
rowNum; ++i) {
1032 unsigned int k = i * tda;
1033 memcpy(&gsl_inv.data[k], (*
this)[i], len);
1037 #if (GSL_MAJOR_VERSION >= 2 && GSL_MINOR_VERSION >= 2)
1038 gsl_linalg_tri_upper_invert(&gsl_inv);
1043 for (
unsigned int i = 0; i <
rowNum; ++i) {
1044 double *Tii = gsl_matrix_ptr(&gsl_inv, i, i);
1046 double aii = -(*Tii);
1048 m = gsl_matrix_submatrix(&gsl_inv, 0, 0, i, i);
1049 v = gsl_matrix_subcolumn(&gsl_inv, i, 0, i);
1050 gsl_blas_dtrmv(CblasUpper, CblasNoTrans, CblasNonUnit, &m.matrix, &v.vector);
1051 gsl_blas_dscal(aii, &v.vector);
1058 #if (GSL_MAJOR_VERSION >= 2 && GSL_MINOR_VERSION >= 2)
1059 gsl_linalg_tri_lower_invert(&gsl_inv);
1064 for (
unsigned int i = 0; i <
rowNum; ++i) {
1065 size_t j =
rowNum - i - 1;
1066 double *Tjj = gsl_matrix_ptr(&gsl_inv, j, j);
1067 *Tjj = 1.0 / (*Tjj);
1068 double ajj = -(*Tjj);
1070 m = gsl_matrix_submatrix(&gsl_inv, j + 1, j + 1,
rowNum - j - 1,
rowNum - j - 1);
1071 v = gsl_matrix_subcolumn(&gsl_inv, j, j + 1,
rowNum - j - 1);
1072 gsl_blas_dtrmv(CblasLower, CblasNoTrans, CblasNonUnit, &m.matrix, &v.vector);
1073 gsl_blas_dscal(ajj, &v.vector);
1082 integer n = (integer)
rowNum;
1089 dtrtri_((
char *)
"L", (
char *)
"N", &n, R.
data, &n, &info);
1091 dtrtri_((
char *)
"U", (
char *)
"N", &n, R.
data, &n, &info);
1095 std::cout <<
"dtrtri_:" << -info <<
"th element had an illegal value" << std::endl;
1096 else if (info > 0) {
1097 std::cout <<
"dtrtri_:R(" << info <<
"," << info <<
")"
1098 <<
" is exactly zero. The triangular matrix is singular "
1099 "and its inverse can not be computed."
1158 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