Visual Servoing Platform  version 3.3.0 under development (2020-02-17)
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_MKL
58 #include <mkl.h>
59 typedef MKL_INT integer;
60 # else
61 # ifdef VISP_HAVE_LAPACK_BUILT_IN
62 typedef long int integer;
63 # else
64 typedef int integer;
65 # endif
66 extern "C" void dpotrf_(char *uplo, integer *n, double *a, integer *lda, integer *info);
67 extern "C" int dpotri_(char *uplo, integer *n, double *a, integer *lda, integer *info);
68 # endif
69 #endif
70 
110 {
111 #if defined(VISP_HAVE_LAPACK)
112  return inverseByCholeskyLapack();
113 #elif (VISP_HAVE_OPENCV_VERSION >= 0x020101)
114  return inverseByCholeskyOpenCV();
115 #else
116  throw(vpException(vpException::fatalError, "Cannot inverse matrix by "
117  "Cholesky. Install Lapack or "
118  "OpenCV 3rd party"));
119 #endif
120 }
121 
122 #if defined(VISP_HAVE_LAPACK)
123 
160 {
161  if (rowNum != colNum) {
162  throw(vpMatrixException(vpMatrixException::matrixError, "Cannot inverse a non-square matrix (%ux%u) by Cholesky",
163  rowNum, colNum));
164  }
165 
166  integer rowNum_ = (integer)this->getRows();
167  integer lda = (integer)rowNum_; // lda is the number of rows because we don't use a submatrix
168  integer info;
169 
170  vpMatrix A = *this;
171  dpotrf_((char *)"L", &rowNum_, A.data, &lda, &info);
172 
173  if (info != 0)
174  throw(vpException(vpException::fatalError, "Cannot inverse by Cholesky with Lapack: error in dpotrf_()"));
175 
176  dpotri_((char *)"L", &rowNum_, A.data, &lda, &info);
177  if (info != 0) {
178  std::cout << "cholesky:dpotri_:error" << std::endl;
180  }
181 
182  for (unsigned int i = 0; i < A.getRows(); i++)
183  for (unsigned int j = 0; j < A.getCols(); j++)
184  if (i > j)
185  A[i][j] = A[j][i];
186 
187  return A;
188 }
189 #endif
190 
191 #if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
192 
229 {
230  if (rowNum != colNum) {
231  throw(
232  vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by Cholesky", rowNum, colNum));
233  }
234 
235  cv::Mat M(rowNum, colNum, CV_64F, this->data);
236  cv::Mat Minv = M.inv(cv::DECOMP_CHOLESKY);
237 
238  vpMatrix A(rowNum, colNum);
239  memcpy(A.data, Minv.data, (size_t)(8 * Minv.rows * Minv.cols));
240 
241  return A;
242 }
243 #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:164
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