Visual Servoing Platform  version 3.5.1 under development (2022-12-04)
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>
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 
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  } 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.",
148  rowNum, colNum));
149  }
150  d = 1. / d;
151  inv[1][1] = (*this)[0][0] * d;
152  inv[0][0] = (*this)[1][1] * d;
153  inv[0][1] = -(*this)[0][1] * d;
154  inv[1][0] = -(*this)[1][0] * d;
155  return inv;
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.",
162  rowNum, colNum));
163  }
164  d = 1. / d;
165  inv[0][0] = ((*this)[1][1] * (*this)[2][2] - (*this)[1][2] * (*this)[2][1]) * d;
166  inv[0][1] = ((*this)[0][2] * (*this)[2][1] - (*this)[0][1] * (*this)[2][2]) * d;
167  inv[0][2] = ((*this)[0][1] * (*this)[1][2] - (*this)[0][2] * (*this)[1][1]) * d;
168 
169  inv[1][0] = ((*this)[1][2] * (*this)[2][0] - (*this)[1][0] * (*this)[2][2]) * d;
170  inv[1][1] = ((*this)[0][0] * (*this)[2][2] - (*this)[0][2] * (*this)[2][0]) * d;
171  inv[1][2] = ((*this)[0][2] * (*this)[1][0] - (*this)[0][0] * (*this)[1][2]) * d;
172 
173  inv[2][0] = ((*this)[1][0] * (*this)[2][1] - (*this)[1][1] * (*this)[2][0]) * d;
174  inv[2][1] = ((*this)[0][1] * (*this)[2][0] - (*this)[0][0] * (*this)[2][1]) * d;
175  inv[2][2] = ((*this)[0][0] * (*this)[1][1] - (*this)[0][1] * (*this)[1][0]) * d;
176  return inv;
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 
224 double vpMatrix::detByLU() const
225 {
226  if (rowNum == 1 && colNum == 1) {
227  return (*this)[0][0];
228  } else if (rowNum == 2 && colNum == 2) {
229  return ((*this)[0][0] * (*this)[1][1] - (*this)[0][1] * (*this)[1][0]);
230  } else if (rowNum == 3 && colNum == 3) {
231  return ((*this)[0][0] * ((*this)[1][1] * (*this)[2][2] - (*this)[1][2] * (*this)[2][1]) -
232  (*this)[0][1] * ((*this)[1][0] * (*this)[2][2] - (*this)[1][2] * (*this)[2][0]) +
233  (*this)[0][2] * ((*this)[1][0] * (*this)[2][1] - (*this)[1][1] * (*this)[2][0]));
234  } else {
235 #if defined(VISP_HAVE_LAPACK)
236  return detByLULapack();
237 #elif defined(VISP_HAVE_EIGEN3)
238  return detByLUEigen3();
239 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101)
240  return detByLUOpenCV();
241 #else
242  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant. "
243  "Install Lapack, Eigen3 or OpenCV 3rd party"));
244 #endif
245  }
246 }
247 
248 #if defined(VISP_HAVE_LAPACK)
280 {
281 #if defined(VISP_HAVE_GSL)
282  {
283  if (rowNum != colNum) {
284  throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
285  }
286 
287  gsl_matrix *A = gsl_matrix_alloc(rowNum, colNum);
288 
289  // copy the input matrix to ensure the function doesn't modify its content
290  unsigned int tda = (unsigned int)A->tda;
291  for (unsigned int i = 0; i < rowNum; i++) {
292  unsigned int k = i * tda;
293  for (unsigned int j = 0; j < colNum; j++)
294  A->data[k + j] = (*this)[i][j];
295  }
296 
297  vpMatrix Ainv(rowNum, colNum);
298 
299  gsl_matrix inverse;
300  inverse.size1 = rowNum;
301  inverse.size2 = colNum;
302  inverse.tda = inverse.size2;
303  inverse.data = Ainv.data;
304  inverse.owner = 0;
305  inverse.block = 0;
306 
307  gsl_permutation *p = gsl_permutation_alloc(rowNum);
308  int s;
309 
310  // Do the LU decomposition on A and use it to solve the system
311  gsl_linalg_LU_decomp(A, p, &s);
312  gsl_linalg_LU_invert(A, p, &inverse);
313 
314  gsl_permutation_free(p);
315  gsl_matrix_free(A);
316 
317  return Ainv;
318  }
319 #else
320  {
321  if (rowNum != colNum) {
322  throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
323  }
324 
325  integer dim = (integer)rowNum;
326  integer lda = dim;
327  integer info;
328  integer lwork = dim * dim;
329  integer *ipiv = new integer[dim + 1];
330  double *work = new double[lwork];
331 
332  vpMatrix A = *this;
333 
334  dgetrf_(&dim, &dim, A.data, &lda, &ipiv[1], &info);
335  if (info) {
336  delete[] ipiv;
337  delete[] work;
338  throw(vpException(vpException::fatalError, "Lapack LU decomposition failed; info=%d", info));
339  }
340 
341  dgetri_(&dim, A.data, &dim, &ipiv[1], work, &lwork, &info);
342 
343  delete[] ipiv;
344  delete[] work;
345 
346  return A;
347  }
348 #endif
349 }
350 
377 {
378 #if defined(VISP_HAVE_GSL)
379  {
380  double det = 0.;
381 
382  if (rowNum != colNum) {
383  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
384  rowNum, colNum));
385  }
386 
387  gsl_matrix *A = gsl_matrix_alloc(rowNum, colNum);
388 
389  // copy the input matrix to ensure the function doesn't modify its content
390  unsigned int tda = (unsigned int)A->tda;
391  for (unsigned int i = 0; i < rowNum; i++) {
392  unsigned int k = i * tda;
393  for (unsigned int j = 0; j < colNum; j++)
394  A->data[k + j] = (*this)[i][j];
395  }
396 
397  gsl_permutation *p = gsl_permutation_alloc(rowNum);
398  int s;
399 
400  // Do the LU decomposition on A and use it to solve the system
401  gsl_linalg_LU_decomp(A, p, &s);
402  det = gsl_linalg_LU_det(A, s);
403 
404  gsl_permutation_free(p);
405  gsl_matrix_free(A);
406 
407  return det;
408  }
409 #else
410  {
411  if (rowNum != colNum) {
412  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
413  rowNum, colNum));
414  }
415 
416  integer dim = (integer)rowNum;
417  integer lda = dim;
418  integer info;
419  integer *ipiv = new integer[dim + 1];
420 
421  vpMatrix A = *this;
422 
423  dgetrf_(&dim, &dim, A.data, &lda, &ipiv[1], &info);
424  if (info < 0) {
425  delete[] ipiv;
426  throw(vpException(vpException::fatalError, "Lapack LU decomposition failed; info=%d", -info));
427  }
428 
429  double det = A[0][0];
430  for (unsigned int i = 1; i < rowNum; i++) {
431  det *= A[i][i];
432  }
433 
434  double sign = 1.;
435  for (int i = 1; i <= dim; i++) {
436  if (ipiv[i] != i)
437  sign = -sign;
438  }
439 
440  det *= sign;
441 
442  delete[] ipiv;
443 
444  return det;
445  }
446 #endif
447 }
448 #endif
449 
450 #if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
482 {
483  if (rowNum != colNum) {
484  throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
485  }
486 
487  cv::Mat M(rowNum, colNum, CV_64F, this->data);
488 
489  cv::Mat Minv = M.inv(cv::DECOMP_LU);
490 
491  vpMatrix A(rowNum, colNum);
492  memcpy(A.data, Minv.data, (size_t)(8 * Minv.rows * Minv.cols));
493 
494  return A;
495 }
496 
523 {
524  double det = 0.;
525 
526  if (rowNum != colNum) {
527  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
528  rowNum, colNum));
529  }
530 
531  cv::Mat M(rowNum, colNum, CV_64F, this->data);
532  det = cv::determinant(M);
533 
534  return (det);
535 }
536 #endif
537 
538 #if defined(VISP_HAVE_EIGEN3)
539 
571 {
572  if (rowNum != colNum) {
573  throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
574  }
575  vpMatrix A(this->getRows(), this->getCols());
576 
577  Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > M(this->data, this->getRows(),
578  this->getCols());
579  Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > A_(A.data, this->getRows(),
580  this->getCols());
581 
582  A_ = M.inverse();
583 
584  return A;
585 }
586 
613 {
614  if (rowNum != colNum) {
615  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
616  rowNum, colNum));
617  }
618 
619  Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > M(this->data, this->getRows(),
620  this->getCols());
621 
622  return M.determinant();
623 }
624 #endif
unsigned int getCols() const
Definition: vpArray2D.h:281
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:306
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
unsigned int getRows() const
Definition: vpArray2D.h:291
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:6464
double detByLULapack() const