lda.cpp

00001 /*
00002  * Copyright (c) 2011. Philipp Wagner <bytefish[at]gmx[dot]de>.
00003  * Released to public domain under terms of the BSD Simplified license.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *   * Redistributions of source code must retain the above copyright
00008  *     notice, this list of conditions and the following disclaimer.
00009  *   * Redistributions in binary form must reproduce the above copyright
00010  *     notice, this list of conditions and the following disclaimer in the
00011  *     documentation and/or other materials provided with the distribution.
00012  *   * Neither the name of the organization nor the names of its contributors
00013  *     may be used to endorse or promote products derived from this software
00014  *     without specific prior written permission.
00015  *
00016  *   See <http://www.opensource.org/licenses/bsd-license>
00017  */
00018 
00019 #include "precomp.hpp"
00020 #include <iostream>
00021 #include <map>
00022 #include <set>
00023 
00024 namespace cv
00025 {
00026 
00027 // Removes duplicate elements in a given vector.
00028 template<typename _Tp>
00029 inline std::vector<_Tp> remove_dups(const std::vector<_Tp>& src) {
00030     typedef typename std::set<_Tp>::const_iterator constSetIterator;
00031     typedef typename std::vector<_Tp>::const_iterator constVecIterator;
00032     std::set<_Tp> set_elems;
00033     for (constVecIterator it = src.begin(); it != src.end(); ++it)
00034         set_elems.insert(*it);
00035     std::vector<_Tp> elems;
00036     for (constSetIterator it = set_elems.begin(); it != set_elems.end(); ++it)
00037         elems.push_back(*it);
00038     return elems;
00039 }
00040 
00041 static Mat argsort(InputArray _src, bool ascending=true)
00042 {
00043     Mat src = _src.getMat();
00044     if (src.rows != 1 && src.cols != 1) {
00045         String error_message = "Wrong shape of input matrix! Expected a matrix with one row or column.";
00046         CV_Error(Error::StsBadArg, error_message);
00047     }
00048     int flags = SORT_EVERY_ROW | (ascending ? SORT_ASCENDING : SORT_DESCENDING);
00049     Mat sorted_indices;
00050     sortIdx(src.reshape(1,1),sorted_indices,flags);
00051     return sorted_indices;
00052 }
00053 
00054 static Mat asRowMatrix(InputArrayOfArrays src, int rtype, double alpha=1, double beta=0) {
00055     // make sure the input data is a vector of matrices or vector of vector
00056     if(src.kind() != _InputArray::STD_VECTOR_MAT && src.kind() != _InputArray::STD_VECTOR_VECTOR) {
00057         String error_message = "The data is expected as InputArray::STD_VECTOR_MAT (a std::vector<Mat>) or _InputArray::STD_VECTOR_VECTOR (a std::vector< std::vector<...> >).";
00058         CV_Error(Error::StsBadArg, error_message);
00059     }
00060     // number of samples
00061     size_t n = src.total();
00062     // return empty matrix if no matrices given
00063     if(n == 0)
00064         return Mat();
00065     // dimensionality of (reshaped) samples
00066     size_t d = src.getMat(0).total();
00067     // create data matrix
00068     Mat data((int)n, (int)d, rtype);
00069     // now copy data
00070     for(int i = 0; i < (int)n; i++) {
00071         // make sure data can be reshaped, throw exception if not!
00072         if(src.getMat(i).total() != d) {
00073             String error_message = format("Wrong number of elements in matrix #%d! Expected %d was %d.", i, (int)d, (int)src.getMat(i).total());
00074             CV_Error(Error::StsBadArg, error_message);
00075         }
00076         // get a hold of the current row
00077         Mat xi = data.row(i);
00078         // make reshape happy by cloning for non-continuous matrices
00079         if(src.getMat(i).isContinuous()) {
00080             src.getMat(i).reshape(1, 1).convertTo(xi, rtype, alpha, beta);
00081         } else {
00082             src.getMat(i).clone().reshape(1, 1).convertTo(xi, rtype, alpha, beta);
00083         }
00084     }
00085     return data;
00086 }
00087 
00088 static void sortMatrixColumnsByIndices(InputArray _src, InputArray _indices, OutputArray _dst) {
00089     if(_indices.getMat().type() != CV_32SC1) {
00090         CV_Error(Error::StsUnsupportedFormat, "cv::sortColumnsByIndices only works on integer indices!");
00091     }
00092     Mat src = _src.getMat();
00093     std::vector<int> indices = _indices.getMat();
00094     _dst.create(src.rows, src.cols, src.type());
00095     Mat dst = _dst.getMat();
00096     for(size_t idx = 0; idx < indices.size(); idx++) {
00097         Mat originalCol = src.col(indices[idx]);
00098         Mat sortedCol = dst.col((int)idx);
00099         originalCol.copyTo(sortedCol);
00100     }
00101 }
00102 
00103 static Mat sortMatrixColumnsByIndices(InputArray src, InputArray indices) {
00104     Mat dst;
00105     sortMatrixColumnsByIndices(src, indices, dst);
00106     return dst;
00107 }
00108 
00109 
00110 template<typename _Tp> static bool
00111 isSymmetric_(InputArray src) {
00112     Mat _src = src.getMat();
00113     if(_src.cols != _src.rows)
00114         return false;
00115     for (int i = 0; i < _src.rows; i++) {
00116         for (int j = 0; j < _src.cols; j++) {
00117             _Tp a = _src.at<_Tp> (i, j);
00118             _Tp b = _src.at<_Tp> (j, i);
00119             if (a != b) {
00120                 return false;
00121             }
00122         }
00123     }
00124     return true;
00125 }
00126 
00127 template<typename _Tp> static bool
00128 isSymmetric_(InputArray src, double eps) {
00129     Mat _src = src.getMat();
00130     if(_src.cols != _src.rows)
00131         return false;
00132     for (int i = 0; i < _src.rows; i++) {
00133         for (int j = 0; j < _src.cols; j++) {
00134             _Tp a = _src.at<_Tp> (i, j);
00135             _Tp b = _src.at<_Tp> (j, i);
00136             if (std::abs(a - b) > eps) {
00137                 return false;
00138             }
00139         }
00140     }
00141     return true;
00142 }
00143 
00144 static bool isSymmetric(InputArray src, double eps=1e-16)
00145 {
00146     Mat m = src.getMat();
00147     switch (m.type()) {
00148         case CV_8SC1: return isSymmetric_<char>(m); break;
00149         case CV_8UC1:
00150             return isSymmetric_<unsigned char>(m); break;
00151         case CV_16SC1:
00152             return isSymmetric_<short>(m); break;
00153         case CV_16UC1:
00154             return isSymmetric_<unsigned short>(m); break;
00155         case CV_32SC1:
00156             return isSymmetric_<int>(m); break;
00157         case CV_32FC1:
00158             return isSymmetric_<float>(m, eps); break;
00159         case CV_64FC1:
00160             return isSymmetric_<double>(m, eps); break;
00161         default:
00162             break;
00163     }
00164     return false;
00165 }
00166 
00167 
00168 //------------------------------------------------------------------------------
00169 // cv::subspaceProject
00170 //------------------------------------------------------------------------------
00171 Mat LDA::subspaceProject(InputArray _W, InputArray _mean, InputArray _src) {
00172     // get data matrices
00173     Mat W = _W.getMat();
00174     Mat mean = _mean.getMat();
00175     Mat src = _src.getMat();
00176     // get number of samples and dimension
00177     int n = src.rows;
00178     int d = src.cols;
00179     // make sure the data has the correct shape
00180     if(W.rows != d) {
00181         String error_message = format("Wrong shapes for given matrices. Was size(src) = (%d,%d), size(W) = (%d,%d).", src.rows, src.cols, W.rows, W.cols);
00182         CV_Error(Error::StsBadArg, error_message);
00183     }
00184     // make sure mean is correct if not empty
00185     if(!mean.empty() && (mean.total() != (size_t) d)) {
00186         String error_message = format("Wrong mean shape for the given data matrix. Expected %d, but was %d.", d, mean.total());
00187         CV_Error(Error::StsBadArg, error_message);
00188     }
00189     // create temporary matrices
00190     Mat X, Y;
00191     // make sure you operate on correct type
00192     src.convertTo(X, W.type());
00193     // safe to do, because of above assertion
00194     if(!mean.empty()) {
00195         for(int i=0; i<n; i++) {
00196             Mat r_i = X.row(i);
00197             subtract(r_i, mean.reshape(1,1), r_i);
00198         }
00199     }
00200     // finally calculate projection as Y = (X-mean)*W
00201     gemm(X, W, 1.0, Mat(), 0.0, Y);
00202     return Y;
00203 }
00204 
00205 //------------------------------------------------------------------------------
00206 // cv::subspaceReconstruct
00207 //------------------------------------------------------------------------------
00208 Mat LDA::subspaceReconstruct(InputArray _W, InputArray _mean, InputArray _src)
00209 {
00210     // get data matrices
00211     Mat W = _W.getMat();
00212     Mat mean = _mean.getMat();
00213     Mat src = _src.getMat();
00214     // get number of samples and dimension
00215     int n = src.rows;
00216     int d = src.cols;
00217     // make sure the data has the correct shape
00218     if(W.cols != d) {
00219         String error_message = format("Wrong shapes for given matrices. Was size(src) = (%d,%d), size(W) = (%d,%d).", src.rows, src.cols, W.rows, W.cols);
00220         CV_Error(Error::StsBadArg, error_message);
00221     }
00222     // make sure mean is correct if not empty
00223     if(!mean.empty() && (mean.total() != (size_t) W.rows)) {
00224         String error_message = format("Wrong mean shape for the given eigenvector matrix. Expected %d, but was %d.", W.cols, mean.total());
00225         CV_Error(Error::StsBadArg, error_message);
00226     }
00227     // initialize temporary matrices
00228     Mat X, Y;
00229     // copy data & make sure we are using the correct type
00230     src.convertTo(Y, W.type());
00231     // calculate the reconstruction
00232     gemm(Y, W, 1.0, Mat(), 0.0, X, GEMM_2_T);
00233     // safe to do because of above assertion
00234     if(!mean.empty()) {
00235         for(int i=0; i<n; i++) {
00236             Mat r_i = X.row(i);
00237             add(r_i, mean.reshape(1,1), r_i);
00238         }
00239     }
00240     return X;
00241 }
00242 
00243 
00244 class EigenvalueDecomposition {
00245 private:
00246 
00247     // Holds the data dimension.
00248     int n;
00249 
00250     // Stores real/imag part of a complex division.
00251     double cdivr, cdivi;
00252 
00253     // Pointer to internal memory.
00254     double *d, *e, *ort;
00255     double **V, **H;
00256 
00257     // Holds the computed eigenvalues.
00258     Mat _eigenvalues;
00259 
00260     // Holds the computed eigenvectors.
00261     Mat _eigenvectors;
00262 
00263     // Allocates memory.
00264     template<typename _Tp>
00265     _Tp *alloc_1d(int m) {
00266         return new _Tp[m];
00267     }
00268 
00269     // Allocates memory.
00270     template<typename _Tp>
00271     _Tp *alloc_1d(int m, _Tp val) {
00272         _Tp *arr = alloc_1d<_Tp> (m);
00273         for (int i = 0; i < m; i++)
00274             arr[i] = val;
00275         return arr;
00276     }
00277 
00278     // Allocates memory.
00279     template<typename _Tp>
00280     _Tp **alloc_2d(int m, int _n) {
00281         _Tp **arr = new _Tp*[m];
00282         for (int i = 0; i < m; i++)
00283             arr[i] = new _Tp[_n];
00284         return arr;
00285     }
00286 
00287     // Allocates memory.
00288     template<typename _Tp>
00289     _Tp **alloc_2d(int m, int _n, _Tp val) {
00290         _Tp **arr = alloc_2d<_Tp> (m, _n);
00291         for (int i = 0; i < m; i++) {
00292             for (int j = 0; j < _n; j++) {
00293                 arr[i][j] = val;
00294             }
00295         }
00296         return arr;
00297     }
00298 
00299     void cdiv(double xr, double xi, double yr, double yi) {
00300         double r, dv;
00301         if (std::abs(yr) > std::abs(yi)) {
00302             r = yi / yr;
00303             dv = yr + r * yi;
00304             cdivr = (xr + r * xi) / dv;
00305             cdivi = (xi - r * xr) / dv;
00306         } else {
00307             r = yr / yi;
00308             dv = yi + r * yr;
00309             cdivr = (r * xr + xi) / dv;
00310             cdivi = (r * xi - xr) / dv;
00311         }
00312     }
00313 
00314     // Nonsymmetric reduction from Hessenberg to real Schur form.
00315 
00316     void hqr2() {
00317 
00318         //  This is derived from the Algol procedure hqr2,
00319         //  by Martin and Wilkinson, Handbook for Auto. Comp.,
00320         //  Vol.ii-Linear Algebra, and the corresponding
00321         //  Fortran subroutine in EISPACK.
00322 
00323         // Initialize
00324         int nn = this->n;
00325         int n1 = nn - 1;
00326         int low = 0;
00327         int high = nn - 1;
00328         double eps = std::pow(2.0, -52.0);
00329         double exshift = 0.0;
00330         double p = 0, q = 0, r = 0, s = 0, z = 0, t, w, x, y;
00331 
00332         // Store roots isolated by balanc and compute matrix norm
00333 
00334         double norm = 0.0;
00335         for (int i = 0; i < nn; i++) {
00336             if (i < low || i > high) {
00337                 d[i] = H[i][i];
00338                 e[i] = 0.0;
00339             }
00340             for (int j = std::max(i - 1, 0); j < nn; j++) {
00341                 norm = norm + std::abs(H[i][j]);
00342             }
00343         }
00344 
00345         // Outer loop over eigenvalue index
00346         int iter = 0;
00347         while (n1 >= low) {
00348 
00349             // Look for single small sub-diagonal element
00350             int l = n1;
00351             while (l > low) {
00352                 s = std::abs(H[l - 1][l - 1]) + std::abs(H[l][l]);
00353                 if (s == 0.0) {
00354                     s = norm;
00355                 }
00356                 if (std::abs(H[l][l - 1]) < eps * s) {
00357                     break;
00358                 }
00359                 l--;
00360             }
00361 
00362             // Check for convergence
00363             // One root found
00364 
00365             if (l == n1) {
00366                 H[n1][n1] = H[n1][n1] + exshift;
00367                 d[n1] = H[n1][n1];
00368                 e[n1] = 0.0;
00369                 n1--;
00370                 iter = 0;
00371 
00372                 // Two roots found
00373 
00374             } else if (l == n1 - 1) {
00375                 w = H[n1][n1 - 1] * H[n1 - 1][n1];
00376                 p = (H[n1 - 1][n1 - 1] - H[n1][n1]) / 2.0;
00377                 q = p * p + w;
00378                 z = std::sqrt(std::abs(q));
00379                 H[n1][n1] = H[n1][n1] + exshift;
00380                 H[n1 - 1][n1 - 1] = H[n1 - 1][n1 - 1] + exshift;
00381                 x = H[n1][n1];
00382 
00383                 // Real pair
00384 
00385                 if (q >= 0) {
00386                     if (p >= 0) {
00387                         z = p + z;
00388                     } else {
00389                         z = p - z;
00390                     }
00391                     d[n1 - 1] = x + z;
00392                     d[n1] = d[n1 - 1];
00393                     if (z != 0.0) {
00394                         d[n1] = x - w / z;
00395                     }
00396                     e[n1 - 1] = 0.0;
00397                     e[n1] = 0.0;
00398                     x = H[n1][n1 - 1];
00399                     s = std::abs(x) + std::abs(z);
00400                     p = x / s;
00401                     q = z / s;
00402                     r = std::sqrt(p * p + q * q);
00403                     p = p / r;
00404                     q = q / r;
00405 
00406                     // Row modification
00407 
00408                     for (int j = n1 - 1; j < nn; j++) {
00409                         z = H[n1 - 1][j];
00410                         H[n1 - 1][j] = q * z + p * H[n1][j];
00411                         H[n1][j] = q * H[n1][j] - p * z;
00412                     }
00413 
00414                     // Column modification
00415 
00416                     for (int i = 0; i <= n1; i++) {
00417                         z = H[i][n1 - 1];
00418                         H[i][n1 - 1] = q * z + p * H[i][n1];
00419                         H[i][n1] = q * H[i][n1] - p * z;
00420                     }
00421 
00422                     // Accumulate transformations
00423 
00424                     for (int i = low; i <= high; i++) {
00425                         z = V[i][n1 - 1];
00426                         V[i][n1 - 1] = q * z + p * V[i][n1];
00427                         V[i][n1] = q * V[i][n1] - p * z;
00428                     }
00429 
00430                     // Complex pair
00431 
00432                 } else {
00433                     d[n1 - 1] = x + p;
00434                     d[n1] = x + p;
00435                     e[n1 - 1] = z;
00436                     e[n1] = -z;
00437                 }
00438                 n1 = n1 - 2;
00439                 iter = 0;
00440 
00441                 // No convergence yet
00442 
00443             } else {
00444 
00445                 // Form shift
00446 
00447                 x = H[n1][n1];
00448                 y = 0.0;
00449                 w = 0.0;
00450                 if (l < n1) {
00451                     y = H[n1 - 1][n1 - 1];
00452                     w = H[n1][n1 - 1] * H[n1 - 1][n1];
00453                 }
00454 
00455                 // Wilkinson's original ad hoc shift
00456 
00457                 if (iter == 10) {
00458                     exshift += x;
00459                     for (int i = low; i <= n1; i++) {
00460                         H[i][i] -= x;
00461                     }
00462                     s = std::abs(H[n1][n1 - 1]) + std::abs(H[n1 - 1][n1 - 2]);
00463                     x = y = 0.75 * s;
00464                     w = -0.4375 * s * s;
00465                 }
00466 
00467                 // MATLAB's new ad hoc shift
00468 
00469                 if (iter == 30) {
00470                     s = (y - x) / 2.0;
00471                     s = s * s + w;
00472                     if (s > 0) {
00473                         s = std::sqrt(s);
00474                         if (y < x) {
00475                             s = -s;
00476                         }
00477                         s = x - w / ((y - x) / 2.0 + s);
00478                         for (int i = low; i <= n1; i++) {
00479                             H[i][i] -= s;
00480                         }
00481                         exshift += s;
00482                         x = y = w = 0.964;
00483                     }
00484                 }
00485 
00486                 iter = iter + 1; // (Could check iteration count here.)
00487 
00488                 // Look for two consecutive small sub-diagonal elements
00489                 int m = n1 - 2;
00490                 while (m >= l) {
00491                     z = H[m][m];
00492                     r = x - z;
00493                     s = y - z;
00494                     p = (r * s - w) / H[m + 1][m] + H[m][m + 1];
00495                     q = H[m + 1][m + 1] - z - r - s;
00496                     r = H[m + 2][m + 1];
00497                     s = std::abs(p) + std::abs(q) + std::abs(r);
00498                     p = p / s;
00499                     q = q / s;
00500                     r = r / s;
00501                     if (m == l) {
00502                         break;
00503                     }
00504                     if (std::abs(H[m][m - 1]) * (std::abs(q) + std::abs(r)) < eps * (std::abs(p)
00505                                                                                      * (std::abs(H[m - 1][m - 1]) + std::abs(z) + std::abs(
00506                                                                                                                                            H[m + 1][m + 1])))) {
00507                         break;
00508                     }
00509                     m--;
00510                 }
00511 
00512                 for (int i = m + 2; i <= n1; i++) {
00513                     H[i][i - 2] = 0.0;
00514                     if (i > m + 2) {
00515                         H[i][i - 3] = 0.0;
00516                     }
00517                 }
00518 
00519                 // Double QR step involving rows l:n and columns m:n
00520 
00521                 for (int k = m; k <= n1 - 1; k++) {
00522                     bool notlast = (k != n1 - 1);
00523                     if (k != m) {
00524                         p = H[k][k - 1];
00525                         q = H[k + 1][k - 1];
00526                         r = (notlast ? H[k + 2][k - 1] : 0.0);
00527                         x = std::abs(p) + std::abs(q) + std::abs(r);
00528                         if (x != 0.0) {
00529                             p = p / x;
00530                             q = q / x;
00531                             r = r / x;
00532                         }
00533                     }
00534                     if (x == 0.0) {
00535                         break;
00536                     }
00537                     s = std::sqrt(p * p + q * q + r * r);
00538                     if (p < 0) {
00539                         s = -s;
00540                     }
00541                     if (s != 0) {
00542                         if (k != m) {
00543                             H[k][k - 1] = -s * x;
00544                         } else if (l != m) {
00545                             H[k][k - 1] = -H[k][k - 1];
00546                         }
00547                         p = p + s;
00548                         x = p / s;
00549                         y = q / s;
00550                         z = r / s;
00551                         q = q / p;
00552                         r = r / p;
00553 
00554                         // Row modification
00555 
00556                         for (int j = k; j < nn; j++) {
00557                             p = H[k][j] + q * H[k + 1][j];
00558                             if (notlast) {
00559                                 p = p + r * H[k + 2][j];
00560                                 H[k + 2][j] = H[k + 2][j] - p * z;
00561                             }
00562                             H[k][j] = H[k][j] - p * x;
00563                             H[k + 1][j] = H[k + 1][j] - p * y;
00564                         }
00565 
00566                         // Column modification
00567 
00568                         for (int i = 0; i <= std::min(n1, k + 3); i++) {
00569                             p = x * H[i][k] + y * H[i][k + 1];
00570                             if (notlast) {
00571                                 p = p + z * H[i][k + 2];
00572                                 H[i][k + 2] = H[i][k + 2] - p * r;
00573                             }
00574                             H[i][k] = H[i][k] - p;
00575                             H[i][k + 1] = H[i][k + 1] - p * q;
00576                         }
00577 
00578                         // Accumulate transformations
00579 
00580                         for (int i = low; i <= high; i++) {
00581                             p = x * V[i][k] + y * V[i][k + 1];
00582                             if (notlast) {
00583                                 p = p + z * V[i][k + 2];
00584                                 V[i][k + 2] = V[i][k + 2] - p * r;
00585                             }
00586                             V[i][k] = V[i][k] - p;
00587                             V[i][k + 1] = V[i][k + 1] - p * q;
00588                         }
00589                     } // (s != 0)
00590                 } // k loop
00591             } // check convergence
00592         } // while (n1 >= low)
00593 
00594         // Backsubstitute to find vectors of upper triangular form
00595 
00596         if (norm == 0.0) {
00597             return;
00598         }
00599 
00600         for (n1 = nn - 1; n1 >= 0; n1--) {
00601             p = d[n1];
00602             q = e[n1];
00603 
00604             // Real vector
00605 
00606             if (q == 0) {
00607                 int l = n1;
00608                 H[n1][n1] = 1.0;
00609                 for (int i = n1 - 1; i >= 0; i--) {
00610                     w = H[i][i] - p;
00611                     r = 0.0;
00612                     for (int j = l; j <= n1; j++) {
00613                         r = r + H[i][j] * H[j][n1];
00614                     }
00615                     if (e[i] < 0.0) {
00616                         z = w;
00617                         s = r;
00618                     } else {
00619                         l = i;
00620                         if (e[i] == 0.0) {
00621                             if (w != 0.0) {
00622                                 H[i][n1] = -r / w;
00623                             } else {
00624                                 H[i][n1] = -r / (eps * norm);
00625                             }
00626 
00627                             // Solve real equations
00628 
00629                         } else {
00630                             x = H[i][i + 1];
00631                             y = H[i + 1][i];
00632                             q = (d[i] - p) * (d[i] - p) + e[i] * e[i];
00633                             t = (x * s - z * r) / q;
00634                             H[i][n1] = t;
00635                             if (std::abs(x) > std::abs(z)) {
00636                                 H[i + 1][n1] = (-r - w * t) / x;
00637                             } else {
00638                                 H[i + 1][n1] = (-s - y * t) / z;
00639                             }
00640                         }
00641 
00642                         // Overflow control
00643 
00644                         t = std::abs(H[i][n1]);
00645                         if ((eps * t) * t > 1) {
00646                             for (int j = i; j <= n1; j++) {
00647                                 H[j][n1] = H[j][n1] / t;
00648                             }
00649                         }
00650                     }
00651                 }
00652                 // Complex vector
00653             } else if (q < 0) {
00654                 int l = n1 - 1;
00655 
00656                 // Last vector component imaginary so matrix is triangular
00657 
00658                 if (std::abs(H[n1][n1 - 1]) > std::abs(H[n1 - 1][n1])) {
00659                     H[n1 - 1][n1 - 1] = q / H[n1][n1 - 1];
00660                     H[n1 - 1][n1] = -(H[n1][n1] - p) / H[n1][n1 - 1];
00661                 } else {
00662                     cdiv(0.0, -H[n1 - 1][n1], H[n1 - 1][n1 - 1] - p, q);
00663                     H[n1 - 1][n1 - 1] = cdivr;
00664                     H[n1 - 1][n1] = cdivi;
00665                 }
00666                 H[n1][n1 - 1] = 0.0;
00667                 H[n1][n1] = 1.0;
00668                 for (int i = n1 - 2; i >= 0; i--) {
00669                     double ra, sa, vr, vi;
00670                     ra = 0.0;
00671                     sa = 0.0;
00672                     for (int j = l; j <= n1; j++) {
00673                         ra = ra + H[i][j] * H[j][n1 - 1];
00674                         sa = sa + H[i][j] * H[j][n1];
00675                     }
00676                     w = H[i][i] - p;
00677 
00678                     if (e[i] < 0.0) {
00679                         z = w;
00680                         r = ra;
00681                         s = sa;
00682                     } else {
00683                         l = i;
00684                         if (e[i] == 0) {
00685                             cdiv(-ra, -sa, w, q);
00686                             H[i][n1 - 1] = cdivr;
00687                             H[i][n1] = cdivi;
00688                         } else {
00689 
00690                             // Solve complex equations
00691 
00692                             x = H[i][i + 1];
00693                             y = H[i + 1][i];
00694                             vr = (d[i] - p) * (d[i] - p) + e[i] * e[i] - q * q;
00695                             vi = (d[i] - p) * 2.0 * q;
00696                             if (vr == 0.0 && vi == 0.0) {
00697                                 vr = eps * norm * (std::abs(w) + std::abs(q) + std::abs(x)
00698                                                    + std::abs(y) + std::abs(z));
00699                             }
00700                             cdiv(x * r - z * ra + q * sa,
00701                                  x * s - z * sa - q * ra, vr, vi);
00702                             H[i][n1 - 1] = cdivr;
00703                             H[i][n1] = cdivi;
00704                             if (std::abs(x) > (std::abs(z) + std::abs(q))) {
00705                                 H[i + 1][n1 - 1] = (-ra - w * H[i][n1 - 1] + q
00706                                                    * H[i][n1]) / x;
00707                                 H[i + 1][n1] = (-sa - w * H[i][n1] - q * H[i][n1
00708                                                                             - 1]) / x;
00709                             } else {
00710                                 cdiv(-r - y * H[i][n1 - 1], -s - y * H[i][n1], z,
00711                                      q);
00712                                 H[i + 1][n1 - 1] = cdivr;
00713                                 H[i + 1][n1] = cdivi;
00714                             }
00715                         }
00716 
00717                         // Overflow control
00718 
00719                         t = std::max(std::abs(H[i][n1 - 1]), std::abs(H[i][n1]));
00720                         if ((eps * t) * t > 1) {
00721                             for (int j = i; j <= n1; j++) {
00722                                 H[j][n1 - 1] = H[j][n1 - 1] / t;
00723                                 H[j][n1] = H[j][n1] / t;
00724                             }
00725                         }
00726                     }
00727                 }
00728             }
00729         }
00730 
00731         // Vectors of isolated roots
00732 
00733         for (int i = 0; i < nn; i++) {
00734             if (i < low || i > high) {
00735                 for (int j = i; j < nn; j++) {
00736                     V[i][j] = H[i][j];
00737                 }
00738             }
00739         }
00740 
00741         // Back transformation to get eigenvectors of original matrix
00742 
00743         for (int j = nn - 1; j >= low; j--) {
00744             for (int i = low; i <= high; i++) {
00745                 z = 0.0;
00746                 for (int k = low; k <= std::min(j, high); k++) {
00747                     z = z + V[i][k] * H[k][j];
00748                 }
00749                 V[i][j] = z;
00750             }
00751         }
00752     }
00753 
00754     // Nonsymmetric reduction to Hessenberg form.
00755     void orthes() {
00756         //  This is derived from the Algol procedures orthes and ortran,
00757         //  by Martin and Wilkinson, Handbook for Auto. Comp.,
00758         //  Vol.ii-Linear Algebra, and the corresponding
00759         //  Fortran subroutines in EISPACK.
00760         int low = 0;
00761         int high = n - 1;
00762 
00763         for (int m = low + 1; m <= high - 1; m++) {
00764 
00765             // Scale column.
00766 
00767             double scale = 0.0;
00768             for (int i = m; i <= high; i++) {
00769                 scale = scale + std::abs(H[i][m - 1]);
00770             }
00771             if (scale != 0.0) {
00772 
00773                 // Compute Householder transformation.
00774 
00775                 double h = 0.0;
00776                 for (int i = high; i >= m; i--) {
00777                     ort[i] = H[i][m - 1] / scale;
00778                     h += ort[i] * ort[i];
00779                 }
00780                 double g = std::sqrt(h);
00781                 if (ort[m] > 0) {
00782                     g = -g;
00783                 }
00784                 h = h - ort[m] * g;
00785                 ort[m] = ort[m] - g;
00786 
00787                 // Apply Householder similarity transformation
00788                 // H = (I-u*u'/h)*H*(I-u*u')/h)
00789 
00790                 for (int j = m; j < n; j++) {
00791                     double f = 0.0;
00792                     for (int i = high; i >= m; i--) {
00793                         f += ort[i] * H[i][j];
00794                     }
00795                     f = f / h;
00796                     for (int i = m; i <= high; i++) {
00797                         H[i][j] -= f * ort[i];
00798                     }
00799                 }
00800 
00801                 for (int i = 0; i <= high; i++) {
00802                     double f = 0.0;
00803                     for (int j = high; j >= m; j--) {
00804                         f += ort[j] * H[i][j];
00805                     }
00806                     f = f / h;
00807                     for (int j = m; j <= high; j++) {
00808                         H[i][j] -= f * ort[j];
00809                     }
00810                 }
00811                 ort[m] = scale * ort[m];
00812                 H[m][m - 1] = scale * g;
00813             }
00814         }
00815 
00816         // Accumulate transformations (Algol's ortran).
00817 
00818         for (int i = 0; i < n; i++) {
00819             for (int j = 0; j < n; j++) {
00820                 V[i][j] = (i == j ? 1.0 : 0.0);
00821             }
00822         }
00823 
00824         for (int m = high - 1; m >= low + 1; m--) {
00825             if (H[m][m - 1] != 0.0) {
00826                 for (int i = m + 1; i <= high; i++) {
00827                     ort[i] = H[i][m - 1];
00828                 }
00829                 for (int j = m; j <= high; j++) {
00830                     double g = 0.0;
00831                     for (int i = m; i <= high; i++) {
00832                         g += ort[i] * V[i][j];
00833                     }
00834                     // Double division avoids possible underflow
00835                     g = (g / ort[m]) / H[m][m - 1];
00836                     for (int i = m; i <= high; i++) {
00837                         V[i][j] += g * ort[i];
00838                     }
00839                 }
00840             }
00841         }
00842     }
00843 
00844     // Releases all internal working memory.
00845     void release() {
00846         // releases the working data
00847         delete[] d;
00848         delete[] e;
00849         delete[] ort;
00850         for (int i = 0; i < n; i++) {
00851             delete[] H[i];
00852             delete[] V[i];
00853         }
00854         delete[] H;
00855         delete[] V;
00856     }
00857 
00858     // Computes the Eigenvalue Decomposition for a matrix given in H.
00859     void compute() {
00860         // Allocate memory for the working data.
00861         V = alloc_2d<double> (n, n, 0.0);
00862         d = alloc_1d<double> (n);
00863         e = alloc_1d<double> (n);
00864         ort = alloc_1d<double> (n);
00865         // Reduce to Hessenberg form.
00866         orthes();
00867         // Reduce Hessenberg to real Schur form.
00868         hqr2();
00869         // Copy eigenvalues to OpenCV Matrix.
00870         _eigenvalues.create(1, n, CV_64FC1);
00871         for (int i = 0; i < n; i++) {
00872             _eigenvalues.at<double> (0, i) = d[i];
00873         }
00874         // Copy eigenvectors to OpenCV Matrix.
00875         _eigenvectors.create(n, n, CV_64FC1);
00876         for (int i = 0; i < n; i++)
00877             for (int j = 0; j < n; j++)
00878                 _eigenvectors.at<double> (i, j) = V[i][j];
00879         // Deallocate the memory by releasing all internal working data.
00880         release();
00881     }
00882 
00883 public:
00884     EigenvalueDecomposition()
00885     : n(0) { }
00886 
00887     // Initializes & computes the Eigenvalue Decomposition for a general matrix
00888     // given in src. This function is a port of the EigenvalueSolver in JAMA,
00889     // which has been released to public domain by The MathWorks and the
00890     // National Institute of Standards and Technology (NIST).
00891     EigenvalueDecomposition(InputArray src) {
00892         compute(src);
00893     }
00894 
00895     // This function computes the Eigenvalue Decomposition for a general matrix
00896     // given in src. This function is a port of the EigenvalueSolver in JAMA,
00897     // which has been released to public domain by The MathWorks and the
00898     // National Institute of Standards and Technology (NIST).
00899     void compute(InputArray src)
00900     {
00901         if(isSymmetric(src)) {
00902             // Fall back to OpenCV for a symmetric matrix!
00903             cv::eigen(src, _eigenvalues, _eigenvectors);
00904         } else {
00905             Mat tmp;
00906             // Convert the given input matrix to double. Is there any way to
00907             // prevent allocating the temporary memory? Only used for copying
00908             // into working memory and deallocated after.
00909             src.getMat().convertTo(tmp, CV_64FC1);
00910             // Get dimension of the matrix.
00911             this->n = tmp.cols;
00912             // Allocate the matrix data to work on.
00913             this->H = alloc_2d<double> (n, n);
00914             // Now safely copy the data.
00915             for (int i = 0; i < tmp.rows; i++) {
00916                 for (int j = 0; j < tmp.cols; j++) {
00917                     this->H[i][j] = tmp.at<double>(i, j);
00918                 }
00919             }
00920             // Deallocates the temporary matrix before computing.
00921             tmp.release();
00922             // Performs the eigenvalue decomposition of H.
00923             compute();
00924         }
00925     }
00926 
00927     ~EigenvalueDecomposition() {}
00928 
00929     // Returns the eigenvalues of the Eigenvalue Decomposition.
00930     Mat eigenvalues() {    return _eigenvalues; }
00931     // Returns the eigenvectors of the Eigenvalue Decomposition.
00932     Mat eigenvectors() { return _eigenvectors; }
00933 };
00934 
00935 
00936 //------------------------------------------------------------------------------
00937 // Linear Discriminant Analysis implementation
00938 //------------------------------------------------------------------------------
00939 
00940 LDA::LDA(int num_components) : _dataAsRow(true), _num_components(num_components) { }
00941 
00942 LDA::LDA(InputArrayOfArrays src, InputArray labels, int num_components) : _dataAsRow(true),  _num_components(num_components)
00943 {
00944     this->compute(src, labels); //! compute eigenvectors and eigenvalues
00945 }
00946 
00947 LDA::~LDA() {}
00948 
00949 void LDA::save(const String& filename) const
00950 {
00951     FileStorage fs(filename, FileStorage::WRITE);
00952     if (!fs.isOpened()) {
00953         CV_Error(Error::StsError, "File can't be opened for writing!");
00954     }
00955     this->save(fs);
00956     fs.release();
00957 }
00958 
00959 // Deserializes this object from a given filename.
00960 void LDA::load(const String& filename) {
00961     FileStorage fs(filename, FileStorage::READ);
00962     if (!fs.isOpened())
00963        CV_Error(Error::StsError, "File can't be opened for writing!");
00964     this->load(fs);
00965     fs.release();
00966 }
00967 
00968 // Serializes this object to a given FileStorage.
00969 void LDA::save(cv::FileStorage& fs) const {
00970     /*the debug
00971     // write matrices
00972     fs << "num_components" << _num_components;
00973     fs << "eigenvalues" << _eigenvalues;
00974     fs << "eigenvectors" << _eigenvectors;*/
00975 }
00976 
00977 // Deserializes this object from a given FileStorage.
00978 void LDA::load(const FileStorage& fs) {
00979     //read matrices
00980     fs["num_components"] >> _num_components;
00981     fs["eigenvalues"] >> _eigenvalues;
00982     fs["eigenvectors"] >> _eigenvectors;
00983 }
00984 
00985 void LDA::lda (InputArrayOfArrays _src, InputArray _lbls) {
00986     // get data
00987     Mat src = _src.getMat();
00988     std::vector<int> labels;
00989     // safely copy the labels
00990     {
00991         Mat tmp = _lbls.getMat();
00992         for(unsigned int i = 0; i < tmp.total(); i++) {
00993             labels.push_back(tmp.at<int>(i));
00994         }
00995     }
00996     // turn into row sampled matrix
00997     Mat data;
00998     // ensure working matrix is double precision
00999     src.convertTo(data, CV_64FC1);
01000     // maps the labels, so they're ascending: [0,1,...,C]
01001     std::vector<int> mapped_labels(labels.size());
01002     std::vector<int> num2label = remove_dups(labels);
01003     std::map<int, int> label2num;
01004     for (int i = 0; i < (int)num2label.size(); i++)
01005         label2num[num2label[i]] = i;
01006     for (size_t i = 0; i < labels.size(); i++)
01007         mapped_labels[i] = label2num[labels[i]];
01008     // get sample size, dimension
01009     int N = data.rows;
01010     int D = data.cols;
01011     // number of unique labels
01012     int C = (int)num2label.size();
01013     // we can't do a LDA on one class, what do you
01014     // want to separate from each other then?
01015     if(C == 1) {
01016         String error_message = "At least two classes are needed to perform a LDA. Reason: Only one class was given!";
01017         CV_Error(Error::StsBadArg, error_message);
01018     }
01019     // throw error if less labels, than samples
01020     if (labels.size() != static_cast<size_t>(N)) {
01021         String error_message = format("The number of samples must equal the number of labels. Given %d labels, %d samples. ", labels.size(), N);
01022         CV_Error(Error::StsBadArg, error_message);
01023     }
01024     // warn if within-classes scatter matrix becomes singular
01025     if (N < D) {
01026         std::cout << "Warning: Less observations than feature dimension given!"
01027                   << "Computation will probably fail."
01028                   << std::endl;
01029     }
01030     // clip number of components to be a valid number
01031     if ((_num_components <= 0) || (_num_components > (C - 1))) {
01032         _num_components = (C - 1);
01033     }
01034     // holds the mean over all classes
01035     Mat meanTotal = Mat::zeros(1, D, data.type());
01036     // holds the mean for each class
01037     std::vector<Mat> meanClass(C);
01038     std::vector<int> numClass(C);
01039     // initialize
01040     for (int i = 0; i < C; i++) {
01041         numClass[i] = 0;
01042         meanClass[i] = Mat::zeros(1, D, data.type()); //! Dx1 image vector
01043     }
01044     // calculate sums
01045     for (int i = 0; i < N; i++) {
01046         Mat instance = data.row(i);
01047         int classIdx = mapped_labels[i];
01048         add(meanTotal, instance, meanTotal);
01049         add(meanClass[classIdx], instance, meanClass[classIdx]);
01050         numClass[classIdx]++;
01051     }
01052     // calculate total mean
01053     meanTotal.convertTo(meanTotal, meanTotal.type(), 1.0 / static_cast<double> (N));
01054     // calculate class means
01055     for (int i = 0; i < C; i++) {
01056         meanClass[i].convertTo(meanClass[i], meanClass[i].type(), 1.0 / static_cast<double> (numClass[i]));
01057     }
01058     // subtract class means
01059     for (int i = 0; i < N; i++) {
01060         int classIdx = mapped_labels[i];
01061         Mat instance = data.row(i);
01062         subtract(instance, meanClass[classIdx], instance);
01063     }
01064     // calculate within-classes scatter
01065     Mat Sw = Mat::zeros(D, D, data.type());
01066     mulTransposed(data, Sw, true);
01067     // calculate between-classes scatter
01068     Mat Sb = Mat::zeros(D, D, data.type());
01069     for (int i = 0; i < C; i++) {
01070         Mat tmp;
01071         subtract(meanClass[i], meanTotal, tmp);
01072         mulTransposed(tmp, tmp, true);
01073         add(Sb, tmp, Sb);
01074     }
01075     // invert Sw
01076     Mat Swi = Sw.inv();
01077     // M = inv(Sw)*Sb
01078     Mat M;
01079     gemm(Swi, Sb, 1.0, Mat(), 0.0, M);
01080     EigenvalueDecomposition es(M);
01081     _eigenvalues = es.eigenvalues();
01082     _eigenvectors = es.eigenvectors();
01083     // reshape eigenvalues, so they are stored by column
01084     _eigenvalues = _eigenvalues.reshape(1, 1);
01085     // get sorted indices descending by their eigenvalue
01086     std::vector<int> sorted_indices = argsort(_eigenvalues, false);
01087     // now sort eigenvalues and eigenvectors accordingly
01088     _eigenvalues = sortMatrixColumnsByIndices(_eigenvalues, sorted_indices);
01089     _eigenvectors = sortMatrixColumnsByIndices(_eigenvectors, sorted_indices);
01090     // and now take only the num_components and we're out!
01091     _eigenvalues = Mat(_eigenvalues, Range::all(), Range(0, _num_components));
01092     _eigenvectors = Mat(_eigenvectors, Range::all(), Range(0, _num_components));
01093 }
01094 
01095 void LDA::compute(InputArrayOfArrays _src, InputArray _lbls) {
01096     switch(_src.kind()) {
01097     case _InputArray::STD_VECTOR_MAT:
01098         lda (asRowMatrix(_src, CV_64FC1), _lbls);
01099         break;
01100     case _InputArray::MAT:
01101         lda (_src.getMat(), _lbls);
01102         break;
01103     default:
01104         String error_message= format("InputArray Datatype %d is not supported.", _src.kind());
01105         CV_Error(Error::StsBadArg, error_message);
01106         break;
01107     }
01108 }
01109 
01110 // Projects one or more row aligned samples into the LDA subspace.
01111 Mat LDA::project(InputArray src) {
01112    return subspaceProject(_eigenvectors, Mat(), src);
01113 }
01114 
01115 // Reconstructs projections from the LDA subspace from one or more row aligned samples.
01116 Mat LDA::reconstruct(InputArray src) {
01117    return subspaceReconstruct(_eigenvectors, Mat(), src);
01118 }
01119 
01120 }
01121