Visual Servoing Platform  version 3.3.1 under development (2020-08-12)
vpMatrix_cholesky.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 Cholesky decomposition.
33  *
34  * Authors:
35  * Filip Novotny
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 // Exception
46 #include <visp3/core/vpException.h>
47 #include <visp3/core/vpMatrixException.h>
48 
49 // Debug trace
50 #include <visp3/core/vpDebug.h>
51 
52 #if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
53 #include <opencv2/core/core.hpp>
54 #endif
55 
56 #ifdef VISP_HAVE_LAPACK
57 # ifdef VISP_HAVE_GSL
58 # include <gsl/gsl_linalg.h>
59 # endif
60 # ifdef VISP_HAVE_MKL
61 # include <mkl.h>
62 typedef MKL_INT integer;
63 # elif !defined(VISP_HAVE_GSL)
64 # ifdef VISP_HAVE_LAPACK_BUILT_IN
65 typedef long int integer;
66 # else
67 typedef int integer;
68 # endif
69 extern "C" void dpotrf_(char *uplo, integer *n, double *a, integer *lda, integer *info);
70 extern "C" int dpotri_(char *uplo, integer *n, double *a, integer *lda, integer *info);
71 # endif
72 #endif
73 
113 {
114 #if defined(VISP_HAVE_LAPACK)
115  return inverseByCholeskyLapack();
116 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101)
117  return inverseByCholeskyOpenCV();
118 #else
119  throw(vpException(vpException::fatalError, "Cannot inverse matrix by "
120  "Cholesky. Install Lapack or "
121  "OpenCV 3rd party"));
122 #endif
123 }
124 
125 #if defined(VISP_HAVE_LAPACK)
126 
163 {
164 #if defined(VISP_HAVE_GSL)
165  {
166  vpMatrix invA = *this;
167 
168  gsl_matrix cholesky;
169  cholesky.size1 = rowNum;
170  cholesky.size2 = colNum;
171  cholesky.tda = cholesky.size2;
172  cholesky.data = invA.data;
173  cholesky.owner = 0;
174  cholesky.block = 0;
175 
176 #if (GSL_MAJOR_VERSION >= 2 && GSL_MINOR_VERSION >= 3)
177  gsl_linalg_cholesky_decomp1(&cholesky);
178 #else
179  gsl_linalg_cholesky_decomp(&cholesky);
180 #endif
181  gsl_linalg_cholesky_invert(&cholesky);
182  return invA;
183  }
184 #else
185  {
186  if (rowNum != colNum) {
187  throw(vpMatrixException(vpMatrixException::matrixError, "Cannot inverse a non-square matrix (%ux%u) by Cholesky",
188  rowNum, colNum));
189  }
190 
191  integer rowNum_ = (integer)this->getRows();
192  integer lda = (integer)rowNum_; // lda is the number of rows because we don't use a submatrix
193  integer info;
194 
195  vpMatrix A = *this;
196  dpotrf_((char *)"L", &rowNum_, A.data, &lda, &info);
197 
198  if (info != 0)
199  throw(vpException(vpException::fatalError, "Cannot inverse by Cholesky with Lapack: error in dpotrf_()"));
200 
201  dpotri_((char *)"L", &rowNum_, A.data, &lda, &info);
202  if (info != 0) {
203  std::cout << "cholesky:dpotri_:error" << std::endl;
205  }
206 
207  for (unsigned int i = 0; i < A.getRows(); i++)
208  for (unsigned int j = 0; j < A.getCols(); j++)
209  if (i > j)
210  A[i][j] = A[j][i];
211 
212  return A;
213  }
214 #endif
215 }
216 #endif
217 
218 #if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
219 
256 {
257  if (rowNum != colNum) {
258  throw(
259  vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by Cholesky", rowNum, colNum));
260  }
261 
262  cv::Mat M(rowNum, colNum, CV_64F, this->data);
263  cv::Mat Minv = M.inv(cv::DECOMP_CHOLESKY);
264 
265  vpMatrix A(rowNum, colNum);
266  memcpy(A.data, Minv.data, (size_t)(8 * Minv.rows * Minv.cols));
267 
268  return A;
269 }
270 #endif
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:156
vpMatrix inverseByCholeskyOpenCV() const
error that can be emited by ViSP classes.
Definition: vpException.h:71
unsigned int getRows() const
Definition: vpArray2D.h:289
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
vpMatrix inverseByCholesky() const
unsigned int colNum
Number of columns in the array.
Definition: vpArray2D.h:137
vpMatrix inverseByCholeskyLapack() const
error that can be emited by the vpMatrix class and its derivates