Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpMatrix_lu.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 LU decomposition.
33 *
34*****************************************************************************/
35
36#include <visp3/core/vpConfig.h>
37
38#include <visp3/core/vpColVector.h>
39#include <visp3/core/vpMath.h>
40#include <visp3/core/vpMatrix.h>
41
42#ifdef VISP_HAVE_EIGEN3
43#include <Eigen/LU>
44#endif
45
46#ifdef VISP_HAVE_LAPACK
47#ifdef VISP_HAVE_GSL
48#include <gsl/gsl_linalg.h>
49#include <gsl/gsl_permutation.h>
50#endif
51#ifdef VISP_HAVE_MKL
52#include <mkl.h>
53typedef MKL_INT integer;
54#else
55#ifdef VISP_HAVE_LAPACK_BUILT_IN
56typedef long int integer;
57#else
58typedef int integer;
59#endif
60extern "C" int dgetrf_(integer *m, integer *n, double *a, integer *lda, integer *ipiv, integer *info);
61extern "C" void dgetri_(integer *n, double *a, integer *lda, integer *ipiv, double *work, integer *lwork,
62 integer *info);
63#endif
64#endif
65
66#if defined(VISP_HAVE_OPENCV) // Require opencv >= 2.1.1
67#include <opencv2/core/core.hpp>
68#endif
69
70// Exception
71#include <visp3/core/vpException.h>
72#include <visp3/core/vpMatrixException.h>
73
74#include <cmath> // std::fabs
75#include <limits> // numeric_limits
76
77/*--------------------------------------------------------------------
78 LU Decomposition related functions
79-------------------------------------------------------------------- */
80
128{
129 if (colNum == 1 && rowNum == 1) {
130 vpMatrix inv;
131 inv.resize(1, 1, false);
132 double d = det();
133 if (std::fabs(d) < std::numeric_limits<double>::epsilon()) {
134 throw(vpException(vpException::fatalError, "Cannot inverse matrix %d by %d by LU. Matrix determinant is 0.",
135 rowNum, colNum));
136 }
137 inv[0][0] = 1. / d;
138 return inv;
139 } else if (colNum == 2 && rowNum == 2) {
140 vpMatrix inv;
141 inv.resize(2, 2, false);
142 double d = det();
143 if (std::fabs(d) < std::numeric_limits<double>::epsilon()) {
144 throw(vpException(vpException::fatalError, "Cannot inverse matrix %d by %d by LU. Matrix determinant is 0.",
145 rowNum, colNum));
146 }
147 d = 1. / d;
148 inv[1][1] = (*this)[0][0] * d;
149 inv[0][0] = (*this)[1][1] * d;
150 inv[0][1] = -(*this)[0][1] * d;
151 inv[1][0] = -(*this)[1][0] * d;
152 return inv;
153 } else if (colNum == 3 && rowNum == 3) {
154 vpMatrix inv;
155 inv.resize(3, 3, false);
156 double d = det();
157 if (std::fabs(d) < std::numeric_limits<double>::epsilon()) {
158 throw(vpException(vpException::fatalError, "Cannot inverse matrix %d by %d by LU. Matrix determinant is 0.",
159 rowNum, colNum));
160 }
161 d = 1. / d;
162 inv[0][0] = ((*this)[1][1] * (*this)[2][2] - (*this)[1][2] * (*this)[2][1]) * d;
163 inv[0][1] = ((*this)[0][2] * (*this)[2][1] - (*this)[0][1] * (*this)[2][2]) * d;
164 inv[0][2] = ((*this)[0][1] * (*this)[1][2] - (*this)[0][2] * (*this)[1][1]) * d;
165
166 inv[1][0] = ((*this)[1][2] * (*this)[2][0] - (*this)[1][0] * (*this)[2][2]) * d;
167 inv[1][1] = ((*this)[0][0] * (*this)[2][2] - (*this)[0][2] * (*this)[2][0]) * d;
168 inv[1][2] = ((*this)[0][2] * (*this)[1][0] - (*this)[0][0] * (*this)[1][2]) * d;
169
170 inv[2][0] = ((*this)[1][0] * (*this)[2][1] - (*this)[1][1] * (*this)[2][0]) * d;
171 inv[2][1] = ((*this)[0][1] * (*this)[2][0] - (*this)[0][0] * (*this)[2][1]) * d;
172 inv[2][2] = ((*this)[0][0] * (*this)[1][1] - (*this)[0][1] * (*this)[1][0]) * d;
173 return inv;
174 } else {
175#if defined(VISP_HAVE_LAPACK)
176 return inverseByLULapack();
177#elif defined(VISP_HAVE_EIGEN3)
178 return inverseByLUEigen3();
179#elif defined(VISP_HAVE_OPENCV)
180 return inverseByLUOpenCV();
181#else
182 throw(vpException(vpException::fatalError, "Cannot inverse by LU. "
183 "Install Lapack, Eigen3 or OpenCV 3rd party"));
184#endif
185 }
186}
187
221double vpMatrix::detByLU() const
222{
223 if (rowNum == 1 && colNum == 1) {
224 return (*this)[0][0];
225 } else if (rowNum == 2 && colNum == 2) {
226 return ((*this)[0][0] * (*this)[1][1] - (*this)[0][1] * (*this)[1][0]);
227 } else if (rowNum == 3 && colNum == 3) {
228 return ((*this)[0][0] * ((*this)[1][1] * (*this)[2][2] - (*this)[1][2] * (*this)[2][1]) -
229 (*this)[0][1] * ((*this)[1][0] * (*this)[2][2] - (*this)[1][2] * (*this)[2][0]) +
230 (*this)[0][2] * ((*this)[1][0] * (*this)[2][1] - (*this)[1][1] * (*this)[2][0]));
231 } else {
232#if defined(VISP_HAVE_LAPACK)
233 return detByLULapack();
234#elif defined(VISP_HAVE_EIGEN3)
235 return detByLUEigen3();
236#elif defined(VISP_HAVE_OPENCV)
237 return detByLUOpenCV();
238#else
239 throw(vpException(vpException::fatalError, "Cannot compute matrix determinant. "
240 "Install Lapack, Eigen3 or OpenCV 3rd party"));
241#endif
242 }
243}
244
245#if defined(VISP_HAVE_LAPACK)
277{
278#if defined(VISP_HAVE_GSL)
279 {
280 if (rowNum != colNum) {
281 throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
282 }
283
284 gsl_matrix *A = gsl_matrix_alloc(rowNum, colNum);
285
286 // copy the input matrix to ensure the function doesn't modify its content
287 unsigned int tda = (unsigned int)A->tda;
288 for (unsigned int i = 0; i < rowNum; i++) {
289 unsigned int k = i * tda;
290 for (unsigned int j = 0; j < colNum; j++)
291 A->data[k + j] = (*this)[i][j];
292 }
293
294 vpMatrix Ainv(rowNum, colNum);
295
296 gsl_matrix inverse;
297 inverse.size1 = rowNum;
298 inverse.size2 = colNum;
299 inverse.tda = inverse.size2;
300 inverse.data = Ainv.data;
301 inverse.owner = 0;
302 inverse.block = 0;
303
304 gsl_permutation *p = gsl_permutation_alloc(rowNum);
305 int s;
306
307 // Do the LU decomposition on A and use it to solve the system
308 gsl_linalg_LU_decomp(A, p, &s);
309 gsl_linalg_LU_invert(A, p, &inverse);
310
311 gsl_permutation_free(p);
312 gsl_matrix_free(A);
313
314 return Ainv;
315 }
316#else
317 {
318 if (rowNum != colNum) {
319 throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
320 }
321
322 integer dim = (integer)rowNum;
323 integer lda = dim;
324 integer info;
325 integer lwork = dim * dim;
326 integer *ipiv = new integer[dim + 1];
327 double *work = new double[lwork];
328
329 vpMatrix A = *this;
330
331 dgetrf_(&dim, &dim, A.data, &lda, &ipiv[1], &info);
332 if (info) {
333 delete[] ipiv;
334 delete[] work;
335 throw(vpException(vpException::fatalError, "Lapack LU decomposition failed; info=%d", info));
336 }
337
338 dgetri_(&dim, A.data, &dim, &ipiv[1], work, &lwork, &info);
339
340 delete[] ipiv;
341 delete[] work;
342
343 return A;
344 }
345#endif
346}
347
374{
375#if defined(VISP_HAVE_GSL)
376 {
377 double det = 0.;
378
379 if (rowNum != colNum) {
380 throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
381 rowNum, colNum));
382 }
383
384 gsl_matrix *A = gsl_matrix_alloc(rowNum, colNum);
385
386 // copy the input matrix to ensure the function doesn't modify its content
387 unsigned int tda = (unsigned int)A->tda;
388 for (unsigned int i = 0; i < rowNum; i++) {
389 unsigned int k = i * tda;
390 for (unsigned int j = 0; j < colNum; j++)
391 A->data[k + j] = (*this)[i][j];
392 }
393
394 gsl_permutation *p = gsl_permutation_alloc(rowNum);
395 int s;
396
397 // Do the LU decomposition on A and use it to solve the system
398 gsl_linalg_LU_decomp(A, p, &s);
399 det = gsl_linalg_LU_det(A, s);
400
401 gsl_permutation_free(p);
402 gsl_matrix_free(A);
403
404 return det;
405 }
406#else
407 {
408 if (rowNum != colNum) {
409 throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
410 rowNum, colNum));
411 }
412
413 integer dim = (integer)rowNum;
414 integer lda = dim;
415 integer info;
416 integer *ipiv = new integer[dim + 1];
417
418 vpMatrix A = *this;
419
420 dgetrf_(&dim, &dim, A.data, &lda, &ipiv[1], &info);
421 if (info < 0) {
422 delete[] ipiv;
423 throw(vpException(vpException::fatalError, "Lapack LU decomposition failed; info=%d", -info));
424 }
425
426 double det = A[0][0];
427 for (unsigned int i = 1; i < rowNum; i++) {
428 det *= A[i][i];
429 }
430
431 double sign = 1.;
432 for (int i = 1; i <= dim; i++) {
433 if (ipiv[i] != i)
434 sign = -sign;
435 }
436
437 det *= sign;
438
439 delete[] ipiv;
440
441 return det;
442 }
443#endif
444}
445#endif
446
447#if defined(VISP_HAVE_OPENCV)
479{
480 if (rowNum != colNum) {
481 throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
482 }
483
484 cv::Mat M(rowNum, colNum, CV_64F, this->data);
485
486 cv::Mat Minv = M.inv(cv::DECOMP_LU);
487
489 memcpy(A.data, Minv.data, (size_t)(8 * Minv.rows * Minv.cols));
490
491 return A;
492}
493
520{
521 double det = 0.;
522
523 if (rowNum != colNum) {
524 throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
525 rowNum, colNum));
526 }
527
528 cv::Mat M(rowNum, colNum, CV_64F, this->data);
529 det = cv::determinant(M);
530
531 return (det);
532}
533#endif
534
535#if defined(VISP_HAVE_EIGEN3)
536
568{
569 if (rowNum != colNum) {
570 throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
571 }
572 vpMatrix A(this->getRows(), this->getCols());
573
574 Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > M(this->data, this->getRows(),
575 this->getCols());
576 Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > A_(A.data, this->getRows(),
577 this->getCols());
578
579 A_ = M.inverse();
580
581 return A;
582}
583
610{
611 if (rowNum != colNum) {
612 throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
613 rowNum, colNum));
614 }
615
616 Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > M(this->data, this->getRows(),
617 this->getCols());
618
619 return M.determinant();
620}
621#endif
unsigned int getCols() const
Definition vpArray2D.h:280
Type * data
Address of the first element of the data array.
Definition vpArray2D.h:144
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition vpArray2D.h:305
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
@ fatalError
Fatal error.
Definition vpException.h:84
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
vpMatrix inverseByLU() const
vpMatrix inverseByLUEigen3() const
vpMatrix inverseByLUOpenCV() const
double detByLUEigen3() const
double detByLUOpenCV() const
vpMatrix inverseByLULapack() const
double detByLU() const
double det(vpDetMethod method=LU_DECOMPOSITION) const
double detByLULapack() const