/**
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);
}
