/** eigentrain computes the eigen space for matrix images. This function is used in the training procedure of the face recognition pca algorithm. INPUT: the data matrix of images OUTPUT: mean: the mean value of the images eigen_values: eigenvalues eigen_base: eigenvectors The data matrix is mean centered, and this is a side effect. */ void eigentrain(Matrix *mean, Matrix *eigen_vals, Matrix *eigen_base, Matrix images) { double p = 0.0; Matrix M, eigenvectors; DEBUG(1, "Calculating mean image."); *mean = get_mean_image(images); DEBUG(1, "Calculating the mean centered images for the training set."); mean_subtract_images(images, *mean); MESSAGE2ARG("Calculating Covariance Matrix: M = images' * images. M is a %d by %d Matrix.", images->col_dim, images->col_dim); M = transposeMultiplyMatrixL(images, images); DEBUG_INT(3, "Covariance Matrix Rows", M->row_dim); DEBUG_INT(3, "Covariance Matrix Cols", M->col_dim); DEBUG(2, "Allocating memory for eigenvectors and eigenvalues."); eigenvectors = makeMatrix(M->row_dim, M->col_dim); *eigen_vals = makeMatrix(M->row_dim, 1); MESSAGE("Computing snap shot eigen vectors using the double precision cv eigensolver."); cvJacobiEigens_64d(M->data, eigenvectors->data, (*eigen_vals)->data, images->col_dim, p, 1); freeMatrix(M); DEBUG(1, "Verifying the eigen vectors"); /* Reconstruct M because it is destroyed in cvJacobiEigens */ M = transposeMultiplyMatrixL(images, images); if (debuglevel >= 3) eigen_verify(M, *eigen_vals, eigenvectors); freeMatrix(M); *eigen_base = multiplyMatrix(images, eigenvectors); MESSAGE2ARG("Recovered the %d by %d high resolution eigen basis.", (*eigen_base)->row_dim, (*eigen_base)->col_dim); DEBUG(1, "Normalizing eigen basis"); basis_normalize(*eigen_base); /*Remove last elements because they are unneeded. Mean centering the image guarantees that the data points define a hyperplane that passes through the origin. Therefore all points are in a k - 1 dimensional subspace. */ (*eigen_base)->col_dim -= 1; (*eigen_vals)->row_dim -= 1; eigenvectors->col_dim -= 1; DEBUG(1, "Verifying eigenbasis"); if (debuglevel >= 3) basis_verify(images, *eigen_base); /* The eigenvectors for the smaller covariance (snap shot) are not needed */ freeMatrix(eigenvectors); }
int SQFitting::estimateParameters(const pcl::PointCloud<pcl::PointXYZ>& cloud, SQParameters& sqParams, const int& eigenVector) { double t[4][4] = {0}; // Check if cloud contains enough data points int len = (int) cloud.points.size(); if (len < 13) { // changed from 13 return -1; } // Find center of gravity double a, b, c, x, y, z, xx, yy, zz, cxy, cxz, cyz, count = 0.0; x = y = z = 0; xx = yy = zz = cxy = cyz = cxz = 0; count = 0; for(int i = 0; i < len; i++) { pcl::PointXYZ pnt = cloud.points[i]; a = (double) pnt.x; b = (double) pnt.y; c = (double) pnt.z; x += a; y += b; z += c; xx += a*a; yy += b*b; zz += c*c; cxz += a*c; cyz += b*c; cxy += a*b; count++; } // Compute center of gravity, covariance and variance double avgx, avgy, avgz; avgx = x/count; avgy = y/count; avgz = z/count; xx /= count; yy /= count; zz /= count; cxz /= count; cyz /= count; cxy /= count; xx -= (avgx * avgx); yy -= (avgy * avgy); zz -= (avgz * avgz); cxy -= (avgx * avgy); cxz -= (avgx * avgz); cyz -= (avgy * avgz); // Build covariance matrix double aa[3][3]; aa[0][0] = xx; aa[0][1] = cxy; aa[0][2] = cxz; aa[1][0] = cxy; aa[1][1] = yy; aa[1][2] = cyz; aa[2][0] = cxz; aa[2][1] = cyz; aa[2][2] = zz; // Compute eigenvectors and eigenvalues double d[3], m_brain[3][3], m_inverse[3][3]; double lambdas[3]; double vectors[3][3]; int nrot; /* * Using the covariance matrix, we get its eigenvalues * and eigenvectors */ cvJacobiEigens_64d((double*) aa, (double*) m_brain, (double*) d, 3, 0.0); // Find the vector with the largest eigenvalue double max_eigen = -1000000000.0; int vectorIndex; if(d[0] > max_eigen) { max_eigen = d[0]; vectorIndex = 0; } if(d[1] > max_eigen) { max_eigen = d[1]; vectorIndex = 1; } if(d[2] > max_eigen) { max_eigen = d[2]; vectorIndex = 2; } for (int j = 0; j < 3; j++) { for (int k = 0; k < 3; k++) { m_inverse[j][k] = m_brain[j][k]; } } // Aligning max eigenvector with z axis #if 0 for (int j = 0; j < 3; j++) { m_inverse[j][2] = m_brain[j][eigenVector]; m_inverse[j][eigenVector] = m_brain[j][2]; } if (vectorIndex != 2) { for (int j = 0; j < 3; j++) { m_inverse[j][vectorIndex] = -m_inverse[j][vectorIndex]; } } #endif for (int j = 0; j < 3; j++) { for (int k = 0; k < 3; k++) { m_brain[j][k] = m_inverse[j][k]; } } // Build transformation matrix (rotation & translation) for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++){ t[i][j] = m_brain[i][j]; } } t[0][3] = avgx; t[1][3] = avgy; t[2][3] = avgz; t[3][3] = 1; t[3][0] = t[3][1] = t[3][2] = 0.0; // Set / Calculate initial parameter sqParams.e1 = 1; sqParams.e2 = 1; sqParams.kx = 0; sqParams.ky = 0; double xmin, ymin, zmin, xmax, ymax, zmax; xmin = ymin = zmin = 100000.00; xmax = ymax = zmax = -100000.00; double tInv[4][4]; matrix_inverse(t, tInv); double vector[4]; for(int i = 0; i < len; i++) { pcl::PointXYZ pnt = cloud.points[i]; vector[0] = (double) pnt.x; vector[1] = (double) pnt.y; vector[2] = (double) pnt.z; vector[3] = 1; double result[4]; matrix_mult(vector, tInv, result); a = result[0]; b = result[1]; c = result[2]; if(xmin > a) xmin = a; if(xmax < a) xmax = a; if(ymin > b) ymin = b; if(ymax < b) ymax = b; if(zmin > c) zmin = c; if(zmax < c) zmax = c; } double r1, r2, r3; r1 = atan2(t[1][2], t[0][2]); r1 = r1 + M_PI; r2 = atan2(cos(r1) * t[0][2] + sin(r1) * t[1][2], t[2][2]); r3 = atan2(-sin(r1) * t[0][0] + cos(r1) * t[1][0], -sin(r1) * t[0][1] + cos(r1) * t[1][1]); sqParams.phi = r1; sqParams.theta = r2; sqParams.psi = r3; sqParams.px = t[0][3] + (xmax + xmin) / 2; sqParams.py = t[1][3] + (ymax + ymin) / 2; sqParams.pz = t[2][3] + (zmax + zmin) / 2; double ta1, ta2, ta3; ta1 = (xmax - xmin)/2; ta2 = (ymax - ymin)/2; ta3 = (zmax - zmin)/2; sqParams.a1 = ta1; sqParams.a2 = ta2; sqParams.a3 = ta3; return 0; }
void fisherTrain(Matrix imspca, ImageList *srt, Matrix *fisherBasis, Matrix *fisherValues, int writeTextInterm) { int i; int numberOfClasses; Matrix G, N, Tmp; Matrix Rw = makeIdentityMatrix(imspca->row_dim); Matrix Siw = makeIdentityMatrix(imspca->row_dim); Matrix Ev = makeMatrix(imspca->row_dim, 1); Matrix Evecs = makeMatrix(imspca->row_dim, imspca->row_dim); Matrix Mw = findWCSMatrix(imspca, srt, &numberOfClasses, writeTextInterm); Matrix Mb = findBCSMatrix(imspca, Mw); *fisherValues = makeMatrix(imspca->row_dim, 1); MESSAGE2ARG("LDA Training started with %d classes and %d total training images.", numberOfClasses, imspca->col_dim); if (writeTextInterm) { SAVE_MATRIX(Mw); SAVE_MATRIX(Mb); } /* output textfiles of intermediate matrices */ MESSAGE("Computing eigenspace decomposition of within class scatter matrix."); cvJacobiEigens_64d(Mw->data, Rw->data, Ev->data, Mw->row_dim, 0.0); MESSAGE("Computing the inverse scale matrix derived from eigenvalues and transformed scatter matrix."); for (i = 0; i < Ev->row_dim; i++) ME(Siw, i, i) = (ME(Ev, i, 0) <= 0.0) ? 0.0 : 1 / ( sqrt( ME(Ev, i, 0) ) ); G = transposeMultiplyMatrixR(Siw, Rw); Tmp = transposeMultiplyMatrixR(Mb, G); N = multiplyMatrix(G, Tmp); freeMatrix(Tmp); if (writeTextInterm) { SAVE_MATRIX(Rw); SAVE_MATRIX(Ev); SAVE_MATRIX(N); SAVE_MATRIX(G); SAVE_MATRIX(Siw); } /* output textfiles of intermediate matrices */ MESSAGE("Computing eigenspace of transformed between class scatter matrix."); cvJacobiEigens_64d(N->data, Evecs->data, (*fisherValues)->data, N->row_dim, 0.0); DEBUG(3, "FINSISHED"); Tmp = multiplyMatrix(Siw, Evecs); DEBUG(1, "Calculating fisher basis"); *fisherBasis = multiplyMatrix(Rw, Tmp); if (writeTextInterm) { SAVE_MATRIX(*fisherBasis); SAVE_MATRIX(*fisherValues); SAVE_MATRIX(Evecs); } /* output textfiles of intermediate matrices */ /* The following verification code does not seem to work so it has been commented out. */ /* fisherVerify(*fisherBasis, *fisherValues, Mw, Mb); */ /* Crop the basis to the proper number of vectors */ (*fisherBasis)->col_dim = numberOfClasses - 1; (*fisherValues)->row_dim = numberOfClasses - 1; basis_normalize(*fisherBasis); MESSAGE2ARG("Completed LDA Training. Fisher basis projection matrix has dimensions %d by %d.", (*fisherBasis)->row_dim, (*fisherBasis)->col_dim); /*Freeing memory allocated during LDA Training. */ freeMatrix(Tmp); freeMatrix(Rw); freeMatrix(Siw); freeMatrix(Ev); freeMatrix(Mw); freeMatrix(Mb); freeMatrix(G); freeMatrix(N); }