LinearAlgebraCV.cpp
Go to the documentation of this file.
1 // ****************************************************************************
2 // This file is part of the Integrating Vision Toolkit (IVT).
3 //
4 // The IVT is maintained by the Karlsruhe Institute of Technology (KIT)
5 // (www.kit.edu) in cooperation with the company Keyetech (www.keyetech.de).
6 //
7 // Copyright (C) 2014 Karlsruhe Institute of Technology (KIT).
8 // All rights reserved.
9 //
10 // Redistribution and use in source and binary forms, with or without
11 // modification, are permitted provided that the following conditions are met:
12 //
13 // 1. Redistributions of source code must retain the above copyright
14 // notice, this list of conditions and the following disclaimer.
15 //
16 // 2. Redistributions in binary form must reproduce the above copyright
17 // notice, this list of conditions and the following disclaimer in the
18 // documentation and/or other materials provided with the distribution.
19 //
20 // 3. Neither the name of the KIT nor the names of its contributors may be
21 // used to endorse or promote products derived from this software
22 // without specific prior written permission.
23 //
24 // THIS SOFTWARE IS PROVIDED BY THE KIT AND CONTRIBUTORS “AS IS” AND ANY
25 // EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
26 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
27 // DISCLAIMED. IN NO EVENT SHALL THE KIT OR CONTRIBUTORS BE LIABLE FOR ANY
28 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
29 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
31 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
32 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
33 // THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
34 // ****************************************************************************
35 
36 
37 
38 // ****************************************************************************
39 // Includes
40 // ****************************************************************************
41 
42 #include <new> // for explicitly using correct new/delete operators on VC DSPs
43 
44 #include "LinearAlgebraCV.h"
45 #include "LinearAlgebra.h"
46 #include "Math/FloatMatrix.h"
47 #include "Math/FloatVector.h"
48 #include "Math/DoubleMatrix.h"
49 #include "Math/DoubleVector.h"
50 #include "Math/Math2d.h"
51 #include "Math/Math3d.h"
52 #include "Helpers/helpers.h"
53 
54 #include <float.h>
55 #include <math.h>
56 #include <stdio.h>
57 
58 
59 
60 // ****************************************************************************
61 // Functions
62 // ****************************************************************************
63 
65 {
66  if (A->columns != x->dimension)
67  {
68  printf("error: A, b, x do not match LinearAlgebraCV::SolveLinearLeastSquaresHomogeneousSVD");
69  return;
70  }
71 
72  if (A->rows < A->columns)
73  {
74  printf("error: equation system is underdetermined in LinearAlgebraCV::SolveLinearLeastSquaresHomogeneousSVD\n");
75  return;
76  }
77 
78  const int m = A->rows;
79  const int n = A->columns;
80 
81  CFloatMatrix W(n, m), V(n, n);
82 
83  SVD(A, &W, 0, &V, false, false, false);
84 
85  for (int i = 0, offset = n - 1; i < n; i++, offset += n)
86  x->data[i] = V.data[offset];
87 }
88 
90 {
91  if (A->columns != x->dimension || A->rows != b->dimension)
92  {
93  printf("error: A, b, x do not match LinearAlgebraCV::SolveLinearLeastSquaresSVD");
94  return;
95  }
96 
97  if (A->rows < A->columns)
98  {
99  printf("error: equation system is underdetermined in LinearAlgebraCV::SolveLinearLeastSquaresSVD\n");
100  return;
101  }
102 
103  CFloatMatrix A_(A->rows, A->columns);
105  LinearAlgebra::MulMatVec(&A_, b, x);
106 }
107 
109 {
110  if (A->columns != x->dimension || A->rows != b->dimension)
111  {
112  printf("error: A, b, x do not match LinearAlgebraCV::SolveLinearLeastSquaresSimple");
113  return;
114  }
115 
116  if (A->rows < A->columns)
117  {
118  printf("error: equation system is underdetermined in LinearAlgebraCV::SolveLinearLeastSquaresSimple\n");
119  return;
120  }
121 
122  CFloatMatrix A_(A->rows, A->columns);
124  LinearAlgebra::MulMatVec(&A_, b, x);
125 }
126 
128 {
129  if (pInputMatrix->columns != pResultMatrix->rows || pInputMatrix->rows != pResultMatrix->columns)
130  {
131  printf("error: input and output matrix do not match LinearAlgebraCV::CalculatePseudoInverseSVD");
132  return;
133  }
134 
135  // algorithm:
136  // 1: compute U * W * V^T = A
137  // 2: compute W' = W^T with all non-zero values inverted
138  // 3: compute V * W' * U^T (=pseudoinverse of A)
139 
140  const CFloatMatrix *A = pInputMatrix;
141  const int m = pInputMatrix->rows;
142  const int n = pInputMatrix->columns;
143 
144  CFloatMatrix W(n, m), WT(m, n), UT(m, m), V(n, n);
145 
146  // calculate SVD
147  SVD(A, &W, &UT, &V, false, true, false);
148 
149  // transpose middle diagonal matrix
150  Transpose(&W, &WT);
151 
152  const int min = MY_MIN(m, n);
153 
154  int i;
155  float fThreshold = 0.0f;
156 
157  for (i = 0; i < min; i++)
158  fThreshold += WT(i, i);
159 
160  fThreshold *= 2 * FLT_EPSILON;
161 
162  // invert non-zero values (along diagonal)
163  for (i = 0; i < min; i++)
164  if (WT(i, i) < fThreshold)
165  WT(i, i) = 0.0f;
166  else
167  WT(i, i) = 1.0f / WT(i, i);
168 
169  // calculate pseudoinverse
170  CFloatMatrix temp(m, n);
171  Multiply(&V, &WT, &temp);
172  Multiply(&temp, &UT, pResultMatrix);
173 }
174 
176 {
177  if (pInputMatrix->columns != pResultMatrix->rows || pInputMatrix->rows != pResultMatrix->columns)
178  {
179  printf("error: input and output matrix do not match LinearAlgebraCV::CalculatePseudoInverseSimple");
180  return;
181  }
182 
183  // algorithm:
184  // compute (A * A^T)^-1 * A^T
185 
186  const CFloatMatrix *A = pInputMatrix;
187  const int m = pInputMatrix->rows;
188  const int n = pInputMatrix->columns;
189 
190  CFloatMatrix AT(m, n), ATA(n, n), ATA_inverted(n, n);
191 
192  Transpose(A, &AT);
193  Multiply(&AT, A, &ATA);
194  Invert(&ATA, &ATA_inverted);
195  Multiply(&ATA_inverted, &AT, pResultMatrix);
196 }
197 
198 void LinearAlgebraCV::Invert(const CFloatMatrix *pInputMatrix, const CFloatMatrix *pResultMatrix)
199 {
200  if (pInputMatrix->columns != pInputMatrix->rows)
201  {
202  printf("error: input is not square matrix in LinearAlgebraCV::Invert");
203  return;
204  }
205 
206  if (pInputMatrix->columns != pResultMatrix->columns || pInputMatrix->rows != pResultMatrix->rows)
207  {
208  printf("error: input and output matrix are not the same in LinearAlgebraCV::Invert");
209  return;
210  }
211 
212  CvMat inputMatrix = cvMat(pInputMatrix->rows, pInputMatrix->columns, CV_32FC1, pInputMatrix->data);
213  CvMat resultMatrix = cvMat(pResultMatrix->rows, pResultMatrix->columns, CV_32FC1, pResultMatrix->data);
214 
215  cvInvert(&inputMatrix, &resultMatrix);
216 }
217 
218 void LinearAlgebraCV::Transpose(const CFloatMatrix *pInputMatrix, const CFloatMatrix *pResultMatrix)
219 {
220  if (pInputMatrix->columns != pResultMatrix->rows || pInputMatrix->rows != pResultMatrix->columns)
221  {
222  printf("error: input and output matrix do not match LinearAlgebraCV::Transpose");
223  return;
224  }
225 
226  CvMat inputMatrix = cvMat(pInputMatrix->rows, pInputMatrix->columns, CV_32FC1, pInputMatrix->data);
227  CvMat resultMatrix = cvMat(pResultMatrix->rows, pResultMatrix->columns, CV_32FC1, pResultMatrix->data);
228 
229  cvTranspose(&inputMatrix, &resultMatrix);
230 }
231 
233 {
234  if (pCovarianceMatrix->columns != pMatrix->columns || pCovarianceMatrix->rows != pMatrix->columns)
235  return;
236 
237  const int columns = pMatrix->columns;
238  const int rows = pMatrix->rows;
239 
240  CvMat covarianceMatrix = cvMat(columns, columns, CV_32FC1, pCovarianceMatrix->data);
241 
242  CvMat **ppInput = new CvMat*[rows];
243  for (int i = 0; i < rows; i++)
244  {
245  CvMat *vector = cvCreateMatHeader(1, columns, CV_32FC1);
246  cvInitMatHeader(vector, 1, columns, CV_32FC1, pMatrix->data + i * columns);
247  ppInput[i] = vector;
248  }
249 
250  CvMat *avg = cvCreateMat(1, columns, CV_32FC1);
251 
252  #ifdef CV_COVAR_NORMAL
253  cvCalcCovarMatrix((const CvArr **) ppInput, rows, &covarianceMatrix, avg, CV_COVAR_NORMAL);
254  #else
255  cvCalcCovarMatrix((const CvArr **) ppInput, &covarianceMatrix, avg);
256  #endif
257 
258  cvReleaseMat(&avg);
259 }
260 
261 void LinearAlgebraCV::Multiply(const CFloatMatrix *A, const CFloatMatrix *B, CFloatMatrix *pResultMatrix, bool bTransposeB)
262 {
263  if (!bTransposeB && (A->columns != B->rows || pResultMatrix->columns != B->columns || pResultMatrix->rows != A->rows))
264  {
265  printf("error: matrices A, B, and pResultMatrix do not satisfy requirements for LinearAlgebraCV::Multiply\n");
266  return;
267  }
268 
269  if (bTransposeB && (A->columns != B->columns || pResultMatrix->columns != B->rows || pResultMatrix->rows != A->rows))
270  {
271  printf("error: matrices A, B, and pResultMatrix do not satisfy requirements for LinearAlgebraCV::Multiply\n");
272  return;
273  }
274 
275  int flags = 0;
276 
277  if (bTransposeB)
278  flags = CV_GEMM_B_T;
279 
280  CvMat matrixA = cvMat(A->rows, A->columns, CV_32FC1, A->data);
281  CvMat matrixB = cvMat(B->rows, B->columns, CV_32FC1, B->data);
282  CvMat result_matrix = cvMat(pResultMatrix->rows, pResultMatrix->columns, CV_32FC1, pResultMatrix->data);
283 
284  cvGEMM(&matrixA, &matrixB, 1, 0, 1, &result_matrix, flags);
285 }
286 
287 void LinearAlgebraCV::SelfProduct(const CFloatMatrix *pMatrix, CFloatMatrix *pResultMatrix, bool bTransposeSecond)
288 {
289  if (pResultMatrix->columns != pMatrix->columns || pResultMatrix->rows != pMatrix->columns)
290  return;
291 
292  CvMat matrix = cvMat(pMatrix->rows, pMatrix->columns, CV_32FC1, pMatrix->data);
293  CvMat result_matrix = cvMat(pResultMatrix->rows, pResultMatrix->columns, CV_32FC1, pResultMatrix->data);
294 
295  if (bTransposeSecond)
296  cvGEMM(&matrix, &matrix, 1, 0, 1, &result_matrix, CV_GEMM_B_T);
297  else
298  cvGEMM(&matrix, &matrix, 1, 0, 1, &result_matrix, CV_GEMM_A_T);
299 }
300 
301 void LinearAlgebraCV::SVD(const CFloatMatrix *A, CFloatMatrix *W, CFloatMatrix *U, CFloatMatrix *V, bool bAllowModifyA, bool bReturnUTransposed, bool bReturnVTransposed)
302 {
303  const int columns = A->columns;
304  const int rows = A->rows;
305 
306  if (W->columns != columns || W->rows != rows)
307  {
308  printf("error: W should have %i columns and %i rows for LinearAlgebra::SVD\n", columns, rows);
309  return;
310  }
311 
312  if (U && (U->columns != rows || U->rows != rows))
313  {
314  printf("error: U should have %i columns and %i rows for LinearAlgebra::SVD\n", rows, rows);
315  return;
316  }
317 
318  if (V && (V->columns != columns || V->rows != columns))
319  {
320  printf("error: V should have %i columns and %i rows for LinearAlgebra::SVD\n", columns, columns);
321  return;
322  }
323 
324  int flags = 0;
325 
326  if (bAllowModifyA)
327  flags |= CV_SVD_MODIFY_A;
328 
329  if (bReturnUTransposed)
330  flags |= CV_SVD_U_T;
331 
332  if (bReturnVTransposed)
333  flags |= CV_SVD_V_T;
334 
335  CvMat matrixA = cvMat(rows, columns, CV_32FC1, A->data);
336  CvMat matrixW = cvMat(rows, columns, CV_32FC1, W->data);
337 
338  if (U && V)
339  {
340  CvMat matrixU = cvMat(rows, rows, CV_32FC1, U->data);
341  CvMat matrixV = cvMat(columns, columns, CV_32FC1, V->data);
342 
343  cvSVD(&matrixA, &matrixW, &matrixU, &matrixV, flags);
344  }
345  else if (U)
346  {
347  CvMat matrixU = cvMat(rows, rows, CV_32FC1, U->data);
348 
349  cvSVD(&matrixA, &matrixW, &matrixU, 0, flags);
350  }
351  else if (V)
352  {
353  CvMat matrixV = cvMat(columns, columns, CV_32FC1, V->data);
354 
355  cvSVD(&matrixA, &matrixW, 0, &matrixV, flags);
356  }
357  else
358  {
359  cvSVD(&matrixA, &matrixW, 0, 0, flags);
360  }
361 }
362 
363 void LinearAlgebraCV::PCA(const CFloatMatrix *pData, CFloatMatrix *pTransformationMatrix, CFloatMatrix *pTransformedData, int nTargetDimension)
364 {
365  if (nTargetDimension > pData->columns)
366  {
367  printf("error: target dimension is greater than number of columns in training data matrix in LinearAlgebraCV::PCA\n");
368  return;
369  }
370 
371  const int samples = pData->rows;
372  const int dimension = pData->columns;
373 
374  if (pTransformationMatrix->columns != dimension || pTransformationMatrix->rows != nTargetDimension ||
375  pTransformedData->columns != samples || pTransformedData->rows != nTargetDimension)
376  {
377  printf("error: input to LinearAlgebraCV::PCA does not match\n");
378  return;
379  }
380 
381  CFloatMatrix adjustedData(pData);
382  CFloatMatrix covarianceMatrix(dimension, dimension);
383  CFloatMatrix eigenValues(dimension, dimension);
384  CFloatMatrix eigenVectors(dimension, dimension);
385 
386  printf("subtracting mean from columns...\n");
387  LinearAlgebra::SubtractMeanFromColumns(pData, &adjustedData);
388 
389  printf("calculating covariance matrix...\n");
390  LinearAlgebraCV::SelfProduct(&adjustedData, &covarianceMatrix);
391 
392  printf("calculating SVD on %ix%i matrix...\n", dimension, dimension);
393  LinearAlgebraCV::SVD(&covarianceMatrix, &eigenValues, &eigenVectors, 0, true, true);
394 
395  printf("SVD calculated\n");
396 
397  for (int i = 0, offset = 0; i < nTargetDimension; i++)
398  {
399  for (int j = 0; j < dimension; j++)
400  {
401  pTransformationMatrix->data[offset] = eigenVectors.data[i * dimension + j];
402  offset++;
403  }
404  }
405 
406  LinearAlgebraCV::Multiply(pTransformationMatrix, pData, pTransformedData, true);
407 }
408 
409 void LinearAlgebraCV::PCA(const CFloatMatrix *pData, CFloatMatrix *pTransformationMatrix, CFloatMatrix *pEigenValues)
410 {
411  const int samples = pData->rows;
412  const int dimension = pData->columns;
413 
414  if (pTransformationMatrix->columns != dimension || pTransformationMatrix->rows != dimension || pEigenValues->columns != 1 || pEigenValues->rows != dimension)
415  return;
416 
417  CFloatMatrix adjustedData(pData);
418  CFloatMatrix covarianceMatrix(dimension, dimension);
419  CFloatMatrix eigenValues(dimension, dimension);
420  CFloatMatrix eigenVectors(dimension, dimension);
421 
422  printf("subtracting mean from columns...\n");
423  LinearAlgebra::SubtractMeanFromColumns(pData, &adjustedData);
424 
425  printf("calculating covariance matrix...\n");
426  LinearAlgebraCV::SelfProduct(&adjustedData, &covarianceMatrix);
427 
428  printf("calculating SVD on %ix%i matrix...\n", dimension, dimension);
429  LinearAlgebraCV::SVD(&covarianceMatrix, &eigenValues, pTransformationMatrix, 0, true, true);
430 
431  printf("SVD calculated\n");
432 
433  for (int i = 0; i < dimension; i++)
434  pEigenValues->data[i] = eigenValues.data[i * dimension + i];
435 }
436 
437 bool LinearAlgebraCV::DetermineAffineTransformation(const Vec2d *pSourcePoints, const Vec2d *pTargetPoints, int nPoints, Mat3d &A, bool bUseSVD)
438 {
439  if (nPoints < 3)
440  {
441  printf("error: not enough input point pairs for LinearAlgebraCV::DetermineAffineTransformation (must be at least 3)\n");
442  return false;
443  }
444 
445  CFloatMatrix M(6, 2 * nPoints);
446  CFloatVector b(2 * nPoints);
447 
448  float *data = M.data;
449 
450  for (int i = 0, offset = 0; i < nPoints; i++, offset += 12)
451  {
452  data[offset] = pSourcePoints[i].x;
453  data[offset + 1] = pSourcePoints[i].y;
454  data[offset + 2] = 1;
455  data[offset + 3] = 0;
456  data[offset + 4] = 0;
457  data[offset + 5] = 0;
458 
459  data[offset + 6] = 0;
460  data[offset + 7] = 0;
461  data[offset + 8] = 0;
462  data[offset + 9] = pSourcePoints[i].x;
463  data[offset + 10] = pSourcePoints[i].y;
464  data[offset + 11] = 1;
465 
466  const int index = 2 * i;
467  b.data[index] = pTargetPoints[i].x;
468  b.data[index + 1] = pTargetPoints[i].y;
469  }
470 
471  CFloatVector x(6);
472 
473  if (bUseSVD)
475  else
477 
478  Math3d::SetMat(A, x.data[0], x.data[1], x.data[2], x.data[3], x.data[4], x.data[5], 0, 0, 1);
479 
480  return true;
481 }
482 
483 bool LinearAlgebraCV::DetermineHomography(const Vec2d *pSourcePoints, const Vec2d *pTargetPoints, int nPoints, Mat3d &A, bool bUseSVD)
484 {
485  if (nPoints < 4)
486  {
487  printf("error: not enough input point pairs for LinearAlgebraCV::DetermineHomography (must be at least 4)\n");
488  return false;
489  }
490 
491  // this least squares problem becomes numerically instable when
492  // using float instead of double!!
493  CDoubleMatrix M(8, 2 * nPoints);
494  CDoubleVector b(2 * nPoints);
495 
496  double *data = M.data;
497 
498  for (int i = 0, offset = 0; i < nPoints; i++, offset += 16)
499  {
500  data[offset] = pSourcePoints[i].x;
501  data[offset + 1] = pSourcePoints[i].y;
502  data[offset + 2] = 1;
503  data[offset + 3] = 0;
504  data[offset + 4] = 0;
505  data[offset + 5] = 0;
506  data[offset + 6] = -pSourcePoints[i].x * pTargetPoints[i].x;
507  data[offset + 7] = -pSourcePoints[i].y * pTargetPoints[i].x;
508 
509  data[offset + 8] = 0;
510  data[offset + 9] = 0;
511  data[offset + 10] = 0;
512  data[offset + 11] = pSourcePoints[i].x;
513  data[offset + 12] = pSourcePoints[i].y;
514  data[offset + 13] = 1;
515  data[offset + 14] = -pSourcePoints[i].x * pTargetPoints[i].y;
516  data[offset + 15] = -pSourcePoints[i].y * pTargetPoints[i].y;
517 
518  const int index = 2 * i;
519  b.data[index] = pTargetPoints[i].x;
520  b.data[index + 1] = pTargetPoints[i].y;
521  }
522 
523  CDoubleVector x(8);
524 
525  if (bUseSVD)
527  else
529 
530  Math3d::SetMat(A, float(x.data[0]), float(x.data[1]), float(x.data[2]), float(x.data[3]), float(x.data[4]), float(x.data[5]), float(x.data[6]), float(x.data[7]), 1);
531 
532  return true;
533 }
534 
535 
536 
537 
538 
539 // copy & paste implementation for CDoubleMatrix and CDoubleVector
540 // not nice, but i don't like templates.
541 
543 {
544  if (A->columns != x->dimension)
545  {
546  printf("error: A, b, x do not match LinearAlgebraCV::SolveLinearLeastSquaresHomogeneousSVD");
547  return;
548  }
549 
550  if (A->rows < A->columns)
551  {
552  printf("error: equation system is underdetermined in LinearAlgebraCV::SolveLinearLeastSquaresHomogeneousSVD\n");
553  return;
554  }
555 
556  const int m = A->rows;
557  const int n = A->columns;
558 
559  CDoubleMatrix W(n, m), V(n, n);
560 
561  SVD(A, &W, 0, &V, false, false, false);
562 
563  for (int i = 0, offset = n - 1; i < n; i++, offset += n)
564  x->data[i] = V.data[offset];
565 }
566 
568 {
569  if (A->columns != x->dimension || A->rows != b->dimension)
570  {
571  printf("error: A, b, x do not match LinearAlgebraCV::SolveLinearLeastSquaresSVD");
572  return;
573  }
574 
575  if (A->rows < A->columns)
576  {
577  printf("error: equation system is underdetermined in LinearAlgebraCV::SolveLinearLeastSquaresSVD\n");
578  return;
579  }
580 
581 #if 0
582  CvMat *AA = cvCreateMat(A->rows, A->columns, CV_64FC1);
583  CvMat *BB = cvCreateMat(b->dimension, 1, CV_64FC1);
584  CvMat *XX = cvCreateMat(x->dimension, 1, CV_64FC1);
585 
586  int i;
587 
588  for (i = 0; i < A->rows * A->columns; i++)
589  AA->data.db[i] = A->data[i];
590 
591  for (i = 0; i < b->dimension; i++)
592  BB->data.db[i] = b->data[i];
593 
594  cvSolve(AA, BB, XX, CV_SVD);
595 
596  for (int k = 0; k < 8; k++)
597  x->data[k] = XX->data.db[k];
598 
599  cvReleaseMat(&AA);
600  cvReleaseMat(&BB);
601  cvReleaseMat(&XX);
602 #else
603  CDoubleMatrix A_(A->rows, A->columns);
605  LinearAlgebra::MulMatVec(&A_, b, x);
606 #endif
607 }
608 
610 {
611  if (A->columns != x->dimension || A->rows != b->dimension)
612  {
613  printf("error: A, b, x do not match LinearAlgebraCV::SolveLinearLeastSquaresSimple");
614  return;
615  }
616 
617  if (A->rows < A->columns)
618  {
619  printf("error: equation system is underdetermined in LinearAlgebraCV::SolveLinearLeastSquaresSimple\n");
620  return;
621  }
622 
623  CDoubleMatrix A_(A->rows, A->columns);
625  LinearAlgebra::MulMatVec(&A_, b, x);
626 }
627 
629 {
630  if (pInputMatrix->columns != pResultMatrix->rows || pInputMatrix->rows != pResultMatrix->columns)
631  {
632  printf("error: input and output matrix do not match LinearAlgebraCV::CalculatePseudoInverseSVD");
633  return;
634  }
635 
636  // algorithm:
637  // 1: compute U * W * V^T = A
638  // 2: compute W' = W^T with all non-zero values inverted
639  // 3: compute V * W' * U^T (=pseudoinverse of A)
640 
641  const CDoubleMatrix *A = pInputMatrix;
642  const int m = pInputMatrix->rows;
643  const int n = pInputMatrix->columns;
644 
645  CDoubleMatrix W(n, m), WT(m, n), UT(m, m), V(n, n);
646 
647  // calculate SVD
648  SVD(A, &W, &UT, &V, false, true, false);
649 
650  // transpose middle diagonal matrix
651  Transpose(&W, &WT);
652 
653  const int min = MY_MIN(m, n);
654 
655  int i;
656  double dThreshold = 0.0;
657 
658  for(i = 0; i < min; i++)
659  dThreshold += WT(i, i);
660 
661  dThreshold *= 2 * DBL_EPSILON;
662 
663  // invert non-zero values (along diagonal)
664  for (i = 0; i < min; i++)
665  if (WT(i, i) < dThreshold)
666  WT(i, i) = 0;
667  else
668  WT(i, i) = 1.0 / WT(i, i);
669 
670  // calculate pseudoinverse
671  CDoubleMatrix temp(m, n);
672  Multiply(&V, &WT, &temp);
673  Multiply(&temp, &UT, pResultMatrix);
674 }
675 
677 {
678  if (pInputMatrix->columns != pResultMatrix->rows || pInputMatrix->rows != pResultMatrix->columns)
679  {
680  printf("error: input and output matrix do not match LinearAlgebraCV::CalculatePseudoInverseSimple");
681  return;
682  }
683 
684  // algorithm:
685  // compute (A * A^T)^-1 * A^T
686 
687  const CDoubleMatrix *A = pInputMatrix;
688  const int m = pInputMatrix->rows;
689  const int n = pInputMatrix->columns;
690 
691  CDoubleMatrix AT(m, n), ATA(n, n), ATA_inverted(n, n);
692 
693  Transpose(A, &AT);
694  Multiply(&AT, A, &ATA);
695  Invert(&ATA, &ATA_inverted);
696  Multiply(&ATA_inverted, &AT, pResultMatrix);
697 }
698 
699 void LinearAlgebraCV::Invert(const CDoubleMatrix *pInputMatrix, const CDoubleMatrix *pResultMatrix)
700 {
701  if (pInputMatrix->columns != pInputMatrix->rows)
702  {
703  printf("error: input is not square matrix in LinearAlgebraCV::Invert");
704  return;
705  }
706 
707  if (pInputMatrix->columns != pResultMatrix->columns || pInputMatrix->rows != pResultMatrix->rows)
708  {
709  printf("error: input and output matrix are not the same in LinearAlgebraCV::Invert");
710  return;
711  }
712 
713  CvMat inputMatrix = cvMat(pInputMatrix->rows, pInputMatrix->columns, CV_64FC1, pInputMatrix->data);
714  CvMat resultMatrix = cvMat(pResultMatrix->rows, pResultMatrix->columns, CV_64FC1, pResultMatrix->data);
715 
716  cvInvert(&inputMatrix, &resultMatrix);
717 }
718 
719 void LinearAlgebraCV::Transpose(const CDoubleMatrix *pInputMatrix, const CDoubleMatrix *pResultMatrix)
720 {
721  if (pInputMatrix->columns != pResultMatrix->rows || pInputMatrix->rows != pResultMatrix->columns)
722  {
723  printf("error: input and output matrix do not match LinearAlgebraCV::Transpose");
724  return;
725  }
726 
727  CvMat inputMatrix = cvMat(pInputMatrix->rows, pInputMatrix->columns, CV_64FC1, pInputMatrix->data);
728  CvMat resultMatrix = cvMat(pResultMatrix->rows, pResultMatrix->columns, CV_64FC1, pResultMatrix->data);
729 
730  cvTranspose(&inputMatrix, &resultMatrix);
731 }
732 
733 void LinearAlgebraCV::Multiply(const CDoubleMatrix *A, const CDoubleMatrix *B, CDoubleMatrix *pResultMatrix, bool bTransposeB)
734 {
735  if (!bTransposeB && (A->columns != B->rows || pResultMatrix->columns != B->columns || pResultMatrix->rows != A->rows))
736  {
737  printf("error: matrices A, B, and pResultMatrix do not satisfy requirements for LinearAlgebraCV::Multiply\n");
738  return;
739  }
740 
741  if (bTransposeB && (A->columns != B->columns || pResultMatrix->columns != B->rows || pResultMatrix->rows != A->rows))
742  {
743  printf("error: matrices A, B, and pResultMatrix do not satisfy requirements for LinearAlgebraCV::Multiply\n");
744  return;
745  }
746 
747  int flags = 0;
748 
749  if (bTransposeB)
750  flags = CV_GEMM_B_T;
751 
752  CvMat matrixA = cvMat(A->rows, A->columns, CV_64FC1, A->data);
753  CvMat matrixB = cvMat(B->rows, B->columns, CV_64FC1, B->data);
754  CvMat result_matrix = cvMat(pResultMatrix->rows, pResultMatrix->columns, CV_64FC1, pResultMatrix->data);
755 
756  cvGEMM(&matrixA, &matrixB, 1, 0, 1, &result_matrix, flags);
757 }
758 
759 void LinearAlgebraCV::SVD(const CDoubleMatrix *A, CDoubleMatrix *W, CDoubleMatrix *U, CDoubleMatrix *V, bool bAllowModifyA, bool bReturnUTransposed, bool bReturnVTransposed)
760 {
761  const int columns = A->columns;
762  const int rows = A->rows;
763 
764  if (W->columns != columns || W->rows != rows)
765  {
766  printf("error: W should have %i columns and %i rows for LinearAlgebra::SVD\n", columns, rows);
767  return;
768  }
769 
770  if (U && (U->columns != rows || U->rows != rows))
771  {
772  printf("error: U should have %i columns and %i rows for LinearAlgebra::SVD\n", rows, rows);
773  return;
774  }
775 
776  if (V && (V->columns != columns || V->rows != columns))
777  {
778  printf("error: V should have %i columns and %i rows for LinearAlgebra::SVD\n", columns, columns);
779  return;
780  }
781 
782  int flags = 0;
783 
784  if (bAllowModifyA)
785  flags |= CV_SVD_MODIFY_A;
786 
787  if (bReturnUTransposed)
788  flags |= CV_SVD_U_T;
789 
790  if (bReturnVTransposed)
791  flags |= CV_SVD_V_T;
792 
793  CvMat matrixA = cvMat(rows, columns, CV_64FC1, A->data);
794  CvMat matrixW = cvMat(rows, columns, CV_64FC1, W->data);
795 
796  if (U && V)
797  {
798  CvMat matrixU = cvMat(rows, rows, CV_64FC1, U->data);
799  CvMat matrixV = cvMat(columns, columns, CV_64FC1, V->data);
800 
801  cvSVD(&matrixA, &matrixW, &matrixU, &matrixV, flags);
802  }
803  else if (U)
804  {
805  CvMat matrixU = cvMat(rows, rows, CV_64FC1, U->data);
806 
807  cvSVD(&matrixA, &matrixW, &matrixU, 0, flags);
808  }
809  else if (V)
810  {
811  CvMat matrixV = cvMat(columns, columns, CV_64FC1, V->data);
812 
813  cvSVD(&matrixA, &matrixW, 0, &matrixV, flags);
814  }
815  else
816  {
817  cvSVD(&matrixA, &matrixW, 0, 0, flags);
818  }
819 }
void SolveLinearLeastSquaresSimple(const CFloatMatrix *A, const CFloatVector *b, CFloatVector *x)
void Transpose(const CFloatMatrix *A, const CFloatMatrix *pResultMatrix)
double * data
Definition: DoubleMatrix.h:85
GLuint GLenum matrix
Definition: glext.h:5683
float y
Definition: Math2d.h:84
GLenum GLsizei n
Definition: glext.h:4209
bool Invert(const CByteImage *pInputImage, CByteImage *pOutputImage)
Calculates the inverted image of a CByteImage and writes the result to a CByteImage.
bool CalculatePseudoInverseSimple(const CFloatMatrix *pInputMatrix, CFloatMatrix *pResultMatrix)
GLintptr offset
Definition: glext.h:3389
void CalculateCovarianceMatrix(const CFloatMatrix *pMatrix, CFloatMatrix *pCovarianceMatrix)
#define CV_SVD_V_T
Definition: SVD.cpp:117
Data structure for the representation of a vector of values of the data type double.
Definition: DoubleVector.h:54
void SVD(const CFloatMatrix *A, CFloatMatrix *W, CFloatMatrix *U=0, CFloatMatrix *V=0, bool bAllowModifyA=false, bool bReturnUTransposed=false, bool bReturnVTransposed=false)
Definition: SVD.cpp:1660
GLenum GLint x
Definition: glext.h:3125
float x
Definition: Math2d.h:84
GLuint index
Definition: glext.h:3500
void Invert(const CFloatMatrix *A, const CFloatMatrix *pResultMatrix)
#define CV_SVD_MODIFY_A
Definition: SVD.cpp:115
float * data
Definition: FloatVector.h:78
Data structure for the representation of a matrix of values of the data type float.
Definition: FloatMatrix.h:56
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3129
bool DetermineHomography(const Vec2d *pSourcePoints, const Vec2d *pTargetPoints, int nPoints, Mat3d &A, bool bUseSVD=false)
void SubtractMeanFromColumns(const CFloatMatrix *pMatrix, CFloatMatrix *pResultMatrix)
void MulMatVec(const CFloatMatrix *pMatrix, const CFloatVector *pVector, CFloatVector *pResultVector)
#define CV_SVD_U_T
Definition: SVD.cpp:116
float * data
Definition: FloatMatrix.h:91
double * data
Definition: DoubleVector.h:79
void Multiply(const CFloatMatrix *A, const CFloatMatrix *B, CFloatMatrix *pResultMatrix, bool bTransposeB=false)
GLubyte GLubyte b
Definition: glext.h:5166
#define MY_MIN(a, b)
Definition: helpers.h:64
void CalculatePseudoInverseSVD(const CFloatMatrix *pInputMatrix, CFloatMatrix *pOutputMatrix)
Data structure for the representation of a vector of values of the data type float.
Definition: FloatVector.h:53
void SelfProduct(const CFloatMatrix *pMatrix, CFloatMatrix *pResultMatrix, bool bTransposeSecond=false)
void SVD(const CFloatMatrix *A, CFloatMatrix *W, CFloatMatrix *U=0, CFloatMatrix *V=0, bool bAllowModifyA=false, bool bReturnUTransposed=false, bool bReturnVTransposed=false)
Data structure for the representation of a 2D vector.
Definition: Math2d.h:82
Data structure for the representation of a matrix of values of the data type double.
Definition: DoubleMatrix.h:54
void Transpose(const CFloatMatrix *pMatrix, CFloatMatrix *pResultMatrix)
void SolveLinearLeastSquaresSVD(const CFloatMatrix *A, const CFloatVector *b, CFloatVector *x)
void SetMat(Mat3d &matrix, float r1, float r2, float r3, float r4, float r5, float r6, float r7, float r8, float r9)
Definition: Math3d.cpp:257
bool DetermineAffineTransformation(const Vec2d *pSourcePoints, const Vec2d *pTargetPoints, int nPoints, Mat3d &A, bool bUseSVD=false)
Data structure for the representation of a 3x3 matrix.
Definition: Math3d.h:93
void SolveLinearLeastSquaresHomogeneousSVD(const CFloatMatrix *A, CFloatVector *x)
void CalculatePseudoInverseSVD(const CFloatMatrix *pInputMatrix, CFloatMatrix *pOutputMatrix)
void PCA(const CFloatMatrix *pData, CFloatMatrix *pTransformationMatrix, CFloatMatrix *pTransformedData, int nTargetDimension)
void CalculatePseudoInverseSimple(const CFloatMatrix *pInputMatrix, CFloatMatrix *pResultMatrix)


asr_ivt
Author(s): Allgeyer Tobias, Hutmacher Robin, Kleinert Daniel, Meißner Pascal, Scholz Jonas, Stöckle Patrick
autogenerated on Mon Dec 2 2019 03:47:28