1 /****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Matrix LU decomposition.
33 *
34 * Authors:
35 * Eric Marchand
36 *
37 *****************************************************************************/
38
39 #include <visp3/core/vpConfig.h>
40
41 #include <visp3/core/vpColVector.h>
42 #include <visp3/core/vpMath.h>
43 #include <visp3/core/vpMatrix.h>
44
45 #ifdef VISP_HAVE_EIGEN3
46 #include <Eigen/LU>
47 #endif
48
49 #ifdef VISP_HAVE_LAPACK
50 # ifdef VISP_HAVE_GSL
51 # include <gsl/gsl_linalg.h>
52 # include <gsl/gsl_permutation.h>
53 # endif
54 # ifdef VISP_HAVE_MKL
55 # include <mkl.h>
56 typedef MKL_INT integer;
57 # else
58 # ifdef VISP_HAVE_LAPACK_BUILT_IN
59 typedef long int integer;
60 # else
61 typedef int integer;
62 # endif
63 extern "C" int dgetrf_(integer *m, integer *n, double *a, integer *lda, integer *ipiv, integer *info);
64 extern "C" void dgetri_(integer *n, double *a, integer *lda, integer *ipiv, double *work, integer *lwork,
65 integer *info);
66 # endif
67 #endif
68
69 #if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
70 #include <opencv2/core/core.hpp>
71 #endif
72
73 // Exception
74 #include <visp3/core/vpException.h>
75 #include <visp3/core/vpMatrixException.h>
76
77 #include <cmath> // std::fabs
78 #include <limits> // numeric_limits
79
80 /*--------------------------------------------------------------------
81 LU Decomposition related functions
82 -------------------------------------------------------------------- */
83
84 /*!
85 Compute the inverse of a n-by-n matrix using the LU decomposition.
86
87 This function calls the first following function that is available:
88 - inverseByLULapack() if Lapack 3rd party is installed
89 - inverseByLUEigen3() if Eigen3 3rd party is installed
90 - inverseByLUOpenCV() if OpenCV 3rd party is installed
91
92 If none of these previous 3rd parties is installed, we use by default
93 inverseByLULapack() with a Lapack built-in version.
94
95 \return The inverse matrix.
96
97 Here an example:
98 \code
99 #include <visp3/core/vpMatrix.h>
100
101 int main()
102 {
103 vpMatrix A(4,4);
104
105 A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.;
106 A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.;
107 A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.;
108 A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.;
109
110 // Compute the inverse
111 vpMatrix A_1 = A.inverseByLU();
112
113 std::cout << "Inverse by LU ";
114 #if defined(VISP_HAVE_LAPACK)
115 std::cout << "(using Lapack)";
116 #elif defined(VISP_HAVE_EIGEN3)
117 std::cout << "(using Eigen3)";
118 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101)
119 std::cout << "(using OpenCV)";
120 #endif
121 std::cout << ": \n" << A_1 << std::endl;
122
123 std::cout << "A*A^-1: \n" << A * A_1 << std::endl;
124 }
125 \endcode
126
127 \sa inverseByLULapack(), inverseByLUEigen3(), inverseByLUOpenCV(),
128 pseudoInverse()
129 */
inverseByLU() const130 vpMatrix vpMatrix::inverseByLU() const
131 {
132 if (colNum == 1 && rowNum == 1) {
133 vpMatrix inv;
134 inv.resize(1, 1, false);
135 double d = det();
136 if (std::fabs(d) < std::numeric_limits<double>::epsilon()) {
137 throw(vpException(vpException::fatalError, "Cannot inverse matrix %d by %d by LU. Matrix determinant is 0.", rowNum, colNum));
138 }
139 inv[0][0] = 1. / d;
140 return inv;
141 }
142 else if (colNum == 2 && rowNum == 2) {
143 vpMatrix inv;
144 inv.resize(2, 2, false);
145 double d = det();
146 if (std::fabs(d) < std::numeric_limits<double>::epsilon()) {
147 throw(vpException(vpException::fatalError, "Cannot inverse matrix %d by %d by LU. Matrix determinant is 0.", rowNum, colNum));
148 }
149 d = 1. / d;
150 inv[1][1] = (*this)[0][0]*d;
151 inv[0][0] = (*this)[1][1]*d;
152 inv[0][1] = -(*this)[0][1]*d;
153 inv[1][0] = -(*this)[1][0]*d;
154 return inv;
155 }
156 else if (colNum == 3 && rowNum == 3) {
157 vpMatrix inv;
158 inv.resize(3, 3, false);
159 double d = det();
160 if (std::fabs(d) < std::numeric_limits<double>::epsilon()) {
161 throw(vpException(vpException::fatalError, "Cannot inverse matrix %d by %d by LU. Matrix determinant is 0.", rowNum, colNum));
162 }
163 d = 1. / d;
164 inv[0][0] = ((*this)[1][1] * (*this)[2][2] - (*this)[1][2] * (*this)[2][1]) * d;
165 inv[0][1] = ((*this)[0][2] * (*this)[2][1] - (*this)[0][1] * (*this)[2][2]) * d;
166 inv[0][2] = ((*this)[0][1] * (*this)[1][2] - (*this)[0][2] * (*this)[1][1]) * d;
167
168 inv[1][0] = ((*this)[1][2] * (*this)[2][0] - (*this)[1][0] * (*this)[2][2]) * d;
169 inv[1][1] = ((*this)[0][0] * (*this)[2][2] - (*this)[0][2] * (*this)[2][0]) * d;
170 inv[1][2] = ((*this)[0][2] * (*this)[1][0] - (*this)[0][0] * (*this)[1][2]) * d;
171
172 inv[2][0] = ((*this)[1][0] * (*this)[2][1] - (*this)[1][1] * (*this)[2][0]) * d;
173 inv[2][1] = ((*this)[0][1] * (*this)[2][0] - (*this)[0][0] * (*this)[2][1]) * d;
174 inv[2][2] = ((*this)[0][0] * (*this)[1][1] - (*this)[0][1] * (*this)[1][0]) * d;
175 return inv;
176 }
177 else {
178 #if defined(VISP_HAVE_LAPACK)
179 return inverseByLULapack();
180 #elif defined(VISP_HAVE_EIGEN3)
181 return inverseByLUEigen3();
182 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101)
183 return inverseByLUOpenCV();
184 #else
185 throw(vpException(vpException::fatalError, "Cannot inverse by LU. "
186 "Install Lapack, Eigen3 or OpenCV 3rd party"));
187 #endif
188 }
189 }
190
191 /*!
192 Compute the determinant of a square matrix using the LU decomposition.
193
194 This function calls the first following function that is available:
195 - detByLULapack() if Lapack 3rd party is installed
196 - detByLUEigen3() if Eigen3 3rd party is installed
197 - detByLUOpenCV() if OpenCV 3rd party is installed.
198
199 If none of these previous 3rd parties is installed, we use by default
200 detByLULapack() with a Lapack built-in version.
201
202 \return The determinant of the matrix if the matrix is square.
203
204 \code
205 #include <iostream>
206
207 #include <visp3/core/vpMatrix.h>
208
209 int main()
210 {
211 vpMatrix A(3,3);
212 A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.;
213 A[1][0] = 1/3.; A[1][1] = 1/4.; A[1][2] = 1/5.;
214 A[2][0] = 1/6.; A[2][1] = 1/7.; A[2][2] = 1/8.;
215 std::cout << "Initial matrix: \n" << A << std::endl;
216
217 // Compute the determinant
218 std:: cout << "Determinant by default method : " << A.det() << std::endl;
219 std:: cout << "Determinant by LU decomposition : " << A.detByLU() << std::endl;
220 }
221 \endcode
222 \sa detByLULapack(), detByLUEigen3(), detByLUOpenCV()
223 */
detByLU() const224 double vpMatrix::detByLU() const
225 {
226 if (rowNum == 1 && colNum == 1) {
227 return (*this)[0][0];
228 }
229 else if (rowNum == 2 && colNum == 2) {
230 return ((*this)[0][0] * (*this)[1][1] - (*this)[0][1] * (*this)[1][0]);
231 }
232 else if (rowNum == 3 && colNum == 3) {
233 return ((*this)[0][0] * ((*this)[1][1] * (*this)[2][2] - (*this)[1][2] * (*this)[2][1]) -
234 (*this)[0][1] * ((*this)[1][0] * (*this)[2][2] - (*this)[1][2] * (*this)[2][0]) +
235 (*this)[0][2] * ((*this)[1][0] * (*this)[2][1] - (*this)[1][1] * (*this)[2][0]));
236 } else {
237 #if defined(VISP_HAVE_LAPACK)
238 return detByLULapack();
239 #elif defined(VISP_HAVE_EIGEN3)
240 return detByLUEigen3();
241 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101)
242 return detByLUOpenCV();
243 #else
244 throw(vpException(vpException::fatalError, "Cannot compute matrix determinant. "
245 "Install Lapack, Eigen3 or OpenCV 3rd party"));
246 #endif
247 }
248 }
249
250 #if defined(VISP_HAVE_LAPACK)
251 /*!
252 Compute the inverse of a n-by-n matrix using the LU decomposition with
253 Lapack 3rd party.
254
255 \return The inverse matrix.
256
257 Here an example:
258 \code
259 #include <visp3/core/vpMatrix.h>
260
261 int main()
262 {
263 vpMatrix A(4,4);
264
265 A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.;
266 A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.;
267 A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.;
268 A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.;
269
270 // Compute the inverse
271 vpMatrix A_1; // A^-1
272 A_1 = A.inverseByLULapack();
273 std::cout << "Inverse by LU (Lapack): \n" << A_1 << std::endl;
274
275 std::cout << "A*A^-1: \n" << A * A_1 << std::endl;
276 }
277 \endcode
278
279 \sa inverseByLU(), inverseByLUEigen3(), inverseByLUOpenCV()
280 */
inverseByLULapack() const281 vpMatrix vpMatrix::inverseByLULapack() const
282 {
283 #if defined(VISP_HAVE_GSL)
284 {
285 if (rowNum != colNum) {
286 throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
287 }
288
289 gsl_matrix *A = gsl_matrix_alloc(rowNum, colNum);
290
291 // copy the input matrix to ensure the function doesn't modify its content
292 unsigned int tda = (unsigned int)A->tda;
293 for (unsigned int i = 0; i < rowNum; i++) {
294 unsigned int k = i * tda;
295 for (unsigned int j = 0; j < colNum; j++)
296 A->data[k + j] = (*this)[i][j];
297 }
298
299 vpMatrix Ainv(rowNum, colNum);
300
301 gsl_matrix inverse;
302 inverse.size1 = rowNum;
303 inverse.size2 = colNum;
304 inverse.tda = inverse.size2;
305 inverse.data = Ainv.data;
306 inverse.owner = 0;
307 inverse.block = 0;
308
309 gsl_permutation *p = gsl_permutation_alloc(rowNum);
310 int s;
311
312 // Do the LU decomposition on A and use it to solve the system
313 gsl_linalg_LU_decomp(A, p, &s);
314 gsl_linalg_LU_invert(A, p, &inverse);
315
316 gsl_permutation_free(p);
317 gsl_matrix_free(A);
318
319 return Ainv;
320 }
321 #else
322 {
323 if (rowNum != colNum) {
324 throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
325 }
326
327 integer dim = (integer)rowNum;
328 integer lda = dim;
329 integer info;
330 integer lwork = dim * dim;
331 integer *ipiv = new integer[dim + 1];
332 double *work = new double[lwork];
333
334 vpMatrix A = *this;
335
336 dgetrf_(&dim, &dim, A.data, &lda, &ipiv[1], &info);
337 if (info) {
338 delete[] ipiv;
339 delete[] work;
340 throw(vpException(vpException::fatalError, "Lapack LU decomposition failed; info=%d", info));
341 }
342
343 dgetri_(&dim, A.data, &dim, &ipiv[1], work, &lwork, &info);
344
345 delete[] ipiv;
346 delete[] work;
347
348 return A;
349 }
350 #endif
351 }
352
353 /*!
354 Compute the determinant of a square matrix using the LU decomposition with
355 Lapack 3rd party.
356
357 \return The determinant of the matrix if the matrix is square.
358
359 \code
360 #include <iostream>
361
362 #include <visp3/core/vpMatrix.h>
363
364 int main()
365 {
366 vpMatrix A(3,3);
367 A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.;
368 A[1][0] = 1/3.; A[1][1] = 1/4.; A[1][2] = 1/5.;
369 A[2][0] = 1/6.; A[2][1] = 1/7.; A[2][2] = 1/8.;
370 std::cout << "Initial matrix: \n" << A << std::endl;
371
372 // Compute the determinant
373 std:: cout << "Determinant by LU decomposition (Lapack): " << A.detByLULapack() << std::endl;
374 }
375 \endcode
376 \sa detByLU(), detByLUEigen3(), detByLUOpenCV()
377 */
detByLULapack() const378 double vpMatrix::detByLULapack() const
379 {
380 #if defined(VISP_HAVE_GSL)
381 {
382 double det = 0.;
383
384 if (rowNum != colNum) {
385 throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
386 rowNum, colNum));
387 }
388
389 gsl_matrix *A = gsl_matrix_alloc(rowNum, colNum);
390
391 // copy the input matrix to ensure the function doesn't modify its content
392 unsigned int tda = (unsigned int)A->tda;
393 for (unsigned int i = 0; i < rowNum; i++) {
394 unsigned int k = i * tda;
395 for (unsigned int j = 0; j < colNum; j++)
396 A->data[k + j] = (*this)[i][j];
397 }
398
399 gsl_permutation *p = gsl_permutation_alloc(rowNum);
400 int s;
401
402 // Do the LU decomposition on A and use it to solve the system
403 gsl_linalg_LU_decomp(A, p, &s);
404 det = gsl_linalg_LU_det(A, s);
405
406 gsl_permutation_free(p);
407 gsl_matrix_free(A);
408
409 return det;
410 }
411 #else
412 {
413 if (rowNum != colNum) {
414 throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
415 rowNum, colNum));
416 }
417
418 integer dim = (integer)rowNum;
419 integer lda = dim;
420 integer info;
421 integer *ipiv = new integer[dim + 1];
422
423 vpMatrix A = *this;
424
425 dgetrf_(&dim, &dim, A.data, &lda, &ipiv[1], &info);
426 if (info < 0) {
427 delete[] ipiv;
428 throw(vpException(vpException::fatalError, "Lapack LU decomposition failed; info=%d", -info));
429 }
430
431 double det = A[0][0];
432 for (unsigned int i = 1; i < rowNum; i++) {
433 det *= A[i][i];
434 }
435
436 double sign = 1.;
437 for (int i = 1; i <= dim; i++) {
438 if (ipiv[i] != i)
439 sign = -sign;
440 }
441
442 det *= sign;
443
444 delete[] ipiv;
445
446 return det;
447 }
448 #endif
449 }
450 #endif
451
452 #if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
453 /*!
454 Compute the inverse of a n-by-n matrix using the LU decomposition with
455 OpenCV 3rd party.
456
457 \return The inverse matrix.
458
459 Here an example:
460 \code
461 #include <visp3/core/vpMatrix.h>
462
463 int main()
464 {
465 vpMatrix A(4,4);
466
467 A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.;
468 A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.;
469 A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.;
470 A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.;
471
472 // Compute the inverse
473 vpMatrix A_1; // A^-1
474 A_1 = A.inverseByLUOpenCV();
475 std::cout << "Inverse by LU (OpenCV): \n" << A_1 << std::endl;
476
477 std::cout << "A*A^-1: \n" << A * A_1 << std::endl;
478 }
479 \endcode
480
481 \sa inverseByLU(), inverseByLUEigen3(), inverseByLULapack()
482 */
inverseByLUOpenCV() const483 vpMatrix vpMatrix::inverseByLUOpenCV() const
484 {
485 if (rowNum != colNum) {
486 throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
487 }
488
489 cv::Mat M(rowNum, colNum, CV_64F, this->data);
490
491 cv::Mat Minv = M.inv(cv::DECOMP_LU);
492
493 vpMatrix A(rowNum, colNum);
494 memcpy(A.data, Minv.data, (size_t)(8 * Minv.rows * Minv.cols));
495
496 return A;
497 }
498
499 /*!
500 Compute the determinant of a n-by-n matrix using the LU decomposition with
501 OpenCV 3rd party.
502
503 \return Determinant of the matrix.
504
505 \code
506 #include <iostream>
507
508 #include <visp3/core/vpMatrix.h>
509
510 int main()
511 {
512 vpMatrix A(3,3);
513 A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.;
514 A[1][0] = 1/3.; A[1][1] = 1/4.; A[1][2] = 1/5.;
515 A[2][0] = 1/6.; A[2][1] = 1/7.; A[2][2] = 1/8.;
516 std::cout << "Initial matrix: \n" << A << std::endl;
517
518 // Compute the determinant
519 std:: cout << "Determinant by LU decomposition (OpenCV): " << A.detByLUOpenCV() << std::endl;
520 }
521 \endcode
522 \sa detByLU(), detByLUEigen3(), detByLULapack()
523 */
detByLUOpenCV() const524 double vpMatrix::detByLUOpenCV() const
525 {
526 double det = 0.;
527
528 if (rowNum != colNum) {
529 throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
530 rowNum, colNum));
531 }
532
533 cv::Mat M(rowNum, colNum, CV_64F, this->data);
534 det = cv::determinant(M);
535
536 return (det);
537 }
538 #endif
539
540 #if defined(VISP_HAVE_EIGEN3)
541
542 /*!
543 Compute the inverse of a n-by-n matrix using the LU decomposition with
544 Eigen3 3rd party.
545
546 \return The inverse matrix.
547
548 Here an example:
549 \code
550 #include <visp3/core/vpMatrix.h>
551
552 int main()
553 {
554 vpMatrix A(4,4);
555
556 A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.;
557 A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.;
558 A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.;
559 A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.;
560
561 // Compute the inverse
562 vpMatrix A_1; // A^-1
563 A_1 = A.inverseByLUEigen3();
564 std::cout << "Inverse by LU (Eigen3): \n" << A_1 << std::endl;
565
566 std::cout << "A*A^-1: \n" << A * A_1 << std::endl;
567 }
568 \endcode
569
570 \sa inverseByLU(), inverseByLULapack(), inverseByLUOpenCV()
571 */
inverseByLUEigen3() const572 vpMatrix vpMatrix::inverseByLUEigen3() const
573 {
574 if (rowNum != colNum) {
575 throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
576 }
577 vpMatrix A(this->getRows(), this->getCols());
578
579 Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > M(this->data, this->getRows(),
580 this->getCols());
581 Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > A_(A.data, this->getRows(),
582 this->getCols());
583
584 A_ = M.inverse();
585
586 return A;
587 }
588
589 /*!
590 Compute the determinant of a square matrix using the LU decomposition with
591 Eigen3 3rd party.
592
593 \return The determinant of the matrix if the matrix is square.
594
595 \code
596 #include <iostream>
597
598 #include <visp3/core/vpMatrix.h>
599
600 int main()
601 {
602 vpMatrix A(3,3);
603 A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.;
604 A[1][0] = 1/3.; A[1][1] = 1/4.; A[1][2] = 1/5.;
605 A[2][0] = 1/6.; A[2][1] = 1/7.; A[2][2] = 1/8.;
606 std::cout << "Initial matrix: \n" << A << std::endl;
607
608 // Compute the determinant
609 std:: cout << "Determinant by LU decomposition (Eigen3): " << A.detByLUEigen3() << std::endl;
610 }
611 \endcode
612 \sa detByLU(), detByLUOpenCV(), detByLULapack()
613 */
detByLUEigen3() const614 double vpMatrix::detByLUEigen3() const
615 {
616 if (rowNum != colNum) {
617 throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
618 rowNum, colNum));
619 }
620
621 Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > M(this->data, this->getRows(),
622 this->getCols());
623
624 return M.determinant();
625 }
626 #endif
627