Visual Servoing Platform  version 3.2.1 under development (2019-11-19)
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_GSL
50 #include <gsl/gsl_linalg.h>
51 #include <gsl/gsl_permutation.h>
52 #endif
53 
54 #ifdef VISP_HAVE_LAPACK
55 # ifdef VISP_HAVE_MKL
56 #include <mkl.h>
57 typedef MKL_INT integer;
58 # else
59 # ifdef VISP_HAVE_LAPACK_BUILT_IN
60 typedef long int integer;
61 # else
62 typedef int integer;
63 # endif
64 extern "C" int dgetrf_(integer *m, integer *n, double *a, integer *lda, integer *ipiv, integer *info);
65 extern "C" void dgetri_(integer *n, double *a, integer *lda, integer *ipiv, double *work, integer *lwork,
66  integer *info);
67 # endif
68 #endif
69 
70 #if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
71 #include <opencv2/core/core.hpp>
72 #endif
73 
74 // Exception
75 #include <visp3/core/vpException.h>
76 #include <visp3/core/vpMatrixException.h>
77 
78 #include <cmath> // std::fabs
79 #include <limits> // numeric_limits
80 
81 /*--------------------------------------------------------------------
82  LU Decomposition related functions
83 -------------------------------------------------------------------- */
84 
135 {
136 #if defined(VISP_HAVE_LAPACK)
137  return inverseByLULapack();
138 #elif defined(VISP_HAVE_EIGEN3)
139  return inverseByLUEigen3();
140 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101)
141  return inverseByLUOpenCV();
142 #elif defined(VISP_HAVE_GSL)
143  return inverseByLUGsl();
144 #else
145  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant. Install Eigen3, "
146  "Lapack, OpenCV or GSL 3rd party"));
147 #endif
148 }
149 
184 double vpMatrix::detByLU() const
185 {
186  if (rowNum == 2 && colNum == 2) {
187  return ((*this)[0][0] * (*this)[1][1] - (*this)[0][1] * (*this)[1][0]);
188  } else if (rowNum == 3 && colNum == 3) {
189  return ((*this)[0][0] * ((*this)[1][1] * (*this)[2][2] - (*this)[1][2] * (*this)[2][1]) -
190  (*this)[0][1] * ((*this)[1][0] * (*this)[2][2] - (*this)[1][2] * (*this)[2][0]) +
191  (*this)[0][2] * ((*this)[1][0] * (*this)[2][1] - (*this)[1][1] * (*this)[2][0]));
192  } else {
193 #if defined(VISP_HAVE_LAPACK)
194  return detByLULapack();
195 #elif defined(VISP_HAVE_EIGEN3)
196  return detByLUEigen3();
197 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101)
198  return detByLUOpenCV();
199 #elif defined(VISP_HAVE_GSL)
200  return detByLUGsl();
201 #else
202  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant. Install Lapack, "
203  "Eigen3, OpenCV or GSL 3rd party"));
204 #endif
205  }
206 }
207 
208 
209 #if defined(VISP_HAVE_GSL)
210 
242 {
243  if (rowNum != colNum) {
244  throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
245  }
246 
247  gsl_matrix *A = gsl_matrix_alloc(rowNum, colNum);
248 
249  // copy the input matrix to ensure the function doesn't modify its content
250  unsigned int tda = (unsigned int)A->tda;
251  for (unsigned int i = 0; i < rowNum; i++) {
252  unsigned int k = i * tda;
253  for (unsigned int j = 0; j < colNum; j++)
254  A->data[k + j] = (*this)[i][j];
255  }
256 
257  vpMatrix Ainv(rowNum, colNum);
258 
259  gsl_matrix inverse;
260  inverse.size1 = rowNum;
261  inverse.size2 = colNum;
262  inverse.tda = inverse.size2;
263  inverse.data = Ainv.data;
264  inverse.owner = 0;
265  inverse.block = 0;
266 
267  gsl_permutation *p = gsl_permutation_alloc(rowNum);
268  int s;
269 
270  // Do the LU decomposition on A and use it to solve the system
271  gsl_linalg_LU_decomp(A, p, &s);
272  gsl_linalg_LU_invert(A, p, &inverse);
273 
274  gsl_permutation_free(p);
275  gsl_matrix_free(A);
276 
277  return Ainv;
278 }
279 
305 double vpMatrix::detByLUGsl() const
306 {
307  double det = 0.;
308 
309  if (rowNum != colNum) {
310  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
311  rowNum, colNum));
312  }
313 
314  gsl_matrix *A = gsl_matrix_alloc(rowNum, colNum);
315 
316  // copy the input matrix to ensure the function doesn't modify its content
317  unsigned int tda = (unsigned int)A->tda;
318  for (unsigned int i = 0; i < rowNum; i++) {
319  unsigned int k = i * tda;
320  for (unsigned int j = 0; j < colNum; j++)
321  A->data[k + j] = (*this)[i][j];
322  }
323 
324  gsl_permutation *p = gsl_permutation_alloc(rowNum);
325  int s;
326 
327  // Do the LU decomposition on A and use it to solve the system
328  gsl_linalg_LU_decomp(A, p, &s);
329  det = gsl_linalg_LU_det(A, s);
330 
331  gsl_permutation_free(p);
332  gsl_matrix_free(A);
333 
334  return det;
335 }
336 #endif
337 
338 #if defined(VISP_HAVE_LAPACK)
339 
371 {
372  if (rowNum != colNum) {
373  throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
374  }
375 
376  integer dim = (integer)rowNum;
377  integer lda = dim;
378  integer info;
379  integer lwork = dim * dim;
380  integer *ipiv = new integer[dim + 1];
381  double *work = new double[lwork];
382 
383  vpMatrix A = *this;
384 
385  dgetrf_(&dim, &dim, A.data, &lda, &ipiv[1], &info);
386  if (info) {
387  delete[] ipiv;
388  delete[] work;
389  throw(vpException(vpException::fatalError, "Lapack LU decomposition failed; info=%d", info));
390  }
391 
392  dgetri_(&dim, A.data, &dim, &ipiv[1], work, &lwork, &info);
393 
394  delete[] ipiv;
395  delete[] work;
396 
397  return A;
398 }
399 
426 {
427  if (rowNum != colNum) {
428  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
429  rowNum, colNum));
430  }
431 
432  integer dim = (integer)rowNum;
433  integer lda = dim;
434  integer info;
435  integer *ipiv = new integer[dim + 1];
436 
437  vpMatrix A = *this;
438 
439  dgetrf_(&dim, &dim, A.data, &lda, &ipiv[1], &info);
440  if (info < 0) {
441  delete[] ipiv;
442  throw(vpException(vpException::fatalError, "Lapack LU decomposition failed; info=%d", -info));
443  }
444 
445  double det = A[0][0];
446  for (unsigned int i = 1; i < rowNum; i++) {
447  det *= A[i][i];
448  }
449 
450  double sign = 1.;
451  for (int i = 1; i <= dim; i++) {
452  if (ipiv[i] != i)
453  sign = -sign;
454  }
455 
456  det *= sign;
457 
458  delete[] ipiv;
459 
460  return det;
461 }
462 #endif
463 
464 #if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
465 
497 {
498  if (rowNum != colNum) {
499  throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
500  }
501 
502  cv::Mat M(rowNum, colNum, CV_64F, this->data);
503 
504  cv::Mat Minv = M.inv(cv::DECOMP_LU);
505 
506  vpMatrix A(rowNum, colNum);
507  memcpy(A.data, Minv.data, (size_t)(8 * Minv.rows * Minv.cols));
508 
509  return A;
510 }
511 
538 {
539  double det = 0.;
540 
541  if (rowNum != colNum) {
542  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
543  rowNum, colNum));
544  }
545 
546  cv::Mat M(rowNum, colNum, CV_64F, this->data);
547  det = cv::determinant(M);
548 
549  return (det);
550 }
551 #endif
552 
553 #if defined(VISP_HAVE_EIGEN3)
554 
587 {
588  if (rowNum != colNum) {
589  throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
590  }
591  vpMatrix A(this->getRows(), this->getCols());
592 
593  Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > M(this->data, this->getRows(),
594  this->getCols());
595  Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > A_(A.data, this->getRows(),
596  this->getCols());
597 
598  A_ = M.inverse();
599 
600  return A;
601 }
602 
629 {
630  if (rowNum != colNum) {
631  throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
632  rowNum, colNum));
633  }
634 
635  Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > M(this->data, this->getRows(),
636  this->getCols());
637 
638  return M.determinant();
639 }
640 #endif
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
double det(vpDetMethod method=LU_DECOMPOSITION) const
Definition: vpMatrix.cpp:5246
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 detByLUGsl() 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
vpMatrix inverseByLUGsl() const