void hueMatrix(GLfloat mat[3][3], float angle)
{
#define SQRT_2 sqrt(2.0)
#define SQRT_3 sqrt(3.0)
float mag, rot[3][3];
float xrs, xrc;
float yrs, yrc;
float zrs, zrc;
// Rotate the grey vector into positive Z
mag = SQRT_2;
xrs = 1.0/mag;
xrc = 1.0/mag;
xRotateMat(mat, xrs, xrc);
mag = SQRT_3;
yrs = -1.0/mag;
yrc = SQRT_2/mag;
yRotateMat(rot, yrs, yrc);
matrixMult(rot, mat, mat);
// Rotate the hue
zrs = sin(angle);
zrc = cos(angle);
zRotateMat(rot, zrs, zrc);
matrixMult(rot, mat, mat);
// Rotate the grey vector back into place
yRotateMat(rot, -yrs, yrc);
matrixMult(rot, mat, mat);
xRotateMat(rot, -xrs, xrc);
matrixMult(rot, mat, mat);
}
// ######################################################################
Image<float> TaskRelevanceMapTigs2::
getTDMap(Image<float> currGist)
{
Image<float> currGistCut(34,1,NO_INIT);
Image<float> currGistPCA(itsGistPCADims.getVal(), 1, NO_INIT);
Image<float> currImgPCA(itsImgPCADims.getVal(), 1, NO_INIT);
Image<float> tmp(300,1, NO_INIT); //300=20x15 (Rob's cvpr 2007 paper, 32x32 for 1 block)
Image<float> tmp2;
for(int i=0; i<34; i++)
{
currGistCut.setVal(i, 0, currGist.getVal(i*NUM_GIST_FEAT) );
}
currGistPCA = matrixMult(currGistCut, itsGistPCAMatrix);
currImgPCA = matrixMult(itsCurrFrameVec, itsImgPCAMatrix);
LINFO("the dim of gist is %d %d, of img is %d %d \n",
currGistCut.getHeight(), currGistCut.getWidth(),
itsCurrFrameVec.getHeight(), itsCurrFrameVec.getWidth());
Image<float> combo = concatX(currGistPCA,currImgPCA);
tmp = matrixMult(combo, itsTigsMatrix);
tmp2 = decode(tmp); // tranfer the 20x15 dims vector to an image
return tmp2;
}
/**
@param[in] vetrices array of vetrices (so far only for 3)
@param[in] E material constatn E
@param[in] mi material constatn mi
@param[in] fs volume force vector
@param[out] stifMat stifness matrix (array 36 long)
@param[out]
*/
void elastLoc(Point *vetrices[], PetscReal E, PetscReal mi, PetscReal *fs,
PetscReal *stifMat, PetscReal *bL) {
PetscReal
T[] =
{ 1, 1, 1, vetrices[0]->x, vetrices[1]->x, vetrices[2]->x, vetrices[0]->y, vetrices[1]->y, vetrices[2]->y };
PetscReal phiGrad[] = { vetrices[1]->y - vetrices[2]->y, vetrices[2]->x
- vetrices[1]->x, vetrices[2]->y - vetrices[0]->y, vetrices[0]->x
- vetrices[2]->x, vetrices[0]->y - vetrices[1]->y, vetrices[1]->x
- vetrices[0]->x
};
//PetscPrintf(PETSC_COMM_SELF, "DET:%e \n", matrixDet(T, 3));
PetscReal detT = fabs(matrixDet(T, 3));
PetscReal phiGradT[6];
matrixTranspose(phiGrad, 3, 2, phiGradT);
double Cmu[] = { 2, 0, 0, 0, 2, 0, 0, 0, 1 };
//double Clm[] = { 1, 1, 0, 1, 1, 0, 0, 0, 0 }; Why is this never used?
PetscReal C[] = { 1 - mi, mi, 0, mi, 1 - mi, 0, 0, 0, (1 - 2 * mi) / 2 };
matrixSum(C, E / ((1 + mi) * (1 - 2 * mi)), Cmu, 0, C, 3, 3);
double R[18], RT[18];
for (int i = 0; i < 18; i++)
R[i] = 0;
int idxR1[] = { 0, 2 };
int idxC1[] = { 0, 2, 4 };
int idxR2[] = { 2, 1 };
int idxC2[] = { 1, 3, 5 };
matrixSetSub(R, 3, 6, idxR1, 2, idxC1, 3, phiGradT);
matrixSetSub(R, 3, 6, idxR2, 2, idxC2, 3, phiGradT);
matrixTranspose(R, 3, 6, RT);
PetscReal temp[18];
matrixMult(C, 3, 3, R, 3, 6, temp);
matrixMult(RT, 6, 3, temp, 3, 6, stifMat);
matrixSum(stifMat, 1 / (2 * detT), stifMat, 0, stifMat, 6, 6);
//PetscPrintf(PETSC_COMM_SELF, "%e %e \n", fs[0], fs[1]);
//Volume Force
for (int i = 0; i < 3; i++) {
bL[i * 2] = detT / 6 * fs[0];
bL[i * 2 + 1] = detT / 6 * fs[1];
}
//@TODO Edge Force!!!
}
LIBRARY_API bool matrixPseudoInv (matrix &in, matrix &out)
{
matrix in_T, inTin, inTinInv;
bool state;
state = matrixTrans (in, in_T);
state = state ? matrixMult (in_T, in, inTin) : state;
state = state ? matrixInv (inTin, inTinInv) : state;
state = state ? matrixMult (inTinInv, in_T, out) : state;
return state;
}
vector<vector<int>> matrixExpo(const vector<vector<int>>& A, int pow) {
vector<vector<int>> result(A.size(), vector<int>(A.size()));
vector<vector<int>> A_exp(A);
for (int i = 0; i < A.size(); ++i) {
result[i][i] = 1;
}
while (pow) {
if (pow % 2 == 1) {
result = matrixMult(result, A_exp);
}
A_exp = matrixMult(A_exp, A_exp);
pow /= 2;
}
return result;
}
Image<double> AffineTransform::computeTransform(const CalibrationTransform::Data& d)
{
//loop through calib pts work out the transform M = pInv(e) x S
//set itsTransform= new transform image
const std::vector<CalPt>& itsCals = d.getItsCalPts();
int numCalPts = (int)itsCals.size();
Image<double> scr(3,numCalPts,ZEROS);
Image<double> eye(3,numCalPts,ZEROS);
Image<double> eyeInv;
Image<double> M(3,3,ZEROS);
Image<double>::iterator scrAptr = scr.beginw();
Image<double>::iterator eyeAptr = eye.beginw();
for(int i=0; i < numCalPts; i++)
{
*scrAptr++ = itsCals[i].scr_x;
*scrAptr++ = itsCals[i].scr_y;
*scrAptr++ = 1;
*eyeAptr++ = itsCals[i].raw_x;
*eyeAptr++ = itsCals[i].raw_y;
*eyeAptr++ = 1;
}
int rank =0;
eyeInv = svdPseudoInv(eye,SVD_LAPACK,&rank,0.0F);
M = matrixMult(eyeInv,scr);
itsTransform = M;
return itsTransform;
}
double computeLinearityIndex(const State &state, const Matd &stateCovarianceMatrix, const MapFeature *mapFeature)
{
int invDepthErrorCovMatrixIndex = mapFeature->covarianceMatrixPos + mapFeature->positionDimension - 1;
double invDepthError = sqrt(stateCovarianceMatrix[invDepthErrorCovMatrixIndex][invDepthErrorCovMatrixIndex]);
double invDepthValue = mapFeature->position[mapFeature->positionDimension - 1];
double sigmaMapFeaturePosError = invDepthError / (invDepthValue * invDepthValue);
double currMapFeatureXYZPos[3] = {0};
changeInverseDepthToDepth(mapFeature->position, currMapFeatureXYZPos);
double currMapFeatureXYZToCamPos[3] = {0.0L};
double currMapFeatureXYZToFirstSeenCamPos[3] = {0.0L};
matrixSubs(currMapFeatureXYZPos, state.position, 1, 3, currMapFeatureXYZToCamPos);
matrixSubs(currMapFeatureXYZPos, mapFeature->position, 1, 3, currMapFeatureXYZToFirstSeenCamPos);
double dotProduct = 0.0L;
matrixMult(currMapFeatureXYZToCamPos, 1, 3, currMapFeatureXYZToFirstSeenCamPos, 1, &dotProduct);
double currMapFeatureXYZToFirstSeenCamPosDistance = euclideanNorm3(currMapFeatureXYZToFirstSeenCamPos);
double currMapFeatureXYZToCamPosDistance = euclideanNorm3(currMapFeatureXYZToCamPos);
double cosAlphaDivDistance = dotProduct / (currMapFeatureXYZToFirstSeenCamPosDistance * currMapFeatureXYZToCamPosDistance);
double result = 4.0L * sigmaMapFeaturePosError * cosAlphaDivDistance / currMapFeatureXYZToCamPosDistance;
return result;
}
void ApplyHouse(Matrix A,Vector v, int start, int end)
{
Matrix M,H;
M = newMatrix();
H = newMatrix();
HouseMatrix(H,v,0,n-1);
/* Apply it A=H*A*H */
matrixMult(M,A,H);
matrixMult(A,H,M);
freeMatrix(H);
freeMatrix(M);
}
void myRotatef(GLfloat *matrix,
GLfloat angle, GLfloat x, GLfloat y, GLfloat z){
// Remember the Rodrigues' rotation formula?
// R = I + S * sin(theta) + Ssquared * (1 - cos(theta))
GLfloat identity[16];
GLfloat skewMatrix[16];
GLfloat skewMatrixSqr[16];
GLfloat skewXsin[16];
GLfloat skewSqrXcos[16];
GLfloat radAngle = DEG2RAD(angle);
identity[0] = 1; identity[4] = 0; identity[8] = 0; identity[12] = 0;
identity[1] = 0; identity[5] = 1; identity[9] = 0; identity[13] = 0;
identity[2] = 0; identity[6] = 0; identity[10] = 1; identity[14] = 0;
identity[3] = 0; identity[7] = 0; identity[11] = 0; identity[15] = 1;
skewMatrix[0] = 0; skewMatrix[4] = z; skewMatrix[8] = -y; skewMatrix[12] = 0;
skewMatrix[1] = -z; skewMatrix[5] = 0; skewMatrix[9] = x; skewMatrix[13] = 0;
skewMatrix[2] = y; skewMatrix[6] = -x; skewMatrix[10] = 0; skewMatrix[14] = 0;
skewMatrix[3] = 0; skewMatrix[7] = 0; skewMatrix[11] = 0; skewMatrix[15] = 0;
matrixMult(skewMatrixSqr, skewMatrix, skewMatrix);
for (int i = 0; i < 16; i++)
{
skewXsin[i] = skewMatrix[i] * sin(radAngle);
skewSqrXcos[i] = skewMatrixSqr[i] * (1 - cos(radAngle));
}
for (int i = 0; i < 16; i++)
{
matrix[i] = identity[i] + skewXsin[i] + skewSqrXcos[i];
}
}
void object::applyTrans(float **result, float **vertex, long vertCnt, bool doProj, float **proj)
{
long i;
float **matrixA;
float *tempA;
if (doProj)
{
matrixA = matrixAlloc();
tempA = new float[4];
matrixMult(matrixA, mTrans, proj);
for (i = 0; i < vertCnt; i++)
{
vectorMatrixMultEx(tempA, vertex[i], matrixA);
result[i][0] = tempA[0] / tempA[3];
result[i][1] = tempA[1] / tempA[3];
result[i][2] = tempA[2] / tempA[3];
}
matrixFree(matrixA);
delete []tempA;
}
else
for (i = 0; i < vertCnt; i++)
vectorMatrixMult(result[i], vertex[i], mTrans);
}
Matrix
build_A(int m){
Matrix K = build_K(2*m);
Matrix L = build_L(2*m);
Matrix A = matrixMult(K, L);
freeMatrix(K);
freeMatrix(L);
return A;
}
// ######################################################################
Image<float> TaskRelevanceMapTigs::
getTDMap(Image<float> currGist)
{
Image<float> currGistCut(34,1,NO_INIT);
Image<float> currGistPCA(itsPCADims.getVal(), 1, NO_INIT);
Image<float> tmp(300,1, NO_INIT); //300=20x15 (Rob's cvpr 2007 paper, 32x32 for 1 block)
Image<float> tmp2;
for(int i=0; i<34; i++)
{
currGistCut.setVal(i, 0, currGist.getVal(i*NUM_GIST_FEAT) );
}
currGistPCA = matrixMult(currGistCut, itsPCAMatrix);
tmp = matrixMult(currGistPCA, itsTigsMatrix);
tmp2 = decode(tmp); // tranfer the 20x15 dims vector to an image
return tmp2;
}
void object::buildTrans()
{
matrixTransEx(mTrans, mOrigin, mAngle, mScale);
mChanged = true;
if (mParent != NULL)
{
if (!mParent->mChanged)
mParent->buildTrans();
matrixMult(mTrans, mTrans, mParent->mTrans);
}
}
void Camera::rotateY( double angle )
{
double matrix[3][3];
matrix[0][0] = cos( angle ); matrix[0][1] = 0.0; matrix[0][2] = sin( angle );
matrix[1][0] = 0.0; matrix[1][1] = 1.0; matrix[1][2] = 0.0;
matrix[2][0] = -sin( angle ); matrix[2][1] = 0.0; matrix[2][2] = cos( angle );
_eye = matrixMult( matrix, _eye );
computeDerivedParameters();
}
float *vectorFromAngles(float *angles)
{
float *result;
float **tempA;
float **tempB;
float **applyTrans;
tempA = matrixRotation('x', angles[1]);
tempB = matrixRotation('y', angles[2]);
applyTrans = matrixIdent(4);
applyTrans = matrixUpdate(applyTrans, matrixMult(applyTrans, tempA, 4), 4);
applyTrans = matrixUpdate(applyTrans, matrixMult(applyTrans, tempB, 4), 4);
result = vectorNull(4);
result[2] = 1;
result[3] = 1;
result = vectorUpdate(result, vectorMatrixMult(result, applyTrans, 4));
matrixFree(tempA, 4);
matrixFree(tempB, 4);
matrixFree(applyTrans, 4);
return result;
}
void myLookAt(GLfloat *matrix,
GLdouble eyeX, GLdouble eyeY, GLdouble eyeZ,
GLdouble centerX, GLdouble centerY, GLdouble centerZ,
GLdouble upX, GLdouble upY, GLdouble upZ){
// Please implement this function.
GLdouble gaze[3] = { centerX - eyeX, centerY - eyeY, centerZ - eyeZ };
GLdouble up[3] = { upX, upY, upZ };
GLdouble normGaze[3];
double gazeLength;
GLdouble normUp[3];
double upLength;
GLdouble right[3];
GLdouble normRight[3];
double rightLength;
GLfloat translate[16];
GLfloat modelView[16];
gazeLength = sqrt(SQR(gaze[0]) + SQR(gaze[1]) + SQR(gaze[2]));
for (int i = 0; i < 3; i++)
{
normGaze[i] = gaze[i] / gazeLength;
}
upLength = sqrt(SQR(up[0]) + SQR(up[1]) + SQR(up[2]));
for (int i = 0; i < 3; i++)
{
normUp[i] = up[i] / upLength;
}
right[0] = normGaze[1] * normUp[2] - normGaze[2] * normUp[1];
right[1] = normGaze[2] * normUp[0] - normGaze[0] * normUp[2];
right[2] = normGaze[0] * normUp[1] - normGaze[1] * normUp[0];
rightLength = sqrt(SQR(right[0]) + SQR(right[1]) + SQR(right[2]));
for (int i = 0; i < 3; i++)
{
normRight[i] = right[i] / rightLength;
}
modelView[0] = normRight[0]; modelView[4] = normRight[1]; modelView[8] = normRight[2]; modelView[12] = 0;
modelView[1] = normUp[0]; modelView[5] = normUp[1]; modelView[9] = normUp[2]; modelView[13] = 0;
modelView[2] = -normGaze[0]; modelView[6] = -normGaze[1]; modelView[10] = -normGaze[2]; modelView[14] = 0;
modelView[3] = 0; modelView[7] = 0; modelView[11] = 0; modelView[15] = 1;
myTranslatef(translate, -eyeX, -eyeY, -eyeZ);
matrixMult(matrix, modelView, translate);
}
// This function first generates a transformation matrix for the camera
// Then it multiplies it with all the transforms in the fractal tree leaves...
void generateOGLTransforms(float ftime)
{
float quaternion[2][4];
float distance[2];
float finalTransform[4][4];
randomQuaternion(quaternion[0], 0.0f);
randomQuaternion(quaternion[1], 0.0f);
// linear interpolation of the two quaternions
float invQuatSize = 0.0f;
for (int dim = 0; dim < 4; dim++)
{
quaternion[0][dim] = 0.5f*ftime * quaternion[1][dim] +
(1.0f - 0.5f*ftime) * quaternion[0][dim];
invQuatSize += quaternion[0][dim] * quaternion[0][dim];
}
invQuatSize = 1.0f / sqrtf(invQuatSize);
for (int dim = 0; dim < 4; dim++)
{
quaternion[0][dim] *= invQuatSize;
}
matrixFromQuaternion(quaternion[0], finalTransform);
distance[0] = frand(&transformationSeed) - 0.2f;
distance[1] = frand(&transformationSeed) + 0.2f;
finalTransform[2][3] = ftime * distance[0] + (1.0f - ftime) * distance[1];
// multiply camera transform with leaf matrices
for (int draw = 0; draw < FRACTAL_NUM_PRE_LEAVES; draw++)
{
float tmpMatrix[4][4];
matrixMult(finalTransform, fractalTree[firstPreLeaf+draw], tmpMatrix);
for (int s = 0; s < 4; s++)
{
for (int t = 0; t < 4; t++)
{
fractalTree[firstPreLeaf+draw][s][t] = tmpMatrix[s][t];
}
}
// encode the color in the matrix
fractalTree[firstPreLeaf+draw][3][0] = fractalColorTree[firstPreLeaf+draw][0];
fractalTree[firstPreLeaf+draw][3][1] = fractalColorTree[firstPreLeaf+draw][1];
fractalTree[firstPreLeaf+draw][3][2] = fractalColorTree[firstPreLeaf+draw][2];
int location = glGetUniformLocation(shaderPrograms[0], "t");
glUniformMatrix4fv(location, 1, GL_FALSE, &(fractalTree[firstPreLeaf+draw][0][0]));
glDrawArrays(GL_POINTS, 0, FRACTAL_NUM_LEAVES);
}
}
void buildTree(void)
{
// Set the first entry in the tree to unity (assuming all zeroes)
fractalTree[0][0][0] = 1.0f;
fractalTree[0][1][1] = 1.0f;
fractalTree[0][2][2] = 1.0f;
fractalTree[0][3][3] = 1.0f;
fractalColorTree[0][0] = 1.0f;
fractalColorTree[0][1] = 1.0f;
fractalColorTree[0][2] = 1.0f;
int startEntry = 0;
int endEntry = 1;
for (int depth = 1; depth < FRACTAL_TREE_DEPTH; depth++)
{
firstPreLeaf = firstTreeLeaf;
firstTreeLeaf = startEntry;
// seed is stored...
for (int entry = startEntry; entry < endEntry; entry++)
{
for (int transform = 0; transform < 4; transform++)
{
int destEntry = entry * 4 + 1 + transform;
matrixMult(transformMat[transform], fractalTree[entry],
fractalTree[destEntry]);
// Create the color:
for (int col = 0; col < 3; col++)
{
fractalColorTree[destEntry][col] =
0.625f * fractalColorTree[entry][col] +
0.375f * transformColor[transform][col];
}
fractalColorTree[destEntry][3] = frand(&transformationSeed);
}
}
startEntry = endEntry;
endEntry += 1 << (2*depth);
}
}
Point2D<double> AffineTransform::getCalibrated(const Point2D<double>& raw)
{
Image<double> rawIm(3,1,ZEROS);
Image<double> resultIm(3,1,ZEROS);
Point2D<double> calibPt;
Image<double>::iterator Aptr = rawIm.beginw();
*Aptr++ = raw.i; //x_raw
*Aptr++ = raw.j; //y_raw
*Aptr++ = 1;
resultIm = matrixMult(rawIm,itsTransform);
Aptr = resultIm.beginw();
calibPt.i = *Aptr++;
calibPt.j = *Aptr;
return calibPt;
}
// gluLookAt() equivalent
void matrixLookAt( float result[16],
const float eye[3],
const float target[3],
const float up[3])
{
float forward[3] = { target[0] - eye[0],
target[1] - eye[1],
target[2] - eye[2]
};
vecNormalize(forward);
float side[3];
vecCross(side, forward, up);
float up_after[3];
vecCross(up_after, side, forward);
float view_matrix[16] = MATRIX_IDENTITY;
view_matrix[0] = side[0];
view_matrix[4] = side[1];
view_matrix[8] = side[2];
view_matrix[1] = up_after[0];
view_matrix[5] = up_after[1];
view_matrix[9] = up_after[2];
view_matrix[2] = -forward[0];
view_matrix[6] = -forward[1];
view_matrix[10] = -forward[2];
// Final translation
float trans_matrix[16];
float minus_eye[3] = {-eye[0], -eye[1], -eye[2]};
matrixTranslate(trans_matrix, minus_eye);
matrixMult(result, view_matrix, trans_matrix);
}
// QR Factorization
void QR(double* A, int m, int n, double* Q, double* R)
{
// Not a very efficient approach, but simple and effective
// Written by Nico van Duijn, 9/13/2015
// Source of algorithm (adjusted by me):
// http://www.keithlantz.net/2012/05/qr-decomposition-using-householder-transformations/
// A is the (m*n) matrix to be factorized A=Q*R
double mag, alpha;
double u[m], v[m], vtrans[m];
double P[m * m], I[m * m], vvtrans[m * m], Rbackup[m * m], Qbackup[m * m];
matrixCopy(A, m, m, R); // Initialize R to A
// Initialize Q, P, I to Identity
for (int i = 0; i < m * m; i++)Q[i] = 0;
for (int i = 0; i < m; i++)Q[i * n + i] = 1;
for (int i = 0; i < m * m; i++)P[i] = 0;
for (int i = 0; i < m; i++)P[i * n + i] = 1;
for (int i = 0; i < m * m; i++)I[i] = 0;
for (int i = 0; i < m; i++)I[i * n + i] = 1;
for (int i = 0; i < n; i++) //loop through all columns
{
for (int q = 0; q < m; q++)u[q] = 0; // set u and v to zero
for (int q = 0; q < m; q++)v[q] = 0;
mag = 0.0; //set mag to zero
for (int j = i; j < m; j++)
{
u[j] = R[j * n + i];
mag += u[j] * u[j];
}
mag = sqrt(mag);
alpha = u[i] < 0 ? mag : -mag;
mag = 0.0;
for (int j = i; j < m; j++)
{
v[j] = j == i ? u[j] + alpha : u[j];
mag += v[j] * v[j];
}
mag = sqrt(mag);
if (mag < 0.0000000001) continue;
for (int j = i; j < m; j++) v[j] /= mag;
// P = I - (v * v.transpose()) * 2.0;
matrixTranspose(v, m, 1, vtrans);
matrixMult(v, vtrans, m, 1, m, vvtrans);
matrixScale(vvtrans, m, m, 2.0);
matrixSub(I, vvtrans, m, m, P);
// R = P * R;
matrixMult(P, R, m, m, m, Rbackup);
matrixCopy(Rbackup, m, m, R);
//Q = Q * P;
matrixMult(Q, P, m, m, m, Qbackup);
matrixCopy(Qbackup, m, m, Q);
}
}
// ######################################################################
Image<float> TaskRelevanceMapGistClassify::
computeGistDist(Image<float> currGist)
{
FILE* itsFile = fopen(itsClusterCenterFileName.getVal().c_str(), "rb");
ASSERT(itsFile != 0);
if(fread(&itsNumOfCategory, sizeof(int), 1, itsFile) != 1) LFATAL("fread failed");
if(fread(&itsUsedDims, sizeof(int),1,itsFile) != 1) LFATAL("fread failed");
ASSERT(itsNumOfCategory > 0 && itsUsedDims == itsPCADims.getVal());
LDEBUG("there are %4d categories, pca_dims: %4d",itsNumOfCategory, itsUsedDims);
Image<float> currGistCut(currGist.getDims().sz()/NUM_GIST_FEAT, 1, NO_INIT);
Image<float> currGistPCA(itsPCADims.getVal(), 1, NO_INIT);
//! only use the whole image's gist feature to do the classification
for(int i=0; i<currGistCut.getDims().sz(); i++)
currGistCut.setVal(i,0, currGist.getVal(i*NUM_GIST_FEAT, 0));
LDEBUG("currGistCut dim: %d, PCA matrix height: %4d",
currGistCut.getWidth(), itsPCAMatrix.getHeight());
ASSERT(currGistCut.getWidth() == itsPCAMatrix.getHeight());
currGistPCA = matrixMult(currGistCut, itsPCAMatrix);
LDEBUG("currGistPCA : %f, %f, %f", currGistPCA.getVal(0,0),
currGistPCA.getVal(1,0), currGistPCA.getVal(2,0));
Image<float> gistDist(itsNumOfCategory, 1,NO_INIT );
Image<float> gistCenter(itsUsedDims, 1, NO_INIT);
Image<float> gistCenterCovarMatrix(itsUsedDims,itsUsedDims, NO_INIT);
Image<float> gistCenterCovarMatrixInv(itsUsedDims, itsUsedDims, NO_INIT);
Image<float> gistDiff(itsUsedDims,1,NO_INIT);
float meanClusterGistDist=0.0F;
float det = 0.0F;
float coef1 = pow(2*PI, itsUsedDims/2.0F);
float coef2 = 0.0F, coef3=0.0F, coef4=0.0F;
for(int i=0; i<itsNumOfCategory; i++)
{
if(fread(gistCenter.beginw(), sizeof(float), itsUsedDims, itsFile) != (size_t)itsUsedDims) LFATAL("fread failed");
if(fread(&meanClusterGistDist, sizeof(float), 1, itsFile) != 1) LFATAL("fread failed");
if(fread(&det, sizeof(float), 1, itsFile) != 1) LFATAL("fread failed");
size_t sz = itsUsedDims*itsUsedDims;
if(fread(gistCenterCovarMatrix.beginw(), sizeof(float), sz, itsFile) != sz) LFATAL("fread failed");
gistCenterCovarMatrixInv = matrixInv(gistCenterCovarMatrix);
gistDiff = gistCenter - currGistPCA;
coef2 =1.0F /( coef1 * pow(det, 0.5)+0.0000001F );
Image<float> tmp = matrixMult(matrixMult(gistDiff, gistCenterCovarMatrixInv),
transpose(gistDiff));
coef3 = exp(-0.5F * tmp.getVal(0,0));
coef4 = coef2 * coef3 / (meanClusterGistDist+0.5F);
LDEBUG("%d's is %f, %f, %f , mean is %f,sumDiff is%f\n", i+1, tmp.getVal(0,0),
coef3, coef4, meanClusterGistDist, sum(gistDiff*gistDiff));
gistDist.setVal(i, 0, coef4);
}
fclose(itsFile);
return gistDist;
}
double nuGibbs(double *nu,int Q,int G,const int *S,double gamma2,
const double *tau2Rho,const double *a,const double *rho,
const double *sigma2,const double *phi,
const int *psi,const double *x,
const int *delta,const double *Delta,Random &ran,int draw) {
double pot = 0.0;
int g;
for (g = 0; g < G; g++) {
//
// compute prior covariance matrix
//
std::vector<std::vector<double> > var;
makeSigma(g,G,var,Q,gamma2,tau2Rho,a,sigma2,rho);
//
// define prior mean
//
std::vector<double> Mean(Q,0.0);
//
// compute extra linear and quadratic terms
//
std::vector<double> lin(Q,0.0);
std::vector<double> quad(Q,0.0);
int s;
int q;
for (q = 0; q < Q; q++) {
int kqg = qg2index(q,g,Q,G);
double var0 = sigma2[kqg] * phi[kqg];
double var1 = sigma2[kqg] / phi[kqg];
for (s = 0; s < S[q]; s++) {
int ksq = sq2index(s,q,S,Q);
double variance = psi[ksq] == 0 ? var0 : var1;
quad[q] += 1.0 / variance;
int ksqg = sqg2index(s,q,g,S,Q,G);
lin[q] += (x[ksqg] - delta[kqg] * (2.0 * psi[ksq] - 1.0) *
Delta[kqg]) / variance;
}
}
//
// Update parameters based on available observations
//
std::vector<std::vector<double> > varInv;
double detPrior = inverse(var,varInv);
for (q = 0; q < Q; q++) {
varInv[q][q] += quad[q];
Mean[q] += lin[q];
}
double detPosterior = 1.0 /inverse(varInv,var);
std::vector<double> mean(Q,0.0);
matrixMult(var,Mean,mean);
//
// Draw new values
//
std::vector<double> vv(Q,0.0);
if (draw == 1)
vv = ran.MultiGaussian(var,mean);
else {
for (q = 0; q < Q; q++) {
int kqg = qg2index(q,g,Q,G);
vv[q] = nu[kqg];
}
}
pot += ran.PotentialMultiGaussian(var,mean,vv);
if (draw == 1) {
for (q = 0; q < Q; q++) {
int kqg = qg2index(q,g,Q,G);
nu[kqg] = vv[q];
}
}
}
return pot;
}
void computeAddFeatureJacobian(const double *orientation,
const double *rotationMatrixWC,
const double *featureImagePos,
double *jacobianPositionAndOrientationSubmatrix,
double *jacobianHAndRhoSubmatrix)
{
CameraCalibration *cameraCalib = ConfigurationManager::getInstance().cameraCalibration;
double undistortedPoint[2] = {0};
undistortPoint(featureImagePos, undistortedPoint);
// Feature con respecto a la camara pasado al espacio proyectivo
double x_c = -(cameraCalib->cx - undistortedPoint[0]) / cameraCalib->fx;
double y_c = -(cameraCalib->cy - undistortedPoint[1]) / cameraCalib->fy;
double z_c = 1.0L;
double xyz_c[3] = {0};
xyz_c[0] = x_c;
xyz_c[1] = y_c;
xyz_c[2] = z_c;
double xyz_w[3] = {0};
multiplyRotationMatrixByVector(rotationMatrixWC, xyz_c, xyz_w);
double x_w = xyz_w[0];
double y_w = xyz_w[1];
double z_w = xyz_w[2];
double xx_plus_zz = x_w*x_w + z_w*z_w;
double dtheta_dgw[3] = {0};
dtheta_dgw[0] = z_w / xx_plus_zz;
dtheta_dgw[1] = 0;
dtheta_dgw[2] = -x_w / xx_plus_zz;
double sqrt_xx_plus_zz = sqrt(x_w*x_w + z_w*z_w);
double norm_sq = xx_plus_zz + y_w*y_w;
double dphi_dgw[3] = {0};
dphi_dgw[0] = x_w * y_w / (norm_sq * sqrt_xx_plus_zz);
dphi_dgw[1] = -sqrt_xx_plus_zz / norm_sq;
dphi_dgw[2] = z_w * y_w / (norm_sq * sqrt_xx_plus_zz);
double dgw_dqwr[12] = {0};
makeJacobianOfQuaternionToRotationMatrix(orientation, xyz_c, dgw_dqwr);
double dtheta_dqwr[4] = {0};
matrixMult(dtheta_dgw, 1, 3, dgw_dqwr, 4, dtheta_dqwr);
double dphi_dqwr[4] = {0};
matrixMult(dphi_dgw, 1, 3, dgw_dqwr, 4, dphi_dqwr);
// int derivativeResultRows = 6;
int derivativeResultCols = 7;
for (int i = 0; i < 3; ++i)
{
jacobianPositionAndOrientationSubmatrix[i*derivativeResultCols + i] = 1.0L;
}
for (int i = 0; i < 4; ++i)
{
// 4ta fila, columnas 3, 4, 5 y 6
jacobianPositionAndOrientationSubmatrix[3*derivativeResultCols + i+3] = dtheta_dqwr[i];
// 5ta fila, columnas 3, 4, 5 y 6
jacobianPositionAndOrientationSubmatrix[4*derivativeResultCols + i+3] = dphi_dqwr[i];
}
// Para las siguientes lineas de codigo conviene ver el archivo de MATLAB add_a_feature_covariance_inverse_depth.m
// Multiplicacion de dyprima_dgw * dgw_dgc.
// subresult_dgw_dgc: 2x3
double subresult_dgw_dgc[6] = {0};
matrixMult(dtheta_dgw, 1, 3, rotationMatrixWC, 3, &subresult_dgw_dgc[0]);
matrixMult(dphi_dgw, 1, 3, rotationMatrixWC, 3, &subresult_dgw_dgc[3]);
// multiplicacion de dyprima_dgw * dgw_dgc * dgc_dhu.
double fku_inv = 1.0L / cameraCalib->fx;
double fkv_inv = 1.0L / cameraCalib->fy;
// dgc_dhu: 3x2
double dgc_dhu[6] = {fku_inv, 0, 0, fkv_inv, 0, 0};
// subresult_dgc_dhu: 2x2
double subresult_dgc_dhu[4] = {0};
matrixMult(subresult_dgw_dgc, 2, 3, dgc_dhu, 2, subresult_dgc_dhu);
double dhu_dhd[4] = {0};
computeUndistortPointJacobian(featureImagePos, dhu_dhd);
// jacobianHAndRhoSubmatrix:6x3 de la forma
// 0 0 0
// 0 0 0
// 0 0 0
// a b 0
// c d 0
// 0 0 1
// subresult_dhu_dhd: 2x2
double subresult_dhu_dhd[4] = {0};
matrixMult(subresult_dgc_dhu, 2, 2, dhu_dhd, 2, subresult_dhu_dhd);
jacobianHAndRhoSubmatrix[9] = subresult_dhu_dhd[0];
jacobianHAndRhoSubmatrix[10] = subresult_dhu_dhd[1];
jacobianHAndRhoSubmatrix[12] = subresult_dhu_dhd[2];
jacobianHAndRhoSubmatrix[13] = subresult_dhu_dhd[3];
jacobianHAndRhoSubmatrix[17] = 1.0L;
}
double DeltaGibbs(int g,double *Delta,int Q,int G,const int *S,double c2,
const double *tau2R,const double *b,const double *r,
const double *sigma2,const double *phi,
const int *psi,const double *x,
const int *delta,const double *nu,Random &ran,int draw) {
double pot = 0.0;
//
// compute prior covariance matrix
//
int dim = 0;
std::vector<int> on(Q,0);
int q;
for (q = 0; q < Q; q++) {
int kqg = qg2index(q,g,Q,G);
if (delta[kqg] == 1) {
on[q] = 1;
dim++;
}
}
if (dim > 0) {
std::vector<std::vector<double> > var;
makeSigma(g,G,var,on,Q,c2,tau2R,b,sigma2,r);
//
// define prior mean
//
std::vector<double> Mean(dim,0.0);
std::vector<double> meanPrior(Mean);
//
// compute extra linear and quadratic terms
//
std::vector<double> mean(dim,0.0);
std::vector<double> lin(dim,0.0);
std::vector<double> quad(dim,0.0);
int s;
int k = 0;
for (q = 0; q < Q; q++) {
if (on[q] == 1) {
int kqg = qg2index(q,g,Q,G);
double var0 = sigma2[kqg] * phi[kqg];
double var1 = sigma2[kqg] / phi[kqg];
int s;
for (s = 0; s < S[q]; s++)
{
int ksq = sq2index(s,q,S,Q);
double variance = psi[ksq] == 0 ? var0 : var1;
quad[k] += 1.0 / variance;
int xIndex = sqg2index(s,q,g,S,Q,G);
lin[k] += (2.0 * psi[ksq] - 1.0) * (x[xIndex] - nu[kqg]) / variance;
}
k++;
}
}
//
// Update parameters based on available observations
//
std::vector<std::vector<double> > varInv;
double detPrior = inverse(var,varInv);
std::vector<std::vector<double> > covInvPrior(varInv);
for (k = 0; k < dim; k++) {
Mean[k] += lin[k];
varInv[k][k] += quad[k];
}
double detPosterior = 1.0 / inverse(varInv,var);
matrixMult(var,Mean,mean);
//
// Draw new values
//
std::vector<double> vv(dim,0.0);
if (draw == 1)
vv = ran.MultiGaussian(var,mean);
else {
k = 0;
for (q = 0; q < Q; q++) {
if (on[q] == 1) {
int kqg = qg2index(q,g,Q,G);
vv[k] = Delta[kqg];
k++;
}
}
}
pot += ran.PotentialMultiGaussian(var,mean,vv);
if (draw == 1) {
k = 0;
for (q = 0; q < Q; q++) {
if (on[q] == 1) {
int kqg = qg2index(q,g,Q,G);
Delta[kqg] = vv[k];
k++;
}
}
}
}
return pot;
}
// ######################################################################
// open a testing file containing images and corresponding ground truth
void setupCases(std::string folder, std::string fname, bool equalize)
{
char comment[200];
FILE *fp;
char inLine[100];
// open a file that lists the sample with ground truth
std::string name = folder + fname;
if((fp = fopen(name.c_str(),"rb")) == NULL)
{
LINFO("samples file: %s not found", name.c_str());
// input and output vector
out.resize(0);
in.resize(0);
nSamples = 0;
return;
}
LINFO("tName: %s",name.c_str());
// get number of samples
if (fgets(inLine, 1000, fp) == NULL) LFATAL("fgets failed");
sscanf(inLine, "%d %s", &nSamples, comment);
// the number of categories -> has to agree with the training file
uint tNout;
if (fgets(inLine, 1000, fp) == NULL) LFATAL("fgets failed");
sscanf(inLine, "%d %s", &tNout, comment);
if(tNout != info->nOutput)
LFATAL("Num categories differ: %d != %d", tNout, info->nOutput);
// get the type of ground truth
char gtOpt[100];
int gtType = -1;
if (fgets(inLine, 1000, fp) == NULL) LFATAL("fgets failed");
sscanf(inLine, "%s %s", gtOpt, comment);
if(strcmp(gtOpt,"ABSOLUTE") == 0)
gtType = ABSOLUTE;
else if(strcmp(gtOpt,"MIXTURE" ) == 0)
gtType = MIXTURE;
else
LFATAL("unknown ground truth type %s",gtOpt);
// set up the size input and output vector
out.resize(nSamples);
in.resize(nSamples);
// skip column headers
if (fgets(inLine, 1000, fp) == NULL) LFATAL("fgets failed");
char cName[100];
char sName[100];
char iName[100];
char ext[100];
int cStart, cNum;
int gTruth;
FILE *ifp;
int count = 0;
int tSamples = 0;
std::vector<uint> nSamples;
while(fgets(inLine, 1000, fp) != NULL)
{
if(gtType == ABSOLUTE)
{
// get the files in this category and ground truth
sscanf(inLine, "%s %d %d %d %s", cName, &cStart, &cNum, &gTruth, ext);
sprintf(sName,"%s%s", folder.c_str(), cName);
printf(" sName: %s %d %d %d %s\n",sName, cStart, cNum, gTruth, ext);
}
else if(gtType == MIXTURE)
{
// get the files in this category and ground truth
//char tStr[300];
//sscanf(inLine, "%s %d %d %s %s", cName, &cStart, &cNum, tStr, ext);
//sprintf(sName,"%s%s", folder, cName);
//printf(" sName: %s %d %d %d %s\n",sName, cStart, cNum, gTruth, ext);
// change to mixture values
LFATAL("MIXTURE ground truth type not yet implemented");
}
else LFATAL("unknown ground truth type %s",gtOpt);
nSamples.push_back(cNum);
// go through every sample
for(int j = cStart; j < cStart+cNum; j++)
{
tSamples++;
// get the corresponding vector file (if exist)
sprintf(iName,"%s%06d%s", sName,j,ext);
// open the file
if((ifp = fopen(iName,"rb")) != NULL)
{
Image<double> tData(1,info->oriFeatSize, NO_INIT);
Image<double>::iterator aptr = tData.beginw();
for(int i = 0; i < tData.getSize(); i++)
{
double val;
if (fread(&val, sizeof(double), 1, ifp) != 1) LFATAL("fread failed");
*aptr++ = val;
}
LINFO("feature file found: %s (%d)",//%7.4f %7.4f %7.4f %7.4f\n",
iName,gTruth);//,tData[0], tData[21], tData[42], tData[63]);
fclose(ifp);
// calculate the reduced features
if(info->isPCA) in[count] = matrixMult(pcaIcaMatrix, tData);
else in[count] = tData;
// load the ground truth
if(gtType == ABSOLUTE)
{
Image<double> res(1,info->nOutput, ZEROS);
res.setVal(0, gTruth, 1.0);
out[count] = res;
}
else if(gtType == MIXTURE)
{
LFATAL("MIXTURE ground truth type not yet implemented");
}
else LFATAL("unknown ground truth type %s",gtOpt);
// // just to test stuff
// for(int k = 0; k < info->oriFeatSize; k++)
// printf("ori[%7d]: %f \n", k, tData.getVal(k));
// printf("\n");
// for(int k = 0; k < info->redFeatSize; k++)
// printf("red[%7d]: %f \n", k, in[count].getVal(k));
// printf("\n");
// //for(uint k = 0; k < info->nOutput; k++)
// // printf("%f \n",out[count].getVal(k));
// Raster::waitForKey();
count++;
}
else LFATAL("file: %s not found\n",iName);
}
}
// equalize the number of samples if requested
if(equalize)
{
// find the max
uint max = 0;
// for(uint i = 0; i < nSamples.size(); i++)
// if(max < nSamples[i]) max = nSamples[i];
max = *max_element(nSamples.begin(),nSamples.end());
LINFO("max element: %d", max);
uint offset = 0;
for(uint i = 0; i < nSamples.size(); i++)
{
LINFO("extra samples for class[%3d]: %d - %d -> %d",
i, max, nSamples[i], max - nSamples[i]);
for(uint j = 0; j < max - nSamples[i]; j++)
{
// index to be copied
uint index = rand()/(RAND_MAX + 1.0) * nSamples[i];
LINFO("[%d] Duplicating class[%3d] sample[%3d]"
" -> actual ind: %3d",
j, i, index, index + offset);
index = index + offset;
in.push_back(in[index]);
out.push_back(out[index]);
}
offset += nSamples[i];
}
LINFO("Total samples before equalized: %d \n",tSamples);
tSamples = in.size();
}
LINFO("Actual total samples: %d \n",tSamples);
fclose(fp);
}
void loop()
{
// Room for the two matricies
int** a = (int**) malloc(ra*sizeof(int*));
for(int i=0; i<ra; i++)
a[i] = malloc(carb*sizeof(int));
int** b = (int**) malloc(carb*sizeof(int*));
for(int i=0; i<carb; i++)
b[i] = malloc(ra*sizeof(int));
// Show and randomly fill A
printf("Matrix A:\n");
for (int i=0; i<ra;i++) {
for (int j=0; j<carb; j++) {
a[i][j] = rand();
printf("%i ", a[i][j]);
}
printf("\n");
}
// Show and randomly fill B
printf("Matrix B:\n");
for (int i=0; i<carb;i++) {
for (int j=0; j<cb; j++) {
b[i][j] = (int) rand();
printf("%i ", b[i][j]);
}
printf("\n");
}
int startCalcTime = micros();
int** result = matrixMult(a, b, ra, carb, cb);
int endCalcTime = micros();
// Show the mult result
printf("Mult result:\n");
int startTransTime = micros();
for (int i=0; i<ra; i++) {
for (int j=0; j<cb; j++) {
printf("%i ", result[i][j]);
}
printf("\n");
}
int endTransTime = micros();
printf("Calc time: %i\n", endCalcTime - startCalcTime);
printf("With transmission: %i\n", endCalcTime - startCalcTime + endTransTime - startTransTime);
for(int i=0; i<ra; i++)
free(a[i]);
free(a);
for(int i=0; i<carb; i++) {
free(result[i]);
free(b[i]);
}
free(b);
free(result);
}
// LLSQ Approx. Trilateration
void locateLLSQ(int numantennas, double* antennas, double* distances, double* x)
{
// function saves triangulated position in vector x
// input *antennas holds
// x1, y1, z1,
// x2, y2, z2,
// x3, y3, z3,
// x4, y4, z4;
// numantennas holds integer number of antennas (min 5)
// double *distances holds pointer to array holding distances from each antenna
// Theory used:
// http://inside.mines.edu/~whereman/talks/TurgutOzal-11-Trilateration.pdf
// define some locals
int m = (numantennas - 1); // Number of rows in A
int n = 3; // Number of columns in A (always three)
if (m >= 4)n = 4; // Number of columns in A: three coordinates plus K
double A[m * n];
double b[m * 1];
double r;
double Atranspose[n * m];
double AtAinA[n * m];
double AtA[n * n];
//Calculating b vector
for (int i = 0; i < m; i++)
{
r = dist(antennas[0], antennas[1], antennas[2], antennas[(i + 1) * 3], antennas[(i + 1) * 3 + 1], antennas[(i + 1) * 3 + 2]);
b[i] = 0.5*(distances[0] * distances[0] - distances[i + 1] * distances[i + 1] + r * r); // If given exact distances
if (n == 4)b[i] = 0.5 * r*r; // For the case when we calculate K, overwrite b
}
#ifdef VERBOSE
matrixPrint(b, m, 1, "b");
#endif // DEBUG
//Calculating A matrix
for (int i = 0; i < m; i++)
{
A[i * n] = antennas[(i + 1) * 3] - antennas[0]; //xi-x1
A[i * n + 1] = antennas[(i + 1) * 3 + 1] - antennas[1]; //yi-y1
A[i * n + 2] = antennas[(i + 1) * 3 + 2] - antennas[2]; //zi-z1
if (n == 4) A[i * n + 3] = -0.5 * (distances[0] * distances[0] - distances[i + 1] * distances[i + 1]); // for K
}
#ifdef VERBOSE
matrixPrint(A, m, n, "A");
#endif // DEBUG
matrixTranspose(A, m, n, Atranspose);
matrixMult(Atranspose, A, n, m, n, AtA);
if (matrixInvert(AtA, n) == 1) //well behaved
{
matrixMult(AtA, Atranspose, n, n, m, AtAinA);
matrixMult(AtAinA, b, n, m, 1, x);
}
else // Ill-behaved A
{
double Q[m * m], Qtranspose[m * m], Qtransposeb[m * 1];
double R[m * m];
QR(A, m, n, Q, R);
matrixTranspose(Q, m, m, Qtranspose);
matrixMult(Qtranspose, b, m, m, 1, Qtransposeb);
matrixInvert(R, m);
matrixMult(R, Qtransposeb, m, m, 1, x);
}
// Adding back the reference point
x[0] = x[0] + antennas[0];
x[1] = x[1] + antennas[1];
x[2] = x[2] + antennas[2];
if (n == 4)x[3] = 1 / sqrt(x[3]); // Calculate K
}
void RunMatrixTest (struct MatrixMultInfo* info)
{
int i, n, numRepeats;
size_t numBytes;
double *aMat, *bMat, *cMat, *dMat;
/*************************************************************************************/
/**************** INITIALIZATION AND SETUP *************************/
/*************************************************************************************/
n = info->matrixSize; /* change this to be what user inputs from APP */
numRepeats = info->numberIterations; /* change this to be what user inputs from APP */
struct timeval totalStart;
gettimeofday(&totalStart, NULL);
/* Allocate memory to store the matrices on the device */
numBytes = n*n*sizeof(double);
mallocOnDevice(numBytes, (void**)&aMat);
mallocOnDevice(numBytes, (void**)&bMat);
mallocOnDevice(numBytes, (void**)&cMat);
mallocOnDevice(numBytes, (void**)&dMat);
/* Fill matrices with some random values */
for (i=0; i<n*n; i++) {
aMat[i] = rand() / (double)RAND_MAX;
}
for (i=0; i<n*n; i++) {
bMat[i] = rand() / (double)RAND_MAX;
}
/*************************************************************************************/
/********************** MATRIX MULTIPLY TESTING ******************************/
/*************************************************************************************/
struct timeval multStart;
gettimeofday(&multStart, NULL);
/* execute the matrix multiply */
for (i=0; i<numRepeats; i++) {
if (!info->useblas)
matrixMult(n, aMat, bMat, cMat);
else
matrixMultBLAS(n, aMat, bMat, cMat);
}
/* matrixMultBLAS (n, aMat, bMat, dMat);
for (i=0; i<n*n; i++) {
double diff = fabs(cMat[i] - dMat[i]);
if (diff > .0000005)
diff = diff;
}
*/
struct timeval multEnd;
gettimeofday(&multEnd, NULL);
/*************************************************************************************/
/****************** SHUTDOWN AND CLEANUP ***************************/
/*************************************************************************************/
/* deallocate memory */
freeOnDevice(aMat);
freeOnDevice(bMat);
freeOnDevice(cMat);
freeOnDevice(dMat);
struct timeval totalEnd;
gettimeofday(&totalEnd, NULL);
/* count the number of MFLOPS for this operation */
info->MFlops = 2.0*n*n*n*numRepeats / 1000000.0;
long elapsed_seconds = multEnd.tv_sec - multStart.tv_sec;
long elapsed_useconds = multEnd.tv_usec - multStart.tv_usec;
info->matrixMultiplyTime = ((double)elapsed_seconds) + (((double)elapsed_useconds)/1000000);
elapsed_seconds = totalEnd.tv_sec - totalStart.tv_sec;
elapsed_useconds = totalEnd.tv_usec - totalStart.tv_usec;
info->totalTime = ((double)elapsed_seconds) + (((double)elapsed_useconds)/1000000);
}