Visual Servoing Platform  version 3.6.1 under development (2024-07-18)
vpMatrix_lu.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Matrix LU decomposition.
32  */
33 
34 #include <visp3/core/vpConfig.h>
35 
36 #include <visp3/core/vpColVector.h>
37 #include <visp3/core/vpMath.h>
38 #include <visp3/core/vpMatrix.h>
39 
40 #ifdef VISP_HAVE_EIGEN3
41 #include <Eigen/LU>
42 #endif
43 
44 #ifdef VISP_HAVE_LAPACK
45 #ifdef VISP_HAVE_GSL
46 #include <gsl/gsl_linalg.h>
47 #include <gsl/gsl_permutation.h>
48 #endif
49 #ifdef VISP_HAVE_MKL
50 #include <mkl.h>
51 typedef MKL_INT integer;
52 #else
53 #ifdef VISP_HAVE_LAPACK_BUILT_IN
54 typedef long int integer;
55 #else
56 typedef int integer;
57 #endif
58 extern "C" int dgetrf_(integer *m, integer *n, double *a, integer *lda, integer *ipiv, integer *info);
59 extern "C" void dgetri_(integer *n, double *a, integer *lda, integer *ipiv, double *work, integer *lwork,
60  integer *info);
61 #endif
62 #endif
63 
64 #if defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
65 #include <opencv2/core/core.hpp>
66 #endif
67 
68 // Exception
69 #include <visp3/core/vpException.h>
70 #include <visp3/core/vpMatrixException.h>
71 
72 #include <cmath> // std::fabs
73 #include <limits> // numeric_limits
74 
76 /*--------------------------------------------------------------------
77  LU Decomposition related functions
78 -------------------------------------------------------------------- */
79 
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.",
138  rowNum, colNum));
139  }
140  inv[0][0] = 1. / d;
141  return inv;
142  }
143  else if ((colNum == 2) && (rowNum == 2)) {
144  vpMatrix inv;
145  inv.resize(2, 2, false);
146  double d = det();
147  if (std::fabs(d) < std::numeric_limits<double>::epsilon()) {
148  throw(vpException(vpException::fatalError, "Cannot inverse matrix %d by %d by LU. Matrix determinant is 0.",
149  rowNum, colNum));
150  }
151  d = 1. / d;
152  inv[1][1] = (*this)[0][0] * d;
153  inv[0][0] = (*this)[1][1] * d;
154  inv[0][1] = -(*this)[0][1] * d;
155  inv[1][0] = -(*this)[1][0] * d;
156  return inv;
157  }
158  else if ((colNum == 3) && (rowNum == 3)) {
159  vpMatrix inv;
160  inv.resize(3, 3, false);
161  double d = det();
162  if (std::fabs(d) < std::numeric_limits<double>::epsilon()) {
163  throw(vpException(vpException::fatalError, "Cannot inverse matrix %d by %d by LU. Matrix determinant is 0.",
164  rowNum, colNum));
165  }
166  d = 1. / d;
167  const unsigned int index_0 = 0;
168  const unsigned int index_1 = 1;
169  const unsigned int index_2 = 2;
170  inv[index_0][index_0] = (((*this)[index_1][index_1] * (*this)[index_2][index_2]) - ((*this)[index_1][index_2] * (*this)[index_2][index_1])) * d;
171  inv[index_0][index_1] = (((*this)[index_0][index_2] * (*this)[index_2][index_1]) - ((*this)[index_0][index_1] * (*this)[index_2][index_2])) * d;
172  inv[index_0][index_2] = (((*this)[index_0][index_1] * (*this)[index_1][index_2]) - ((*this)[index_0][index_2] * (*this)[index_1][index_1])) * d;
173 
174  inv[index_1][index_0] = (((*this)[index_1][index_2] * (*this)[index_2][index_0]) - ((*this)[index_1][index_0] * (*this)[index_2][index_2])) * d;
175  inv[index_1][index_1] = (((*this)[index_0][index_0] * (*this)[index_2][index_2]) - ((*this)[index_0][index_2] * (*this)[index_2][index_0])) * d;
176  inv[index_1][index_2] = (((*this)[index_0][index_2] * (*this)[index_1][index_0]) - ((*this)[index_0][index_0] * (*this)[index_1][index_2])) * d;
177 
178  inv[index_2][index_0] = (((*this)[index_1][index_0] * (*this)[index_2][index_1]) - ((*this)[index_1][index_1] * (*this)[index_2][index_0])) * d;
179  inv[index_2][index_1] = (((*this)[index_0][index_1] * (*this)[index_2][index_0]) - ((*this)[index_0][index_0] * (*this)[index_2][index_1])) * d;
180  inv[index_2][index_2] = (((*this)[index_0][index_0] * (*this)[index_1][index_1]) - ((*this)[index_0][index_1] * (*this)[index_1][index_0])) * d;
181  return inv;
182  }
183  else {
184 #if defined(VISP_HAVE_LAPACK)
185  return inverseByLULapack();
186 #elif defined(VISP_HAVE_EIGEN3)
187  return inverseByLUEigen3();
188 #elif defined(VISP_HAVE_OPENCV)
189  return inverseByLUOpenCV();
190 #else
191  throw(vpException(vpException::fatalError, "Cannot inverse by LU. "
192  "Install Lapack, Eigen3 or OpenCV 3rd party"));
193 #endif
194  }
195 }
196 
234 double vpMatrix::detByLU() const
235 {
236  if ((rowNum == 1) && (colNum == 1)) {
237  return (*this)[0][0];
238  }
239  else if ((rowNum == 2) && (colNum == 2)) {
240  return (((*this)[0][0] * (*this)[1][1]) - ((*this)[0][1] * (*this)[1][0]));
241  }
242  else if ((rowNum == 3) && (colNum == 3)) {
243  const unsigned int index_0 = 0;
244  const unsigned int index_1 = 1;
245  const unsigned int index_2 = 2;
246  return ((((*this)[index_0][index_0] * (((*this)[index_1][index_1] * (*this)[index_2][index_2]) - ((*this)[index_1][index_2] * (*this)[index_2][index_1]))) -
247  ((*this)[index_0][index_1] * (((*this)[index_1][index_0] * (*this)[index_2][index_2]) - ((*this)[index_1][index_2] * (*this)[index_2][index_0])))) +
248  ((*this)[index_0][index_2] * (((*this)[index_1][index_0] * (*this)[index_2][index_1]) - ((*this)[index_1][index_1] * (*this)[index_2][index_0]))));
249  }
250  else {
251 #if defined(VISP_HAVE_LAPACK)
252  return detByLULapack();
253 #elif defined(VISP_HAVE_EIGEN3)
254  return detByLUEigen3();
255 #elif defined(VISP_HAVE_OPENCV)
256  return detByLUOpenCV();
257 #else
258  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant. "
259  "Install Lapack, Eigen3 or OpenCV 3rd party"));
260 #endif
261  }
262 }
263 
264 #if defined(VISP_HAVE_LAPACK)
300 {
301 #if defined(VISP_HAVE_GSL)
302  {
303  if (rowNum != colNum) {
304  throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
305  }
306 
307  gsl_matrix *A = gsl_matrix_alloc(rowNum, colNum);
308 
309  // copy the input matrix to ensure the function doesn't modify its content
310  unsigned int tda = static_cast<unsigned int>(A->tda);
311  for (unsigned int i = 0; i < rowNum; ++i) {
312  unsigned int k = i * tda;
313  for (unsigned int j = 0; j < colNum; ++j)
314  A->data[k + j] = (*this)[i][j];
315  }
316 
317  vpMatrix Ainv(rowNum, colNum);
318 
319  gsl_matrix inverse;
320  inverse.size1 = rowNum;
321  inverse.size2 = colNum;
322  inverse.tda = inverse.size2;
323  inverse.data = Ainv.data;
324  inverse.owner = 0;
325  inverse.block = 0;
326 
327  gsl_permutation *p = gsl_permutation_alloc(rowNum);
328  int s;
329 
330  // Do the LU decomposition on A and use it to solve the system
331  gsl_linalg_LU_decomp(A, p, &s);
332  gsl_linalg_LU_invert(A, p, &inverse);
333 
334  gsl_permutation_free(p);
335  gsl_matrix_free(A);
336 
337  return Ainv;
338  }
339 #else
340  {
341  if (rowNum != colNum) {
342  throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
343  }
344 
345  integer dim = (integer)rowNum;
346  integer lda = dim;
347  integer info;
348  integer lwork = dim * dim;
349  integer *ipiv = new integer[dim + 1];
350  double *work = new double[lwork];
351 
352  vpMatrix A = *this;
353 
354  dgetrf_(&dim, &dim, A.data, &lda, &ipiv[1], &info);
355  if (info) {
356  delete[] ipiv;
357  delete[] work;
358  throw(vpException(vpException::fatalError, "Lapack LU decomposition failed; info=%d", info));
359  }
360 
361  dgetri_(&dim, A.data, &dim, &ipiv[1], work, &lwork, &info);
362 
363  delete[] ipiv;
364  delete[] work;
365 
366  return A;
367  }
368 #endif
369 }
370 
401 {
402 #if defined(VISP_HAVE_GSL)
403  {
404  double det = 0.;
405 
406  if (rowNum != colNum) {
407  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
408  rowNum, colNum));
409  }
410 
411  gsl_matrix *A = gsl_matrix_alloc(rowNum, colNum);
412 
413  // copy the input matrix to ensure the function doesn't modify its content
414  unsigned int tda = (unsigned int)A->tda;
415  for (unsigned int i = 0; i < rowNum; i++) {
416  unsigned int k = i * tda;
417  for (unsigned int j = 0; j < colNum; j++)
418  A->data[k + j] = (*this)[i][j];
419  }
420 
421  gsl_permutation *p = gsl_permutation_alloc(rowNum);
422  int s;
423 
424  // Do the LU decomposition on A and use it to solve the system
425  gsl_linalg_LU_decomp(A, p, &s);
426  det = gsl_linalg_LU_det(A, s);
427 
428  gsl_permutation_free(p);
429  gsl_matrix_free(A);
430 
431  return det;
432  }
433 #else
434  {
435  if (rowNum != colNum) {
436  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
437  rowNum, colNum));
438  }
439 
440  integer dim = (integer)rowNum;
441  integer lda = dim;
442  integer info;
443  integer *ipiv = new integer[dim + 1];
444 
445  vpMatrix A = *this;
446 
447  dgetrf_(&dim, &dim, A.data, &lda, &ipiv[1], &info);
448  if (info < 0) {
449  delete[] ipiv;
450  throw(vpException(vpException::fatalError, "Lapack LU decomposition failed; info=%d", -info));
451  }
452 
453  double det = A[0][0];
454  for (unsigned int i = 1; i < rowNum; ++i) {
455  det *= A[i][i];
456  }
457 
458  double sign = 1.;
459  for (int i = 1; i <= dim; ++i) {
460  if (ipiv[i] != i)
461  sign = -sign;
462  }
463 
464  det *= sign;
465 
466  delete[] ipiv;
467 
468  return det;
469  }
470 #endif
471 }
472 #endif
473 
474 #if defined(VISP_HAVE_OPENCV)
510 {
511  if (rowNum != colNum) {
512  throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
513  }
514 
515  cv::Mat M(rowNum, colNum, CV_64F, this->data);
516 
517  cv::Mat Minv = M.inv(cv::DECOMP_LU);
518 
519  vpMatrix A(rowNum, colNum);
520  memcpy(A.data, Minv.data, (size_t)(8 * Minv.rows * Minv.cols));
521 
522  return A;
523 }
524 
555 {
556  double det = 0.;
557 
558  if (rowNum != colNum) {
559  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
560  rowNum, colNum));
561  }
562 
563  cv::Mat M(rowNum, colNum, CV_64F, this->data);
564  det = cv::determinant(M);
565 
566  return (det);
567 }
568 #endif
569 
570 #if defined(VISP_HAVE_EIGEN3)
571 
607 {
608  if (rowNum != colNum) {
609  throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
610  }
611  vpMatrix A(this->getRows(), this->getCols());
612 
613  Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > M(this->data, this->getRows(),
614  this->getCols());
615  Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > A_(A.data, this->getRows(),
616  this->getCols());
617 
618  A_ = M.inverse();
619 
620  return A;
621 }
622 
653 {
654  if (rowNum != colNum) {
655  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
656  rowNum, colNum));
657  }
658 
659  Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > M(this->data, this->getRows(),
660  this->getCols());
661 
662  return M.determinant();
663 }
664 #endif
665 END_VISP_NAMESPACE
unsigned int getCols() const
Definition: vpArray2D.h:337
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:148
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:362
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:1098
unsigned int getRows() const
Definition: vpArray2D.h:347
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:1100
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ fatalError
Fatal error.
Definition: vpException.h:72
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
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:1641
double detByLULapack() const