Visual Servoing Platform version 3.5.0
vpMatrix_lu.cpp
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>
56typedef MKL_INT integer;
57# else
58# ifdef VISP_HAVE_LAPACK_BUILT_IN
59typedef long int integer;
60# else
61typedef int integer;
62# endif
63extern "C" int dgetrf_(integer *m, integer *n, double *a, integer *lda, integer *ipiv, integer *info);
64extern "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
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
224double 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)
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
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)
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
494 memcpy(A.data, Minv.data, (size_t)(8 * Minv.rows * Minv.cols));
495
496 return A;
497}
498
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
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
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
unsigned int getCols() const
Definition: vpArray2D.h:279
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
unsigned int getRows() const
Definition: vpArray2D.h:289
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:137
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ fatalError
Fatal error.
Definition: vpException.h:96
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
vpMatrix inverseByLU() const
vpMatrix inverseByLUEigen3() const
vpMatrix inverseByLUOpenCV() const
double detByLUEigen3() const
double detByLUOpenCV() const
vpMatrix inverseByLULapack() const
double detByLU() const
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:6476
double detByLULapack() const