Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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.", 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 
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  } 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)
251 
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)
453 
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 
493  vpMatrix A(rowNum, colNum);
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
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:6476
vpMatrix inverseByLUEigen3() const
vpMatrix inverseByLU() const
error that can be emited by ViSP classes.
Definition: vpException.h:71
unsigned int getRows() const
Definition: vpArray2D.h:289
vpMatrix inverseByLUOpenCV() const
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
unsigned int getCols() const
Definition: vpArray2D.h:279
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:135
double detByLUEigen3() const
double detByLUOpenCV() const
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:137
vpMatrix inverseByLULapack() const
double detByLULapack() const
double detByLU() const