Visual Servoing Platform  version 3.6.1 under development (2024-03-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  } else if (colNum == 2 && rowNum == 2) {
140  vpMatrix inv;
141  inv.resize(2, 2, false);
142  double d = det();
143  if (std::fabs(d) < std::numeric_limits<double>::epsilon()) {
144  throw(vpException(vpException::fatalError, "Cannot inverse matrix %d by %d by LU. Matrix determinant is 0.",
145  rowNum, colNum));
146  }
147  d = 1. / d;
148  inv[1][1] = (*this)[0][0] * d;
149  inv[0][0] = (*this)[1][1] * d;
150  inv[0][1] = -(*this)[0][1] * d;
151  inv[1][0] = -(*this)[1][0] * d;
152  return inv;
153  } else if (colNum == 3 && rowNum == 3) {
154  vpMatrix inv;
155  inv.resize(3, 3, false);
156  double d = det();
157  if (std::fabs(d) < std::numeric_limits<double>::epsilon()) {
158  throw(vpException(vpException::fatalError, "Cannot inverse matrix %d by %d by LU. Matrix determinant is 0.",
159  rowNum, colNum));
160  }
161  d = 1. / d;
162  inv[0][0] = ((*this)[1][1] * (*this)[2][2] - (*this)[1][2] * (*this)[2][1]) * d;
163  inv[0][1] = ((*this)[0][2] * (*this)[2][1] - (*this)[0][1] * (*this)[2][2]) * d;
164  inv[0][2] = ((*this)[0][1] * (*this)[1][2] - (*this)[0][2] * (*this)[1][1]) * d;
165 
166  inv[1][0] = ((*this)[1][2] * (*this)[2][0] - (*this)[1][0] * (*this)[2][2]) * d;
167  inv[1][1] = ((*this)[0][0] * (*this)[2][2] - (*this)[0][2] * (*this)[2][0]) * d;
168  inv[1][2] = ((*this)[0][2] * (*this)[1][0] - (*this)[0][0] * (*this)[1][2]) * d;
169 
170  inv[2][0] = ((*this)[1][0] * (*this)[2][1] - (*this)[1][1] * (*this)[2][0]) * d;
171  inv[2][1] = ((*this)[0][1] * (*this)[2][0] - (*this)[0][0] * (*this)[2][1]) * d;
172  inv[2][2] = ((*this)[0][0] * (*this)[1][1] - (*this)[0][1] * (*this)[1][0]) * d;
173  return inv;
174  } else {
175 #if defined(VISP_HAVE_LAPACK)
176  return inverseByLULapack();
177 #elif defined(VISP_HAVE_EIGEN3)
178  return inverseByLUEigen3();
179 #elif defined(VISP_HAVE_OPENCV)
180  return inverseByLUOpenCV();
181 #else
182  throw(vpException(vpException::fatalError, "Cannot inverse by LU. "
183  "Install Lapack, Eigen3 or OpenCV 3rd party"));
184 #endif
185  }
186 }
187 
221 double vpMatrix::detByLU() const
222 {
223  if (rowNum == 1 && colNum == 1) {
224  return (*this)[0][0];
225  } else if (rowNum == 2 && colNum == 2) {
226  return ((*this)[0][0] * (*this)[1][1] - (*this)[0][1] * (*this)[1][0]);
227  } else if (rowNum == 3 && colNum == 3) {
228  return ((*this)[0][0] * ((*this)[1][1] * (*this)[2][2] - (*this)[1][2] * (*this)[2][1]) -
229  (*this)[0][1] * ((*this)[1][0] * (*this)[2][2] - (*this)[1][2] * (*this)[2][0]) +
230  (*this)[0][2] * ((*this)[1][0] * (*this)[2][1] - (*this)[1][1] * (*this)[2][0]));
231  } else {
232 #if defined(VISP_HAVE_LAPACK)
233  return detByLULapack();
234 #elif defined(VISP_HAVE_EIGEN3)
235  return detByLUEigen3();
236 #elif defined(VISP_HAVE_OPENCV)
237  return detByLUOpenCV();
238 #else
239  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant. "
240  "Install Lapack, Eigen3 or OpenCV 3rd party"));
241 #endif
242  }
243 }
244 
245 #if defined(VISP_HAVE_LAPACK)
277 {
278 #if defined(VISP_HAVE_GSL)
279  {
280  if (rowNum != colNum) {
281  throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
282  }
283 
284  gsl_matrix *A = gsl_matrix_alloc(rowNum, colNum);
285 
286  // copy the input matrix to ensure the function doesn't modify its content
287  unsigned int tda = (unsigned int)A->tda;
288  for (unsigned int i = 0; i < rowNum; i++) {
289  unsigned int k = i * tda;
290  for (unsigned int j = 0; j < colNum; j++)
291  A->data[k + j] = (*this)[i][j];
292  }
293 
294  vpMatrix Ainv(rowNum, colNum);
295 
296  gsl_matrix inverse;
297  inverse.size1 = rowNum;
298  inverse.size2 = colNum;
299  inverse.tda = inverse.size2;
300  inverse.data = Ainv.data;
301  inverse.owner = 0;
302  inverse.block = 0;
303 
304  gsl_permutation *p = gsl_permutation_alloc(rowNum);
305  int s;
306 
307  // Do the LU decomposition on A and use it to solve the system
308  gsl_linalg_LU_decomp(A, p, &s);
309  gsl_linalg_LU_invert(A, p, &inverse);
310 
311  gsl_permutation_free(p);
312  gsl_matrix_free(A);
313 
314  return Ainv;
315  }
316 #else
317  {
318  if (rowNum != colNum) {
319  throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
320  }
321 
322  integer dim = (integer)rowNum;
323  integer lda = dim;
324  integer info;
325  integer lwork = dim * dim;
326  integer *ipiv = new integer[dim + 1];
327  double *work = new double[lwork];
328 
329  vpMatrix A = *this;
330 
331  dgetrf_(&dim, &dim, A.data, &lda, &ipiv[1], &info);
332  if (info) {
333  delete[] ipiv;
334  delete[] work;
335  throw(vpException(vpException::fatalError, "Lapack LU decomposition failed; info=%d", info));
336  }
337 
338  dgetri_(&dim, A.data, &dim, &ipiv[1], work, &lwork, &info);
339 
340  delete[] ipiv;
341  delete[] work;
342 
343  return A;
344  }
345 #endif
346 }
347 
374 {
375 #if defined(VISP_HAVE_GSL)
376  {
377  double det = 0.;
378 
379  if (rowNum != colNum) {
380  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
381  rowNum, colNum));
382  }
383 
384  gsl_matrix *A = gsl_matrix_alloc(rowNum, colNum);
385 
386  // copy the input matrix to ensure the function doesn't modify its content
387  unsigned int tda = (unsigned int)A->tda;
388  for (unsigned int i = 0; i < rowNum; i++) {
389  unsigned int k = i * tda;
390  for (unsigned int j = 0; j < colNum; j++)
391  A->data[k + j] = (*this)[i][j];
392  }
393 
394  gsl_permutation *p = gsl_permutation_alloc(rowNum);
395  int s;
396 
397  // Do the LU decomposition on A and use it to solve the system
398  gsl_linalg_LU_decomp(A, p, &s);
399  det = gsl_linalg_LU_det(A, s);
400 
401  gsl_permutation_free(p);
402  gsl_matrix_free(A);
403 
404  return det;
405  }
406 #else
407  {
408  if (rowNum != colNum) {
409  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
410  rowNum, colNum));
411  }
412 
413  integer dim = (integer)rowNum;
414  integer lda = dim;
415  integer info;
416  integer *ipiv = new integer[dim + 1];
417 
418  vpMatrix A = *this;
419 
420  dgetrf_(&dim, &dim, A.data, &lda, &ipiv[1], &info);
421  if (info < 0) {
422  delete[] ipiv;
423  throw(vpException(vpException::fatalError, "Lapack LU decomposition failed; info=%d", -info));
424  }
425 
426  double det = A[0][0];
427  for (unsigned int i = 1; i < rowNum; i++) {
428  det *= A[i][i];
429  }
430 
431  double sign = 1.;
432  for (int i = 1; i <= dim; i++) {
433  if (ipiv[i] != i)
434  sign = -sign;
435  }
436 
437  det *= sign;
438 
439  delete[] ipiv;
440 
441  return det;
442  }
443 #endif
444 }
445 #endif
446 
447 #if defined(VISP_HAVE_OPENCV)
479 {
480  if (rowNum != colNum) {
481  throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
482  }
483 
484  cv::Mat M(rowNum, colNum, CV_64F, this->data);
485 
486  cv::Mat Minv = M.inv(cv::DECOMP_LU);
487 
488  vpMatrix A(rowNum, colNum);
489  memcpy(A.data, Minv.data, (size_t)(8 * Minv.rows * Minv.cols));
490 
491  return A;
492 }
493 
520 {
521  double det = 0.;
522 
523  if (rowNum != colNum) {
524  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
525  rowNum, colNum));
526  }
527 
528  cv::Mat M(rowNum, colNum, CV_64F, this->data);
529  det = cv::determinant(M);
530 
531  return (det);
532 }
533 #endif
534 
535 #if defined(VISP_HAVE_EIGEN3)
536 
568 {
569  if (rowNum != colNum) {
570  throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
571  }
572  vpMatrix A(this->getRows(), this->getCols());
573 
574  Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > M(this->data, this->getRows(),
575  this->getCols());
576  Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > A_(A.data, this->getRows(),
577  this->getCols());
578 
579  A_ = M.inverse();
580 
581  return A;
582 }
583 
610 {
611  if (rowNum != colNum) {
612  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
613  rowNum, colNum));
614  }
615 
616  Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > M(this->data, this->getRows(),
617  this->getCols());
618 
619  return M.determinant();
620 }
621 #endif
unsigned int getCols() const
Definition: vpArray2D.h:274
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:138
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:299
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:128
unsigned int getRows() const
Definition: vpArray2D.h:284
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:130
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:6199
double detByLULapack() const