Visual Servoing Platform  version 3.6.1 under development (2024-04-18)
vpMatrix_lu.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
35 
36 #include <visp3/core/vpConfig.h>
37 
38 #include <visp3/core/vpColVector.h>
39 #include <visp3/core/vpMath.h>
40 #include <visp3/core/vpMatrix.h>
41 
42 #ifdef VISP_HAVE_EIGEN3
43 #include <Eigen/LU>
44 #endif
45 
46 #ifdef VISP_HAVE_LAPACK
47 #ifdef VISP_HAVE_GSL
48 #include <gsl/gsl_linalg.h>
49 #include <gsl/gsl_permutation.h>
50 #endif
51 #ifdef VISP_HAVE_MKL
52 #include <mkl.h>
53 typedef MKL_INT integer;
54 #else
55 #ifdef VISP_HAVE_LAPACK_BUILT_IN
56 typedef long int integer;
57 #else
58 typedef int integer;
59 #endif
60 extern "C" int dgetrf_(integer *m, integer *n, double *a, integer *lda, integer *ipiv, integer *info);
61 extern "C" void dgetri_(integer *n, double *a, integer *lda, integer *ipiv, double *work, integer *lwork,
62  integer *info);
63 #endif
64 #endif
65 
66 #if defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
67 #include <opencv2/core/core.hpp>
68 #endif
69 
70 // Exception
71 #include <visp3/core/vpException.h>
72 #include <visp3/core/vpMatrixException.h>
73 
74 #include <cmath> // std::fabs
75 #include <limits> // numeric_limits
76 
77 /*--------------------------------------------------------------------
78  LU Decomposition related functions
79 -------------------------------------------------------------------- */
80 
128 {
129  if ((colNum == 1) && (rowNum == 1)) {
130  vpMatrix inv;
131  inv.resize(1, 1, false);
132  double d = det();
133  if (std::fabs(d) < std::numeric_limits<double>::epsilon()) {
134  throw(vpException(vpException::fatalError, "Cannot inverse matrix %d by %d by LU. Matrix determinant is 0.",
135  rowNum, colNum));
136  }
137  inv[0][0] = 1. / d;
138  return inv;
139  }
140  else if ((colNum == 2) && (rowNum == 2)) {
141  vpMatrix inv;
142  inv.resize(2, 2, false);
143  double d = det();
144  if (std::fabs(d) < std::numeric_limits<double>::epsilon()) {
145  throw(vpException(vpException::fatalError, "Cannot inverse matrix %d by %d by LU. Matrix determinant is 0.",
146  rowNum, colNum));
147  }
148  d = 1. / d;
149  inv[1][1] = (*this)[0][0] * d;
150  inv[0][0] = (*this)[1][1] * d;
151  inv[0][1] = -(*this)[0][1] * d;
152  inv[1][0] = -(*this)[1][0] * d;
153  return inv;
154  }
155  else if ((colNum == 3) && (rowNum == 3)) {
156  vpMatrix inv;
157  inv.resize(3, 3, false);
158  double d = det();
159  if (std::fabs(d) < std::numeric_limits<double>::epsilon()) {
160  throw(vpException(vpException::fatalError, "Cannot inverse matrix %d by %d by LU. Matrix determinant is 0.",
161  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 defined(VISP_HAVE_OPENCV)
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 
224 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  }
237  else {
238 #if defined(VISP_HAVE_LAPACK)
239  return detByLULapack();
240 #elif defined(VISP_HAVE_EIGEN3)
241  return detByLUEigen3();
242 #elif defined(VISP_HAVE_OPENCV)
243  return detByLUOpenCV();
244 #else
245  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant. "
246  "Install Lapack, Eigen3 or OpenCV 3rd party"));
247 #endif
248  }
249 }
250 
251 #if defined(VISP_HAVE_LAPACK)
283 {
284 #if defined(VISP_HAVE_GSL)
285  {
286  if (rowNum != colNum) {
287  throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
288  }
289 
290  gsl_matrix *A = gsl_matrix_alloc(rowNum, colNum);
291 
292  // copy the input matrix to ensure the function doesn't modify its content
293  unsigned int tda = static_cast<unsigned int>(A->tda);
294  for (unsigned int i = 0; i < rowNum; ++i) {
295  unsigned int k = i * tda;
296  for (unsigned int j = 0; j < colNum; ++j)
297  A->data[k + j] = (*this)[i][j];
298  }
299 
300  vpMatrix Ainv(rowNum, colNum);
301 
302  gsl_matrix inverse;
303  inverse.size1 = rowNum;
304  inverse.size2 = colNum;
305  inverse.tda = inverse.size2;
306  inverse.data = Ainv.data;
307  inverse.owner = 0;
308  inverse.block = 0;
309 
310  gsl_permutation *p = gsl_permutation_alloc(rowNum);
311  int s;
312 
313  // Do the LU decomposition on A and use it to solve the system
314  gsl_linalg_LU_decomp(A, p, &s);
315  gsl_linalg_LU_invert(A, p, &inverse);
316 
317  gsl_permutation_free(p);
318  gsl_matrix_free(A);
319 
320  return Ainv;
321  }
322 #else
323  {
324  if (rowNum != colNum) {
325  throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
326  }
327 
328  integer dim = (integer)rowNum;
329  integer lda = dim;
330  integer info;
331  integer lwork = dim * dim;
332  integer *ipiv = new integer[dim + 1];
333  double *work = new double[lwork];
334 
335  vpMatrix A = *this;
336 
337  dgetrf_(&dim, &dim, A.data, &lda, &ipiv[1], &info);
338  if (info) {
339  delete[] ipiv;
340  delete[] work;
341  throw(vpException(vpException::fatalError, "Lapack LU decomposition failed; info=%d", info));
342  }
343 
344  dgetri_(&dim, A.data, &dim, &ipiv[1], work, &lwork, &info);
345 
346  delete[] ipiv;
347  delete[] work;
348 
349  return A;
350  }
351 #endif
352 }
353 
380 {
381 #if defined(VISP_HAVE_GSL)
382  {
383  double det = 0.;
384 
385  if (rowNum != colNum) {
386  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
387  rowNum, colNum));
388  }
389 
390  gsl_matrix *A = gsl_matrix_alloc(rowNum, colNum);
391 
392  // copy the input matrix to ensure the function doesn't modify its content
393  unsigned int tda = (unsigned int)A->tda;
394  for (unsigned int i = 0; i < rowNum; i++) {
395  unsigned int k = i * tda;
396  for (unsigned int j = 0; j < colNum; j++)
397  A->data[k + j] = (*this)[i][j];
398  }
399 
400  gsl_permutation *p = gsl_permutation_alloc(rowNum);
401  int s;
402 
403  // Do the LU decomposition on A and use it to solve the system
404  gsl_linalg_LU_decomp(A, p, &s);
405  det = gsl_linalg_LU_det(A, s);
406 
407  gsl_permutation_free(p);
408  gsl_matrix_free(A);
409 
410  return det;
411  }
412 #else
413  {
414  if (rowNum != colNum) {
415  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
416  rowNum, colNum));
417  }
418 
419  integer dim = (integer)rowNum;
420  integer lda = dim;
421  integer info;
422  integer *ipiv = new integer[dim + 1];
423 
424  vpMatrix A = *this;
425 
426  dgetrf_(&dim, &dim, A.data, &lda, &ipiv[1], &info);
427  if (info < 0) {
428  delete[] ipiv;
429  throw(vpException(vpException::fatalError, "Lapack LU decomposition failed; info=%d", -info));
430  }
431 
432  double det = A[0][0];
433  for (unsigned int i = 1; i < rowNum; ++i) {
434  det *= A[i][i];
435  }
436 
437  double sign = 1.;
438  for (int i = 1; i <= dim; ++i) {
439  if (ipiv[i] != i)
440  sign = -sign;
441  }
442 
443  det *= sign;
444 
445  delete[] ipiv;
446 
447  return det;
448  }
449 #endif
450 }
451 #endif
452 
453 #if defined(VISP_HAVE_OPENCV)
485 {
486  if (rowNum != colNum) {
487  throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
488  }
489 
490  cv::Mat M(rowNum, colNum, CV_64F, this->data);
491 
492  cv::Mat Minv = M.inv(cv::DECOMP_LU);
493 
494  vpMatrix A(rowNum, colNum);
495  memcpy(A.data, Minv.data, (size_t)(8 * Minv.rows * Minv.cols));
496 
497  return A;
498 }
499 
526 {
527  double det = 0.;
528 
529  if (rowNum != colNum) {
530  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
531  rowNum, colNum));
532  }
533 
534  cv::Mat M(rowNum, colNum, CV_64F, this->data);
535  det = cv::determinant(M);
536 
537  return (det);
538 }
539 #endif
540 
541 #if defined(VISP_HAVE_EIGEN3)
542 
574 {
575  if (rowNum != colNum) {
576  throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
577  }
578  vpMatrix A(this->getRows(), this->getCols());
579 
580  Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > M(this->data, this->getRows(),
581  this->getCols());
582  Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > A_(A.data, this->getRows(),
583  this->getCols());
584 
585  A_ = M.inverse();
586 
587  return A;
588 }
589 
616 {
617  if (rowNum != colNum) {
618  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
619  rowNum, colNum));
620  }
621 
622  Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > M(this->data, this->getRows(),
623  this->getCols());
624 
625  return M.determinant();
626 }
627 #endif
unsigned int getCols() const
Definition: vpArray2D.h:314
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:139
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:339
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:129
unsigned int getRows() const
Definition: vpArray2D.h:324
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:131
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ fatalError
Fatal error.
Definition: vpException.h:84
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
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:6275
double detByLULapack() const