Visual Servoing Platform  version 3.6.1 under development (2024-12-07)
vpMatrix_cholesky.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 Cholesky 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 // Exception
41 #include <visp3/core/vpException.h>
42 #include <visp3/core/vpMatrixException.h>
43 
44 #if defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
45 #include <opencv2/core/core.hpp>
46 #endif
47 
48 #ifdef VISP_HAVE_LAPACK
49 #ifdef VISP_HAVE_GSL
50 #include <gsl/gsl_linalg.h>
51 #endif
52 #ifdef VISP_HAVE_MKL
53 #include <mkl.h>
54 typedef MKL_INT integer;
55 #elif !defined(VISP_HAVE_GSL)
56 #ifdef VISP_HAVE_LAPACK_BUILT_IN
57 typedef long int integer;
58 #else
59 typedef int integer;
60 #endif
61 extern "C" void dpotrf_(char *uplo, integer *n, double *a, integer *lda, integer *info);
62 extern "C" int dpotri_(char *uplo, integer *n, double *a, integer *lda, integer *info);
63 #endif
64 #endif
65 
66 #if defined(VISP_HAVE_EIGEN3)
67 #include <Eigen/Dense>
68 #endif
69 
70 BEGIN_VISP_NAMESPACE
113 {
114 #if defined(VISP_HAVE_LAPACK)
115  return inverseByCholeskyLapack();
116 #elif defined(VISP_HAVE_OPENCV)
117  return inverseByCholeskyOpenCV();
118 #else
119  throw(vpException(vpException::fatalError, "Cannot inverse matrix by Cholesky. Install Lapack or OpenCV 3rd party"));
120 #endif
121 }
122 
123 #if defined(VISP_HAVE_LAPACK)
165 {
166 #if defined(VISP_HAVE_GSL)
167  {
168  vpMatrix invA = *this;
169 
170  gsl_matrix cholesky;
171  cholesky.size1 = rowNum;
172  cholesky.size2 = colNum;
173  cholesky.tda = cholesky.size2;
174  cholesky.data = invA.data;
175  cholesky.owner = 0;
176  cholesky.block = 0;
177 
178 #if (GSL_MAJOR_VERSION >= 2 && GSL_MINOR_VERSION >= 3)
179  gsl_linalg_cholesky_decomp1(&cholesky);
180 #else
181  gsl_linalg_cholesky_decomp(&cholesky);
182 #endif
183  gsl_linalg_cholesky_invert(&cholesky);
184  return invA;
185  }
186 #else
187  {
188  if (rowNum != colNum) {
189  throw(vpMatrixException(vpMatrixException::matrixError, "Cannot inverse a non-square matrix (%ux%u) by Cholesky",
190  rowNum, colNum));
191  }
192 
193  integer rowNum_ = (integer)this->getRows();
194  integer lda = (integer)rowNum_; // lda is the number of rows because we don't use a submatrix
195  integer info;
196 
197  vpMatrix A = *this;
198  dpotrf_((char *)"L", &rowNum_, A.data, &lda, &info);
199 
200  if (info != 0) {
201  std::stringstream errMsg;
202  errMsg << "Cannot inverse by Cholesky with Lapack: error "<< info << " in dpotrf_()";
203  throw(vpException(vpException::fatalError, errMsg.str()));
204  }
205 
206  dpotri_((char *)"L", &rowNum_, A.data, &lda, &info);
207  if (info != 0) {
208  std::cout << "cholesky:dpotri_:error" << std::endl;
210  }
211 
212  for (unsigned int i = 0; i < A.getRows(); ++i)
213  for (unsigned int j = 0; j < A.getCols(); ++j)
214  if (i > j)
215  A[i][j] = A[j][i];
216 
217  return A;
218  }
219 #endif
220 }
221 
229 {
230  vpMatrix L = *this;
231 #if defined(VISP_HAVE_GSL)
232  gsl_matrix cholesky;
233  cholesky.size1 = rowNum;
234  cholesky.size2 = colNum;
235  cholesky.tda = cholesky.size2;
236  cholesky.data = L.data;
237  cholesky.owner = 0;
238  cholesky.block = 0;
239 
240 #if (GSL_MAJOR_VERSION >= 2 && GSL_MINOR_VERSION >= 3)
241  gsl_linalg_cholesky_decomp1(&cholesky);
242 #else
243  gsl_linalg_cholesky_decomp(&cholesky);
244 #endif
245 #else
246  if (rowNum != colNum) {
247  throw(vpMatrixException(vpMatrixException::matrixError, "Cannot inverse a non-square matrix (%ux%u) by Cholesky",
248  rowNum, colNum));
249  }
250 
251  integer rowNum_ = (integer)this->getRows();
252  integer lda = (integer)rowNum_; // lda is the number of rows because we don't use a submatrix
253  integer info;
254  dpotrf_((char *)"L", &rowNum_, L.data, &lda, &info);
255  L = L.transpose(); // For an unknown reason, dpotrf seems to return the transpose of L
256  if (info < 0) {
257  std::stringstream errMsg;
258  errMsg << "The " << -info << "th argument has an illegal value" << std::endl;
260  }
261  else if (info > 0) {
262  std::stringstream errMsg;
263  errMsg << "The leading minor of order" << info << "is not positive definite." << std::endl;
265  }
266 #endif
267  // Make sure that the upper part of L is null
268  unsigned int nbRows = this->getRows();
269  unsigned int nbCols = this->getCols();
270  for (unsigned int r = 0; r < nbRows; ++r) {
271  for (unsigned int c = r + 1; c < nbCols; ++c) {
272  L[r][c] = 0.;
273  }
274  }
275  return L;
276 }
277 #endif // VISP_HAVE_LAPACK
278 
279 #if defined(VISP_HAVE_OPENCV)
321 {
322  if (rowNum != colNum) {
323  throw(
324  vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by Cholesky", rowNum, colNum));
325  }
326 
327  cv::Mat M(rowNum, colNum, CV_64F, this->data);
328  cv::Mat Minv = M.inv(cv::DECOMP_CHOLESKY);
329 
330  vpMatrix A(rowNum, colNum);
331  memcpy(A.data, Minv.data, (size_t)(8 * Minv.rows * Minv.cols));
332 
333  return A;
334 }
335 
343 {
344  cv::Mat M(rowNum, colNum, CV_64F);
345  memcpy(M.data, this->data, (size_t)(8 * M.rows * M.cols));
346  std::size_t bytes_per_row = sizeof(double)*rowNum;
347  bool result = cv::Cholesky(M.ptr<double>(), bytes_per_row, rowNum, nullptr, bytes_per_row, rowNum);
348  if (!result) {
349  throw(vpMatrixException(vpMatrixException::fatalError, "Could not compute the Cholesky's decomposition of the input matrix."));
350  }
351  vpMatrix L(rowNum, colNum);
352  memcpy(L.data, M.data, (size_t)(8 * M.rows * M.cols));
353  // Make sure that the upper part of L is null
354  unsigned int nbRows = this->getRows();
355  unsigned int nbCols = this->getCols();
356  for (unsigned int r = 0; r < nbRows; ++r) {
357  for (unsigned int c = r + 1; c < nbCols; ++c) {
358  L[r][c] = 0.;
359  }
360  }
361  return L;
362 }
363 #endif // VISP_HAVE_OPENCV
364 
365 #if defined(VISP_HAVE_EIGEN3)
373 {
374  unsigned int nbRows = this->getRows();
375  unsigned int nbCols = this->getCols();
376  Eigen::MatrixXd A(nbRows, nbCols);
377  for (unsigned int r = 0; r < nbRows; ++r) {
378  for (unsigned int c = 0; c < nbCols; ++c) {
379  A(r, c) = (*this)[r][c];
380  }
381  }
382  Eigen::MatrixXd L = A.llt().matrixL();
383  vpMatrix Lvisp(static_cast<unsigned int>(L.rows()), static_cast<unsigned int>(L.cols()), 0.);
384  for (unsigned int r = 0; r < nbRows; ++r) {
385  for (unsigned int c = 0; c <= r; ++c) {
386  Lvisp[r][c] = L(r, c);
387  }
388  }
389  return Lvisp;
390 }
391 #endif // VISP_HAVE_EIGEN3
392 
393 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
unsigned int rowNum
Number of rows in the array.
Definition: vpArray2D.h:1101
unsigned int getRows() const
Definition: vpArray2D.h:347
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:1103
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:73
@ fatalError
Fatal error.
Definition: vpException.h:72
error that can be emitted by the vpMatrix class and its derivatives
@ forbiddenOperatorError
Forbidden operation.
@ matrixError
Matrix operation error.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
vpMatrix inverseByCholesky() const
vpMatrix cholesky() const
Definition: vpMatrix.cpp:1648
vpMatrix inverseByCholeskyLapack() const
vpMatrix choleskyByOpenCV() const
Compute the Cholesky decomposition of a Hermitian positive-definite matrix using OpenCV library.
vpMatrix choleskyByEigen3() const
Compute the Cholesky decomposition of a Hermitian positive-definite matrix using Eigen3 library.
vpMatrix inverseByCholeskyOpenCV() const
vpMatrix choleskyByLapack() const
Compute the Cholesky decomposition of a Hermitian positive-definite matrix using Lapack library.