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