Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpMatrix_cholesky.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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 defined(VISP_HAVE_OPENCV) // 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>
62typedef MKL_INT integer;
63#elif !defined(VISP_HAVE_GSL)
64#ifdef VISP_HAVE_LAPACK_BUILT_IN
65typedef long int integer;
66#else
67typedef int integer;
68#endif
69extern "C" void dpotrf_(char *uplo, integer *n, double *a, integer *lda, integer *info);
70extern "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)
116#elif defined(VISP_HAVE_OPENCV)
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)
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 defined(VISP_HAVE_OPENCV)
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
266 memcpy(A.data, Minv.data, (size_t)(8 * Minv.rows * Minv.cols));
267
268 return A;
269}
270#endif
unsigned int getCols() const
Definition vpArray2D.h:280
Type * data
Address of the first element of the data array.
Definition vpArray2D.h:144
unsigned int rowNum
Number of rows in the array.
Definition vpArray2D.h:134
unsigned int getRows() const
Definition vpArray2D.h:290
unsigned int colNum
Number of columns in the array.
Definition vpArray2D.h:136
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ badValue
Used to indicate that a value is not in the allowed range.
Definition vpException.h:85
@ fatalError
Fatal error.
Definition vpException.h:84
error that can be emitted by the vpMatrix class and its derivatives
@ matrixError
Matrix operation error.
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
vpMatrix inverseByCholeskyLapack() const
vpMatrix inverseByCholesky() const
vpMatrix inverseByCholeskyOpenCV() const