/*
eigen_verify
Verify properties of the eigen vectors.
The eigenbasis should be ortonormal: R'*R - I == 0
The basis should be decomposed such that: MR - RD == 0
returns true if tests fail.
*/
int eigen_verify(Matrix M, Matrix lambda, Matrix R) {
Matrix RtR = transposeMultiplyMatrixL(R, R);
Matrix identity = makeIdentityMatrix(R->col_dim);
Matrix MR = multiplyMatrix(M, R);
Matrix D = makeIdentityMatrix(lambda->row_dim);
Matrix RD;
Matrix test;
int i, j;
int failed = 0;
const double tol = 1.0e-7;
for (i = 0; i < lambda->row_dim; i++) {
ME(D, i, i) = ME(lambda, i, 0);
}
RD = multiplyMatrix(R, D);
freeMatrix(D);
DEBUG(2, "Checking orthogonality of eigenvectors");
test = subtractMatrix(RtR, identity);
freeMatrix(RtR);
freeMatrix(identity);
for (i = 0; i < test->row_dim; i++) {
for (j = 0; j < test->col_dim; j++) {
if (!EQUAL_ZERO(ME(test, i, j), tol)) {
failed = 1;
MESSAGE("Eigenvectors are not orthogonal to within tolerance.");
DEBUG_DOUBLE(1, "Matrix Element", ME(test, i, j));
DEBUG_DOUBLE(1, "Tolerance", tol);
exit(1);
}
}
}
freeMatrix(test);
DEBUG(2, "Checking reconstruction property of eigensystem");
test = subtractMatrix(MR, RD);
freeMatrix(MR);
freeMatrix(RD);
for (i = 0; i < test->row_dim; i++) {
for (j = 0; j < test->col_dim; j++) {
if (!EQUAL_ZERO(ME(test, i, j), tol)) {
failed = 1;
MESSAGE("Covariance matrix is not reconstructable to within tolerance.");
DEBUG_DOUBLE(1, "Matrix Element", ME(test, i, j));
DEBUG_DOUBLE(1, "Tolerance", tol);
exit(1);
}
}
}
freeMatrix(test);
return failed;
}
Matrix reflectMatrix(int bool_x, int bool_y){
Matrix transform = makeIdentityMatrix(3);
if(bool_x) ME(transform,0,0) = -1;
if(bool_y) ME(transform,1,1) = -1;
return transform;
}
/* Return a scale Matrix */
Matrix scaleMatrix(double s){
Matrix transform = makeIdentityMatrix(3);
ME(transform,0,0) = s;
ME(transform,1,1) = s;
return transform;
}
void fisherVerify(Matrix fisherBasis, Matrix fisherValues, Matrix Sw, Matrix Sb) {
Matrix SbW = multiplyMatrix(Sb, fisherBasis);
Matrix SwW = multiplyMatrix(Sw, fisherBasis);
Matrix D = makeIdentityMatrix(fisherBasis->row_dim);
Matrix DSwW;
Matrix zeroMat;
int i, j;
MESSAGE("Verifying Fisher Basis.");
for (i = 0; i < D->row_dim; i++) {
ME(D, i, i) = ME(fisherValues, i, 0);
}
DSwW = multiplyMatrix(D, SwW);
zeroMat = subtractMatrix(SbW, DSwW);
for (i = 0; i < zeroMat->row_dim; i++) {
for (j = 0; j < zeroMat->col_dim; j++) {
if (!EQUAL_ZERO(ME(zeroMat, i, j), 0.000001)) {
DEBUG( -1, "Fisher validation failed.");
printf("Element: (%d,%d) value = %f", i, j, ME(zeroMat, i, j));
exit(1);
}
}
}
}
/* Return a translation matrix */
Matrix translateMatrix(double dx, double dy){
Matrix transform = makeIdentityMatrix(3);
ME(transform,0,2) = dx;
ME(transform,1,2) = dy;
return transform;
}
/* Return a rotation matrix */
Matrix rotateMatrix(double theta){
Matrix transform = makeIdentityMatrix(3);
ME(transform,0,0) = cos(theta);
ME(transform,1,1) = cos(theta);
ME(transform,0,1) = -sin(theta);
ME(transform,1,0) = sin(theta);
return transform;
}
/**
Verify properties of the eigen basis used for pca.
The eigenbasis should be ortonormal: U'*U - I == 0
The basis should be decomposed such that: X == U*D*V'
returns true if tests fail.
*/
int basis_verify(Matrix X, Matrix U) {
Matrix UtX = transposeMultiplyMatrixL(U, X);
Matrix UUtX = multiplyMatrix(U, UtX);
Matrix UtU = transposeMultiplyMatrixL(U, U);
Matrix identity = makeIdentityMatrix(U->col_dim);
Matrix test;
int i, j;
int failed = 0;
const double tol = 1.0e-7;
freeMatrix(UtX);
DEBUG(2, "Checking orthogonality of eigenbasis");
test = subtractMatrix(UtU, identity);
freeMatrix(UtU);
freeMatrix(identity);
for (i = 0; i < test->row_dim; i++) {
for (j = 0; j < test->col_dim; j++) {
if (!EQUAL_ZERO(ME(test, i, j), tol)) {
failed = 1;
MESSAGE("Eigenbasis is not orthogonal to within tolerance.");
DEBUG_DOUBLE(1, "Matrix Element", ME(test, i, j));
DEBUG_DOUBLE(1, "Tolerance", tol);
exit(1);
}
}
}
freeMatrix(test);
DEBUG(2, "Checking reconstruction property of the eigen decomposition");
test = subtractMatrix(X, UUtX);
freeMatrix(UUtX);
for (i = 0; i < test->row_dim; i++) {
for (j = 0; j < test->col_dim; j++) {
if (!EQUAL_ZERO(ME(test, i, j), tol)) {
failed = 1;
MESSAGE("Data matrix is not reconstructable to within tolerance.");
DEBUG_DOUBLE(1, "Matrix Element", ME(test, i, j));
DEBUG_DOUBLE(1, "Tolarence", tol);
exit(1);
}
}
}
freeMatrix(test);
return failed;
}
void
MetricBundleOptimizerBase::poseDerivatives(int i, int j, Vector3d& XX,
Matrix3x6d& d_dRT, Matrix3x3d& d_dX) const
{
XX = _cams[i].transformPointIntoCameraSpace(_Xs[j]);
// See Frank Dellaerts bundle adjustment tutorial.
// d(dR * R0 * X + t)/d omega = -[R0 * X]_x
Matrix3x3d J;
makeCrossProductMatrix(XX - _cams[i].getTranslation(), J);
scaleMatrixIP(-1.0, J);
// Now the transformation from world coords into camera space is xx = Rx + T
// Hence the derivative of x wrt. T is just the identity matrix.
makeIdentityMatrix(d_dRT);
copyMatrixSlice(J, 0, 0, 3, 3, d_dRT, 0, 3);
// The derivative of Rx+T wrt x is just R.
copyMatrix(_cams[i].getRotation(), d_dX);
} // end MetricBundleOptimizerBase::poseDerivatives()
void convertImages(Arguments* args){
char** mask = NULL;
TwoPoints source, dest;
FILE* eyeList;
char line[ FILE_LINE_LENGTH ];
char filename[MAX_FILENAME_LENGTH];
char imagename[MAX_FILENAME_LENGTH];
char suffix[MAX_FILENAME_LENGTH];
int i;
scaleArgs(args, args->scale);
dest.x1 = args->eyeLx;
dest.y1 = args->eyeLy;
dest.x2 = args->eyeRx;
dest.y2 = args->eyeRy;
/* Prepare file suffix encoding preprocessing settings, blank if not requested */
if (args->configSuffix) {
sprintf(suffix,"_%s", imageSuffix(args)); }
else {
suffix[0] = '\0'; }
if(args->maskType == CL_YES){
MESSAGE("Creating Mask.");
mask = generateMask(args->sizeWidth, args->sizeHeight, args->ellipseX, args->ellipseY, args->ellipseA, args->ellipseB);
}
eyeList = fopen(args->eyeFile,"r");
DEBUG_CHECK(eyeList,"Error opening eye coordinates file");
for(i = 1;;i++){
Image pgm;
Image geo;
Matrix transform;
fgets(line, FILE_LINE_LENGTH, eyeList);
if(feof(eyeList)) break;
if(sscanf(line,"%s %lf %lf %lf %lf",filename, &(source.x1), &(source.y1), &(source.x2), &(source.y2)) != 5){
printf("Error parsing line %d of eye coordinate file. Exiting...",i);
exit(1);
}
/* shift the eye coordinates if neccessary */
source.x1 += args->shiftX;
source.y1 += args->shiftY;
source.x2 += args->shiftX;
source.y2 += args->shiftY;
sprintf(imagename,"%s\\%s.pgm",args->inputDir,filename);
MESSAGE1ARG("Processing image: %s",filename);
pgm = readPGMImage(imagename);
if(args->histType == HIST_PRE){
DEBUG(1," Performing Pre Histogram Equalization.");
histEqual(pgm,256);
}
if(args->preNormType == CL_YES){
DEBUG(1," Performing Pre Pixel Normalization.");
ZeroMeanOneStdDev(pgm);
}
if(args->preEdge){
smoothImageEdge(pgm, args->preEdge);
}
if(args->geoType == CL_YES){
DEBUG(1," Performing Geometric Normalization.");
transform = generateTransform(&source,&dest,args->reflect);
geo = transformImage(pgm,args->sizeWidth,args->sizeHeight,transform);
}
else{
transform = makeIdentityMatrix(3);
geo = transformImage(pgm,args->sizeWidth,args->sizeHeight,transform);
}
if(args->noise != 0.0){
DEBUG(1," Adding Gausian Noise.");
gaussianNoise(geo,args->noise);
}
if(args->histType == HIST_POST){
DEBUG(1," Performing Post Histogram Equalization.");
histEqualMask(geo,256, (const char**) mask);
}
if(args->nrmType == CL_YES){
DEBUG(1," Performing final value normalization and Applying Mask.");
ZeroMeanOneStdDevMasked(geo, (const char **) mask);
}
else{
DEBUG(1," No Value Normalization. Just Applying Mask.");
applyMask(geo, (const char **) mask);
}
if(args->postEdge){
smoothImageEdge(geo, args->postEdge);
}
if(args->nrmDir){
sprintf(imagename,"%s\\%s%s.nrm", args->nrmDir, filename, suffix);
DEBUG_STRING(1," Saving nrm: %s",imagename);
writeFeretImage(geo,imagename);
}
if(args->pgmDir){
sprintf(imagename,"%s\\%s%s.pgm", args->pgmDir, filename, suffix);
DEBUG_STRING(1," Saving pgm: %s",imagename);
writePGMImage(geo,imagename,0);
}
if(args->sfiDir){
sprintf(imagename,"%s\\%s%s.sfi", args->sfiDir, filename, suffix);
DEBUG_STRING(1," Saving sfi: %s",imagename);
writeRawImage(geo,imagename);
}
freeImage(geo);
freeImage(pgm);
freeMatrix(transform);
}
fclose(eyeList);
}
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);
}
void
computeConsistentRotations(int const nViews,
std::vector<Matrix3x3d> const& relativeRotations,
std::vector<std::pair<int, int> > const& viewPairs,
std::vector<Matrix3x3d>& rotations, int method)
{
#if !defined(V3DLIB_ENABLE_ARPACK)
if (method == V3D_CONSISTENT_ROTATION_METHOD_SPARSE_EIG)
method = V3D_CONSISTENT_ROTATION_METHOD_EIG_ATA;
#endif
int const nRelPoses = relativeRotations.size();
rotations.resize(nViews);
switch (method)
{
case V3D_CONSISTENT_ROTATION_METHOD_SVD:
{
Matrix<double> A(3*nRelPoses, 3*nViews, 0.0);
Matrix3x3d I;
makeIdentityMatrix(I);
scaleMatrixIP(-1.0, I);
for (int i = 0; i < nRelPoses; ++i)
{
int const view1 = viewPairs[i].first;
int const view2 = viewPairs[i].second;
Matrix3x3d const& Rrel = relativeRotations[i];
copyMatrixSlice(Rrel, 0, 0, 3, 3, A, 3*i, 3*view1);
copyMatrixSlice(I, 0, 0, 3, 3, A, 3*i, 3*view2);
} // end for (i)
SVD<double> svd(A);
int const startColumn = A.num_cols()-3; // last columns of right sing. vec for SVD
Matrix<double> const& V = svd.getV();
for (int i = 0; i < nViews; ++i)
{
copyMatrixSlice(V, 3*i, startColumn, 3, 3, rotations[i], 0, 0);
enforceRotationMatrix(rotations[i]);
}
break;
}
case V3D_CONSISTENT_ROTATION_METHOD_SVD_ATA:
case V3D_CONSISTENT_ROTATION_METHOD_EIG_ATA:
case V3D_CONSISTENT_ROTATION_METHOD_SPARSE_EIG:
{
vector<pair<int, int> > nzA;
vector<double> valsA;
nzA.reserve(12*nRelPoses);
valsA.reserve(12*nRelPoses);
for (int i = 0; i < nRelPoses; ++i)
{
int const view1 = viewPairs[i].first;
int const view2 = viewPairs[i].second;
Matrix3x3d const& Rrel = relativeRotations[i];
nzA.push_back(make_pair(3*i+0, 3*view1+0)); valsA.push_back(Rrel[0][0]);
nzA.push_back(make_pair(3*i+0, 3*view1+1)); valsA.push_back(Rrel[0][1]);
nzA.push_back(make_pair(3*i+0, 3*view1+2)); valsA.push_back(Rrel[0][2]);
nzA.push_back(make_pair(3*i+1, 3*view1+0)); valsA.push_back(Rrel[1][0]);
nzA.push_back(make_pair(3*i+1, 3*view1+1)); valsA.push_back(Rrel[1][1]);
nzA.push_back(make_pair(3*i+1, 3*view1+2)); valsA.push_back(Rrel[1][2]);
nzA.push_back(make_pair(3*i+2, 3*view1+0)); valsA.push_back(Rrel[2][0]);
nzA.push_back(make_pair(3*i+2, 3*view1+1)); valsA.push_back(Rrel[2][1]);
nzA.push_back(make_pair(3*i+2, 3*view1+2)); valsA.push_back(Rrel[2][2]);
nzA.push_back(make_pair(3*i+0, 3*view2+0)); valsA.push_back(-1.0);
nzA.push_back(make_pair(3*i+1, 3*view2+1)); valsA.push_back(-1.0);
nzA.push_back(make_pair(3*i+2, 3*view2+2)); valsA.push_back(-1.0);
} // end for (i)
CCS_Matrix<double> A(3*nRelPoses, 3*nViews, nzA, valsA);
if (method == V3D_CONSISTENT_ROTATION_METHOD_SPARSE_EIG)
{
#if defined(V3DLIB_ENABLE_ARPACK)
Vector<double> sigma;
Matrix<double> V;
SparseSymmetricEigConfig cfg;
cfg.maxArnoldiIterations = 100000;
computeSparseSVD(A, V3D_ARPACK_SMALLEST_MAGNITUDE_EIGENVALUES, 3, sigma, V, cfg);
//computeSparseSVD(A, V3D_ARPACK_SMALLEST_EIGENVALUES, 3, sigma, V, cfg);
for (int i = 0; i < nViews; ++i)
{
copyMatrixSlice(V, 3*i, 0, 3, 1, rotations[i], 0, 2);
copyMatrixSlice(V, 3*i, 1, 3, 1, rotations[i], 0, 1);
copyMatrixSlice(V, 3*i, 2, 3, 1, rotations[i], 0, 0);
}
#endif
}
else
{
Matrix<double> AtA(3*nViews, 3*nViews);
multiply_At_A_SparseDense(A, AtA);
if (method == V3D_CONSISTENT_ROTATION_METHOD_SVD_ATA)
{
SVD<double> svd(AtA);
int const startColumn = A.num_cols()-3; // last columns of right sing. vec for SVD
Matrix<double> const& V = svd.getV();
for (int i = 0; i < nViews; ++i)
copyMatrixSlice(V, 3*i, startColumn, 3, 3, rotations[i], 0, 0);
}
else
{
Eigenvalue<double> svd(AtA);
int const startColumn = 0; // first columns of eigenvector matrix
Matrix<double> const& V = svd.getV();
for (int i = 0; i < nViews; ++i)
copyMatrixSlice(V, 3*i, startColumn, 3, 3, rotations[i], 0, 0);
} // end if
} // end if
break;
}
default:
throwV3DErrorHere("Unknown method argument for computeConsistentRotations().");
} // end switch
for (int i = 0; i < nViews; ++i)
enforceRotationMatrix(rotations[i]);
// Remove gauge freedom by setting R[0] = I.
Matrix3x3d const R0t = rotations[0].transposed();
for (int i = 0; i < nViews; ++i)
rotations[i] = rotations[i] * R0t;
// Note: it seems, that either all Rs have det(R)=1 or all have det(R)=-1.
// Since we remove the gauge freedem by multiplying all rotations with R_0^t,
// we always end up with det(R)=1 and any code to enforce a positive determinant
// is not necessary.
} // end computeConsistentRotations()
