diff options
-rwxr-xr-x | dave/AffineSolve | bin | 65424 -> 40104 bytes | |||
-rw-r--r-- | dave/AffineSolve.c | 659 | ||||
-rw-r--r-- | dave/Makefile | 20 | ||||
-rw-r--r-- | dave/OrthoPlot.c | 451 | ||||
-rw-r--r-- | dave/dclapack_test.c | 2 | ||||
-rw-r--r-- | dave/fileutil.c | 133 | ||||
-rw-r--r-- | dave/fileutil.h | 35 | ||||
-rw-r--r-- | src/poser_daveortho.c | 85 | ||||
-rw-r--r-- | src/survive_cal.h | 2 | ||||
-rw-r--r-- | src/survive_data.c | 2 |
10 files changed, 1065 insertions, 324 deletions
diff --git a/dave/AffineSolve b/dave/AffineSolve Binary files differindex 98a9590..7d51b34 100755 --- a/dave/AffineSolve +++ b/dave/AffineSolve diff --git a/dave/AffineSolve.c b/dave/AffineSolve.c index 4fba56b..685062e 100644 --- a/dave/AffineSolve.c +++ b/dave/AffineSolve.c @@ -11,10 +11,13 @@ #include <math.h> #include "dclapack.h" #include <linmath.h> +#define RegisterDriver(a,b) +#include "poser_daveortho.c" #define LH_ID 0 #define NUM_HMD 32 +#define INDIR "full_test_triangle_on_floor/" -#define MAX_POINTS 128 +#define MAX_POINTS SENSORS_PER_OBJECT //#define _ABS(a) ( (a)<=0 ? -(a) : (a) ) #define _SIGN(a) ( (a)<=0 ? -1.0f : 1.0f ) #define RANDF ( (float)rand() / (float)RAND_MAX ) @@ -31,7 +34,7 @@ float hmd_pos[NUM_HMD][3]; void ReadHmdPoints() { int i; - FILE *fin = fopen("HMD_points.csv","r"); + FILE *fin = fopen(INDIR "HMD_points.csv","r"); if (fin==NULL) { printf("ERROR: could not open HMD_points.csv for reading\n"); exit(1); @@ -52,7 +55,7 @@ void ReadPtinfo() for (i=0; i<NUM_HMD; i++) { hmd_angle[i][0]=-9999.0; hmd_angle[i][1]=-9999.0; } // Read ptinfo.csv - FILE *fin = fopen("ptinfo.csv", "r"); + FILE *fin = fopen(INDIR "ptinfo.csv", "r"); if (fin==NULL) { printf("ERROR: could not open ptinfo.csv for reading\n"); exit(1); } while (!feof(fin)) { @@ -74,6 +77,350 @@ void ReadPtinfo() fclose(fin); } +void AffineSolve( + float T[4][4], // OUTPUT: transform + float O[MAX_POINTS][4], // INPUT: points, offsets + float N[MAX_POINTS][3], // INPUT: plane normals + float D[MAX_POINTS], // INPUT: plane offsets + int nPoints, int nIter, + float stepSizeRot, float stepSizePos, float falloff, int constrain) +{ + int i,j,k,iter; + //T[3][3] = 1.0f; + + printf("iter x y z error\n"); + + float gradDot = 1.0; + float prevGradDot = 1.0; + float de_dT[3][4]; // the gradient + float conj[3][4]; // the conjugate + float errorSq=0.0; + for (iter=0; iter<nIter; iter++) + { + //---------------------------------- + // Calculate the gradient direction + //---------------------------------- + errorSq = 0.0; + memset(de_dT, 0, 3*4*sizeof(float)); + for (i=0; i<nPoints; i++) + { + // What is the plane deviation error + float Ei = -D[i]; + for (j=0; j<3; j++) { + float Tj_oi = 0.0f; + for (k=0; k<4; k++) { + Tj_oi += T[j][k] * O[i][k]; + } + Ei += N[i][j] * Tj_oi; + } +// printf("E[%d] %f\n", i, Ei); + + // Figure out contribution to the error + for (j=0; j<3; j++) { + for (k=0; k<4; k++) { + de_dT[j][k] += N[i][j] * O[i][k] * Ei; + } + } + + errorSq += Ei*Ei; + } + +// printf("%d %f %f %f %f\n", iter, T[0][3], T[1][3], T[2][3], sqrt(errorSq)); +//exit(1); + // Constrain the gradient (such that dot products are zero) + if (constrain) + { + float T0T1 = 0.0, T1T2 = 0.0, T2T0 = 0.0; + for (k=0; k<3; k++) { + T0T1 += T[0][k] * T[1][k]; + T1T2 += T[1][k] * T[2][k]; + T2T0 += T[2][k] * T[0][k]; + } +// printf("T0T1 %f T1T2 %f T2T0 %f\n", T0T1, T1T2, T2T0); + for (k=0; k<3; k++) { + de_dT[0][k] += ORTHOG_PENALTY * 2.0 * T0T1 * T[1][k]; + de_dT[0][k] += ORTHOG_PENALTY * 2.0 * T2T0 * T[2][k]; + de_dT[1][k] += ORTHOG_PENALTY * 2.0 * T1T2 * T[2][k]; + de_dT[1][k] += ORTHOG_PENALTY * 2.0 * T0T1 * T[0][k]; + de_dT[2][k] += ORTHOG_PENALTY * 2.0 * T1T2 * T[1][k]; + de_dT[2][k] += ORTHOG_PENALTY * 2.0 * T2T0 * T[0][k]; + } + } + + // Calculate the gradient dot product + // (used by conjugate gradient method) + prevGradDot = gradDot; + gradDot = 0.0; + for (j=0; j<3; j++) { + for (k=0; k<4; k++) { + gradDot += de_dT[j][k] * de_dT[j][k]; + } + } + +// printf("Iter %d error %f gradDot %f prevGradDot %f\n", iter, sqrt(errorSq), gradDot, prevGradDot); + + //---------------------------------- + // Calculate the conjugate direction + //---------------------------------- +// if (iter==0) { + // First iteration, just use the gradient + for (j=0; j<3; j++) { + for (k=0; k<4; k++) { + conj[j][k] = -de_dT[j][k]; + } + } +/* } else { + // Calculate "beta" for Fletcher Reeves method + float beta = gradDot / prevGradDot; +//printf("gradDot %f prevGradDot %f beta %f\n", gradDot, prevGradDot, beta); + + // Update the conjugate + for (j=0; j<3; j++) { + for (k=0; k<4; k++) { + conj[j][k] = beta*conj[j][k] - de_dT[j][k]; + } + } + } +*/ + +// PRINT_MAT(de_dT,4,4); +// exit(1); + + //---------------------------------- + // How large is the gradient ? + //---------------------------------- + + double gradSizeRot = 0.0; + double gradSizePos = 0.0; + for (j=0; j<3; j++) { + for (k=0; k<3; k++) { + gradSizeRot += _ABS(conj[j][k]); + } + gradSizePos += _ABS(conj[j][k]); + } + if (gradSizeRot <= TOO_SMALL && gradSizePos <= TOO_SMALL) { break; } // Quit, we've totally converged + + //---------------------------------- + // Descend in the gradient direction + //---------------------------------- + if (gradSizeRot > TOO_SMALL) { + float scaleRot = stepSizeRot / gradSizeRot; + for (j=0; j<3; j++) { + for (k=0; k<3; k++) { + T[j][k] += scaleRot * conj[j][k]; + } + } + stepSizeRot *= falloff; + } + + if (gradSizePos > TOO_SMALL) { + float scalePos = stepSizePos / gradSizePos; + for (j=0; j<3; j++) { + T[j][3] += scalePos * conj[j][3]; + } + stepSizePos *= falloff; + } + + // Constrain the gradient (such that scaling is one) + if (constrain) + { + // Measure the scales + float len[3] = {0.0, 0.0, 0.0}; + for (j=0; j<3; j++) { + double lenSq = 0.0; + for (k=0; k<3; k++) { lenSq += (double)T[j][k] * (double)T[j][k]; } + len[j] = sqrt(lenSq); + } + + // How far off is the scale? + float xzLen = 0.5 * (len[0] + len[2]); + if (xzLen > TOO_SMALL) { + float inv_xzLen = 1.0 / xzLen; + for (j=0; j<3; j++) { + T[3][j] *= inv_xzLen; + } + } + + // Rescale the thing + for (j=0; j<3; j++) + { + if (len[j] > TOO_SMALL) { + float inv_len = 1.0 / len[j]; + for (k=0; k<3; k++) { T[j][k] *= inv_len; } + } + } + } + } + float dist = sqrt(T[0][3]*T[0][3] + T[1][3]*T[1][3] + T[2][3]*T[2][3]); + printf("AffineSolve: pos: %f %f %f dist: %f\n", T[0][3], T[1][3], T[2][3], dist); +} + +int main() +{ + int i,j,k,sen,axis; + + // Read the data files + ReadHmdPoints(); + ReadPtinfo(); + + //------------------------- + // Package the lighthouse data for "AffineSolve" + //------------------------- + + // Data for the "iterative" affine solve formula + float Tcalc[4][4]; + float O[MAX_POINTS][4]; + float N[MAX_POINTS][3]; + float D[MAX_POINTS]; + int nPlanes = 0; + + for (sen=0; sen<NUM_HMD; sen++) + { + for (axis=0; axis<2; axis++) + { + if (hmd_angle[sen][axis] != -9999.0) + { + // Set the offset + O[nPlanes][0] = hmd_pos[sen][0]; + O[nPlanes][1] = hmd_pos[sen][1]; + O[nPlanes][2] = hmd_pos[sen][2]; + O[nPlanes][3] = 1.0; + + // Calculate the plane equation + if (axis == 0) { // Horizontal + N[nPlanes][0] = -cos(hmd_angle[sen][axis]); + N[nPlanes][1] = -sin(hmd_angle[sen][axis]); + N[nPlanes][2] = 0.0; + D[nPlanes] = 0.0; + } else { // Vertical + N[nPlanes][0] = 0.0; + N[nPlanes][1] = -sin(hmd_angle[sen][axis]); + N[nPlanes][2] = cos(hmd_angle[sen][axis]); + D[nPlanes] = 0.0; + } + + printf("plane %d O %.3f %.3f %.3f %.3f N %.3f %.3f %.3f D %.3f\n", + nPlanes, + O[nPlanes][0], O[nPlanes][1], O[nPlanes][2], O[nPlanes][3], + N[nPlanes][0], N[nPlanes][1], N[nPlanes][2], + D[nPlanes]); + nPlanes++; + } + } + } + + + printf("nPlanes %d\n", nPlanes); + + //} + + PRINT_MAT(Tcalc,4,4); + + + //-------------------------------------------------- + // Package the data for "OrthoSolve" + //-------------------------------------------------- + + // Data for the "fake" ortho solve formula + float Tortho[4][4]; // OUTPUT: 4x4 transformation matrix + FLOAT S_out[2][MAX_POINTS]; // INPUT: array of screenspace points + FLOAT S_in[2][MAX_POINTS]; // INPUT: array of screenspace points + FLOAT X_in[3][MAX_POINTS]; // INPUT: array of offsets + int nPoints=0; + + // Transform into the "OrthoSolve" format + for (sen=0; sen<NUM_HMD; sen++) + { + if (hmd_angle[sen][0] != -9999.0 && hmd_angle[sen][1] != -9999.0) + { + S_in[0][nPoints] = hmd_angle[sen][0]; + S_in[1][nPoints] = hmd_angle[sen][1]; + X_in[0][nPoints] = hmd_pos[sen][0]; + X_in[1][nPoints] = hmd_pos[sen][1]; + X_in[2][nPoints] = hmd_pos[sen][2]; + nPoints++; + } + } + printf("OrthoSolve nPoints %d\n", nPoints); + + //-------------------------------------------------- + // Run the "OrthoSolve" and then the "AffineSolve" + //-------------------------------------------------- + + int loop; + for (loop=0; loop<1; loop++) + { + // Run OrthoSolve + OrthoSolve( + Tortho, // OUTPUT: 4x4 transformation matrix + S_out, // OUTPUT: array of output screenspace points + S_in, // INPUT: array of screenspace points + X_in, // INPUT: array of offsets + nPoints); + printf( "POS: %f %f %f\n", Tortho[0][3], Tortho[1][3], Tortho[2][3]); + + // Come up with rotation and transposed version of Tortho + FLT TorthoTr[4][4], Rortho[4][4], RorthoTr[4][4]; + TRANSP(Tortho,TorthoTr,4,4); + memcpy(Rortho,Tortho,4*4*sizeof(FLT)); + Rortho[3][0]=0.0; Rortho[3][1]=0.0; Rortho[3][2]=0.0; + TRANSP(Rortho,RorthoTr,4,4); + PRINT(Tortho,4,4); + PRINT(Rortho,4,4); + + // Print out some quaternions + FLT Tquat[4], TquatTr[4], Rquat[4], RquatTr[4]; + quatfrommatrix(Tquat, &Tortho[0][0] ); + quatfrommatrix(TquatTr, &TorthoTr[0][0] ); + quatfrommatrix(Rquat, &Rortho[0][0] ); + quatfrommatrix(RquatTr, &RorthoTr[0][0] ); + printf( "Tquat : %f %f %f %f = %f\n", Tquat [0], Tquat [1], Tquat [2], Tquat [3], quatmagnitude(Tquat)); + printf( "TquatTr: %f %f %f %f = %f\n", TquatTr[0], TquatTr[1], TquatTr[2], TquatTr[3], quatmagnitude(TquatTr)); + printf( "Rquat : %f %f %f %f = %f\n", Rquat [0], Rquat [1], Rquat [2], Rquat [3], quatmagnitude(Rquat)); + printf( "RquatTr: %f %f %f %f = %f\n", RquatTr[0], RquatTr[1], RquatTr[2], RquatTr[3], quatmagnitude(RquatTr)); + + // Flip y and z axies + FLT T2[4][4] = { + { Tortho[0][0], -Tortho[0][2], -Tortho[0][1], 0.0 }, + { Tortho[1][0], -Tortho[1][2], -Tortho[1][1], 0.0 }, + { Tortho[2][0], -Tortho[2][2], -Tortho[2][1], 0.0 }, + { 0.0, 0.0, 0.0, 1.0 } }; +PRINT(T2,4,4); + + // Print out the quaternions + FLT T2quat[4]; + quatfrommatrix(T2quat, &T2[0][0] ); + printf( "T2quat : %f %f %f %f = %f\n", T2quat [0], T2quat [1], T2quat [2], T2quat [3], quatmagnitude(T2quat)); + } + + // Run the calculation for Tcalc + //int run; + //for (run=0; run<100; run++) { +/* + // Initialize Tcalc to the identity matrix + memcpy(Tcalc, Tortho, 4*4*sizeof(float)); + //memset(Tcalc, 0, 4*4*sizeof(float)); + //for (i=0; i<4; i++) { Tcalc[i][i] = 1.0f; } + + // Solve it! + AffineSolve( + Tcalc, // OUTPUT: transform + O, // INPUT: points, offsets + N, // INPUT: plane normals + D, // INPUT: plane offsets + nPlanes, NITER, + STEP_SIZE_ROT, STEP_SIZE_POS, FALLOFF, + 1); +*/ + // insert code here... + return 0; +} + + + + + +#if 0 #define PRINT_MAT(A,M,N) { \ int m,n; \ printf(#A "\n"); \ @@ -433,308 +780,4 @@ PRINT(ab,2,1); // printf("xbar %f %f %f\n", xbar[0], xbar[1], xbar[2]); // printf("trans %f %f %f dist: %f\n", trans[0], trans[1], trans[2], transdist); } - -void AffineSolve( - float T[4][4], // OUTPUT: transform - float O[MAX_POINTS][4], // INPUT: points, offsets - float N[MAX_POINTS][3], // INPUT: plane normals - float D[MAX_POINTS], // INPUT: plane offsets - int nPoints, int nIter, - float stepSizeRot, float stepSizePos, float falloff, int constrain) -{ - int i,j,k,iter; - //T[3][3] = 1.0f; - - printf("iter x y z error\n"); - - float gradDot = 1.0; - float prevGradDot = 1.0; - float de_dT[3][4]; // the gradient - float conj[3][4]; // the conjugate - float errorSq=0.0; - for (iter=0; iter<nIter; iter++) - { - //---------------------------------- - // Calculate the gradient direction - //---------------------------------- - errorSq = 0.0; - memset(de_dT, 0, 3*4*sizeof(float)); - for (i=0; i<nPoints; i++) - { - // What is the plane deviation error - float Ei = -D[i]; - for (j=0; j<3; j++) { - float Tj_oi = 0.0f; - for (k=0; k<4; k++) { - Tj_oi += T[j][k] * O[i][k]; - } - Ei += N[i][j] * Tj_oi; - } -// printf("E[%d] %f\n", i, Ei); - - // Figure out contribution to the error - for (j=0; j<3; j++) { - for (k=0; k<4; k++) { - de_dT[j][k] += N[i][j] * O[i][k] * Ei; - } - } - - errorSq += Ei*Ei; - } - -// printf("%d %f %f %f %f\n", iter, T[0][3], T[1][3], T[2][3], sqrt(errorSq)); -//exit(1); - // Constrain the gradient (such that dot products are zero) - if (constrain) - { - float T0T1 = 0.0, T1T2 = 0.0, T2T0 = 0.0; - for (k=0; k<3; k++) { - T0T1 += T[0][k] * T[1][k]; - T1T2 += T[1][k] * T[2][k]; - T2T0 += T[2][k] * T[0][k]; - } -// printf("T0T1 %f T1T2 %f T2T0 %f\n", T0T1, T1T2, T2T0); - for (k=0; k<3; k++) { - de_dT[0][k] += ORTHOG_PENALTY * 2.0 * T0T1 * T[1][k]; - de_dT[0][k] += ORTHOG_PENALTY * 2.0 * T2T0 * T[2][k]; - de_dT[1][k] += ORTHOG_PENALTY * 2.0 * T1T2 * T[2][k]; - de_dT[1][k] += ORTHOG_PENALTY * 2.0 * T0T1 * T[0][k]; - de_dT[2][k] += ORTHOG_PENALTY * 2.0 * T1T2 * T[1][k]; - de_dT[2][k] += ORTHOG_PENALTY * 2.0 * T2T0 * T[0][k]; - } - } - - // Calculate the gradient dot product - // (used by conjugate gradient method) - prevGradDot = gradDot; - gradDot = 0.0; - for (j=0; j<3; j++) { - for (k=0; k<4; k++) { - gradDot += de_dT[j][k] * de_dT[j][k]; - } - } - -// printf("Iter %d error %f gradDot %f prevGradDot %f\n", iter, sqrt(errorSq), gradDot, prevGradDot); - - //---------------------------------- - // Calculate the conjugate direction - //---------------------------------- -// if (iter==0) { - // First iteration, just use the gradient - for (j=0; j<3; j++) { - for (k=0; k<4; k++) { - conj[j][k] = -de_dT[j][k]; - } - } -/* } else { - // Calculate "beta" for Fletcher Reeves method - float beta = gradDot / prevGradDot; -//printf("gradDot %f prevGradDot %f beta %f\n", gradDot, prevGradDot, beta); - - // Update the conjugate - for (j=0; j<3; j++) { - for (k=0; k<4; k++) { - conj[j][k] = beta*conj[j][k] - de_dT[j][k]; - } - } - } -*/ - -// PRINT_MAT(de_dT,4,4); -// exit(1); - - //---------------------------------- - // How large is the gradient ? - //---------------------------------- - - double gradSizeRot = 0.0; - double gradSizePos = 0.0; - for (j=0; j<3; j++) { - for (k=0; k<3; k++) { - gradSizeRot += _ABS(conj[j][k]); - } - gradSizePos += _ABS(conj[j][k]); - } - if (gradSizeRot <= TOO_SMALL && gradSizePos <= TOO_SMALL) { break; } // Quit, we've totally converged - - //---------------------------------- - // Descend in the gradient direction - //---------------------------------- - if (gradSizeRot > TOO_SMALL) { - float scaleRot = stepSizeRot / gradSizeRot; - for (j=0; j<3; j++) { - for (k=0; k<3; k++) { - T[j][k] += scaleRot * conj[j][k]; - } - } - stepSizeRot *= falloff; - } - - if (gradSizePos > TOO_SMALL) { - float scalePos = stepSizePos / gradSizePos; - for (j=0; j<3; j++) { - T[j][3] += scalePos * conj[j][3]; - } - stepSizePos *= falloff; - } - - // Constrain the gradient (such that scaling is one) - if (constrain) - { - // Measure the scales - float len[3] = {0.0, 0.0, 0.0}; - for (j=0; j<3; j++) { - double lenSq = 0.0; - for (k=0; k<3; k++) { lenSq += (double)T[j][k] * (double)T[j][k]; } - len[j] = sqrt(lenSq); - } - - // How far off is the scale? - float xzLen = 0.5 * (len[0] + len[2]); - if (xzLen > TOO_SMALL) { - float inv_xzLen = 1.0 / xzLen; - for (j=0; j<3; j++) { - T[3][j] *= inv_xzLen; - } - } - - // Rescale the thing - for (j=0; j<3; j++) - { - if (len[j] > TOO_SMALL) { - float inv_len = 1.0 / len[j]; - for (k=0; k<3; k++) { T[j][k] *= inv_len; } - } - } - } - } - float dist = sqrt(T[0][3]*T[0][3] + T[1][3]*T[1][3] + T[2][3]*T[2][3]); - printf("AffineSolve: pos: %f %f %f dist: %f\n", T[0][3], T[1][3], T[2][3], dist); -} - -int main() -{ - int i,j,k,sen,axis; - - // Read the data files - ReadHmdPoints(); - ReadPtinfo(); - - //------------------------- - // Package the lighthouse data for "AffineSolve" - //------------------------- - - // Data for the "iterative" affine solve formula - float Tcalc[4][4]; - float O[MAX_POINTS][4]; - float N[MAX_POINTS][3]; - float D[MAX_POINTS]; - int nPlanes = 0; - - for (sen=0; sen<NUM_HMD; sen++) - { - for (axis=0; axis<2; axis++) - { - if (hmd_angle[sen][axis] != -9999.0) - { - // Set the offset - O[nPlanes][0] = hmd_pos[sen][0]; - O[nPlanes][1] = hmd_pos[sen][1]; - O[nPlanes][2] = hmd_pos[sen][2]; - O[nPlanes][3] = 1.0; - - // Calculate the plane equation - if (axis == 0) { // Horizontal - N[nPlanes][0] = -cos(hmd_angle[sen][axis]); - N[nPlanes][1] = -sin(hmd_angle[sen][axis]); - N[nPlanes][2] = 0.0; - D[nPlanes] = 0.0; - } else { // Vertical - N[nPlanes][0] = 0.0; - N[nPlanes][1] = -sin(hmd_angle[sen][axis]); - N[nPlanes][2] = cos(hmd_angle[sen][axis]); - D[nPlanes] = 0.0; - } - - printf("plane %d O %.3f %.3f %.3f %.3f N %.3f %.3f %.3f D %.3f\n", - nPlanes, - O[nPlanes][0], O[nPlanes][1], O[nPlanes][2], O[nPlanes][3], - N[nPlanes][0], N[nPlanes][1], N[nPlanes][2], - D[nPlanes]); - nPlanes++; - } - } - } - - - printf("nPlanes %d\n", nPlanes); - - //} - - PRINT_MAT(Tcalc,4,4); - - - //-------------------------------------------------- - // Package the data for "OrthoSolve" - //-------------------------------------------------- - - // Data for the "fake" ortho solve formula - float Tortho[4][4]; // OUTPUT: 4x4 transformation matrix - FLOAT S_out[2][MAX_POINTS]; // INPUT: array of screenspace points - FLOAT S_in[2][MAX_POINTS]; // INPUT: array of screenspace points - FLOAT X_in[3][MAX_POINTS]; // INPUT: array of offsets - int nPoints=0; - - // Transform into the "OrthoSolve" format - for (sen=0; sen<NUM_HMD; sen++) - { - if (hmd_angle[sen][0] != -9999.0 && hmd_angle[sen][1] != -9999.0) - { - S_in[0][nPoints] = hmd_angle[sen][0]; - S_in[1][nPoints] = hmd_angle[sen][1]; - X_in[0][nPoints] = hmd_pos[sen][0]; - X_in[1][nPoints] = hmd_pos[sen][1]; - X_in[2][nPoints] = hmd_pos[sen][2]; - nPoints++; - } - } - printf("OrthoSolve nPoints %d\n", nPoints); - - //-------------------------------------------------- - // Run the "OrthoSolve" and then the "AffineSolve" - //-------------------------------------------------- - - int loop; - for (loop=0; loop<1; loop++) - { - // Run OrthoSolve - OrthoSolve( - Tortho, // OUTPUT: 4x4 transformation matrix - S_out, // OUTPUT: array of output screenspace points - S_in, // INPUT: array of screenspace points - X_in, // INPUT: array of offsets - nPoints); - } - - // Run the calculation for Tcalc - //int run; - //for (run=0; run<100; run++) { -/* - // Initialize Tcalc to the identity matrix - memcpy(Tcalc, Tortho, 4*4*sizeof(float)); - //memset(Tcalc, 0, 4*4*sizeof(float)); - //for (i=0; i<4; i++) { Tcalc[i][i] = 1.0f; } - - // Solve it! - AffineSolve( - Tcalc, // OUTPUT: transform - O, // INPUT: points, offsets - N, // INPUT: plane normals - D, // INPUT: plane offsets - nPlanes, NITER, - STEP_SIZE_ROT, STEP_SIZE_POS, FALLOFF, - 1); -*/ - // insert code here... - return 0; -} +#endif diff --git a/dave/Makefile b/dave/Makefile index bf62837..330bda3 100644 --- a/dave/Makefile +++ b/dave/Makefile @@ -1,7 +1,21 @@ +UNAME:=$(shell uname) + +CFLAGS:= -lm -I../redist -I../src -I../include/libsurvive + +ifeq ($(UNAME), Linux) +CFLAGS:= $(CFLAGS) -lGL -lGLU -lglut -lX11 -I../../redist -DLINUX -lm -lpthread -DLINUX +endif + +# Darwin is Mac OSX !! +ifeq ($(UNAME), Darwin) +CFLAGS:= $(CFLAGS) -I../../redist -w -framework OpenGL -framework GLUT +endif + + all: # gcc -O3 -o kalman_filter kalman_filter.c main.c - gcc -O3 -o dclapack_test dclapack_test.c -lm - gcc -O0 -g -o AffineSolve AffineSolve.c -lm -I../redist ../redist/linmath.c #-Wall - + gcc -O3 -o dclapack_test dclapack_test.c $(CFLAGS) + gcc -O3 -o AffineSolve AffineSolve.c $(CFLAGS) ../redist/linmath.c #-Wall + gcc -O3 -o OrthoPlot OrthoPlot.c fileutil.c ../redist/linmath.c ../redist/os_generic.c $(CFLAGS) clean: rm -f kalman_filter dclapack_test diff --git a/dave/OrthoPlot.c b/dave/OrthoPlot.c new file mode 100644 index 0000000..222828c --- /dev/null +++ b/dave/OrthoPlot.c @@ -0,0 +1,451 @@ +/* + When creating your project, uncheck OWL, + uncheck Class Library, select Static + instead of Dynamic and change the target + model to Console from GUI. + Also link glut.lib to your project once its done. + */ + +#include <stdio.h> // Standard Header For Most Programs +#include <stdlib.h> +#include <string.h> +#include <math.h> +#include "os_generic.h" +#include "linmath.h" +#include "fileutil.h" + +#ifdef __APPLE__ +#include <OpenGL/gl.h> // The GL Header File +#include <GLUT/glut.h> // The GL Utility Toolkit (Glut) Header +#else +#include <GL/gl.h> +#include <GL/glu.h> +#endif +#ifdef __linux__ +#include <GL/freeglut.h> +#endif + +#define RegisterDriver(a,b) +#include "poser_daveortho.c" + + +// Required to set up a window +#define WIDTH 1280 +#define HEIGHT 1280 +#define FULLSCREEN 0 +int keys[256]; // Regular keyboard keys +int sp_keys[256]; // Special keyboard keycodes (GLUT specific) + +#define LH_ID 0 +#define NUM_HMD 32 +#define INDIR "dave/full_test_triangle_on_floor/" +#define MAX_POINTS SENSORS_PER_OBJECT + +#define PI 3.1415926535897932386264 +#define MOVESPEED 1.0 +#define ROTSPEED 5.0 + +// View space +float posx=0.0f; +float posy=0.0f; +float posz=0.0f; +float rotx=0.0f; +float roty=0.0f; +float rotz=0.0f; + +// Data for the "fake" ortho solve formula +float Tortho[4][4]; // OUTPUT: 4x4 transformation matrix +FLOAT S_out[2][MAX_POINTS]; // INPUT: array of screenspace points +FLOAT S_in[2][MAX_POINTS]; // INPUT: array of screenspace points +FLOAT X_in[3][MAX_POINTS]; // INPUT: array of offsets +int nPoints=0; + +//-------------------------------------------------------------------- +// +//-------------------------------------------------------------------- + +void DrawGrid( + float minX, float maxX, + float minY, float maxY, + float minZ, float maxZ, + float stepX, float stepY, float stepZ); + +void DrawCoordinateSystem( + float x, float y, float z, + float qx, float qy, float qz, float qr); + + +float hmd_pos[NUM_HMD][3]; +void ReadHmdPoints() +{ + int i; + FILE *fin = fopen(INDIR "HMD_points.csv","r"); + if (fin==NULL) { + printf("ERROR: could not open " INDIR "HMD_points.csv for reading\n"); + exit(1); + } + + for (i=0; i<NUM_HMD; i++) { + fscanf(fin, "%f %f %f", &(hmd_pos[i][0]), &(hmd_pos[i][1]), &(hmd_pos[i][2])); + } + + fclose(fin); +} + +float hmd_angle[NUM_HMD][2]; +void ReadPtinfo() +{ + // Initialize to -9999 + int i; + for (i=0; i<NUM_HMD; i++) { hmd_angle[i][0]=-9999.0; hmd_angle[i][1]=-9999.0; } + + // Read ptinfo.csv + FILE *fin = fopen(INDIR "ptinfo.csv", "r"); + if (fin==NULL) { printf("ERROR: could not open ptinfo.csv for reading\n"); exit(1); } + while (!feof(fin)) + { + // Read the angle + int sen,lh,axis,count; + float angle, avglen, stddevang, stddevlen; + float max_outlier_length, max_outlier_angle; + int rt = fscanf( fin, "%d %d %d %d %f %f %f %f %f %f\n", + &sen, &lh, &axis, &count, + &angle, &avglen, &stddevang, &stddevlen, + &max_outlier_length, &max_outlier_angle); + if (rt != 10) { break; } + + // If it's valid, store in the result + if (lh == LH_ID && sen < NUM_HMD) { + hmd_angle[sen][axis] = angle; + } + } + fclose(fin); +} + + +//-------------------------------------------------------------------- +// +//-------------------------------------------------------------------- + +/* + * init() is called at program startup + */ +void init() +{ + int i,j,k,sen,axis; + + // Read the data files + ReadHmdPoints(); + ReadPtinfo(); + + //-------------------------------------------------- + // Package the data for "OrthoSolve" + //-------------------------------------------------- + + // Transform into the "OrthoSolve" format + for (sen=0; sen<NUM_HMD; sen++) + { + if (hmd_angle[sen][0] != -9999.0 && hmd_angle[sen][1] != -9999.0) + { + S_in[0][nPoints] = hmd_angle[sen][0]; + S_in[1][nPoints] = hmd_angle[sen][1]; + X_in[0][nPoints] = hmd_pos[sen][0]; + X_in[1][nPoints] = hmd_pos[sen][1]; + X_in[2][nPoints] = hmd_pos[sen][2]; + nPoints++; + } + } + printf("OrthoSolve nPoints %d\n", nPoints); + //-------------------------------------------------- + // Run the "OrthoSolve" and then the "AffineSolve" + //-------------------------------------------------- + + // Run OrthoSolve + OrthoSolve( + Tortho, // OUTPUT: 4x4 transformation matrix + S_out, // OUTPUT: array of output screenspace points + S_in, // INPUT: array of screenspace points + X_in, // INPUT: array of offsets + nPoints); + //printf( "POS: %f %f %f\n", Tortho[0][3], Tortho[1][3], Tortho[2][3]); + + //-------------------------------------------------- + // Spawn a thread to read the HMD angles + //-------------------------------------------------- + OGCreateThread(ThreadReadHmtAngles,0); + + //-------------------------------------------------- + // Initialize OpenGL + //-------------------------------------------------- + glShadeModel(GL_SMOOTH); // Enable Smooth Shading + glClearColor(0.0f, 0.0f, 0.0f, 1.0f); // Black Background + glClearDepth(1.0f); // Depth Buffer Setup + glEnable(GL_DEPTH_TEST); // Enables Depth Testing + glDepthFunc(GL_LEQUAL); // The Type Of Depth Testing To Do + glEnable ( GL_COLOR_MATERIAL ); + glDisable(GL_CULL_FACE); + glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST); +} + +/* + * draw() is called once every frame + */ +void draw() +{ + int i,j; + + //------------------------ + // Check for keyboard input + //------------------------ + if (keys['w'] || keys['W']) { + posz += MOVESPEED; + } + if (keys['s'] || keys['S']) { + posz -= MOVESPEED; + } + if (keys['a'] || keys['A']) { + roty += ROTSPEED; + } + if (keys['d'] || keys['D']) { + roty -= ROTSPEED; + } + if (keys[27]) { + exit(0); + } + + //------------------------ + // Update the scene + //------------------------ + + //------------------------ + // Draw using OpenGL + //------------------------ + + // Clear the screen + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // Clear Screen And Depth Buffer + + // Translate and rotate the camera + glLoadIdentity(); // Reset The Current Modelview Matrix +// glTranslatef(-posx,-posy,posz); // Move Left 1.5 Units And Into The + + // Bouning box around the points (in radians) + float x0=-45.0 * (PI/180.0); + float x1= 45.0 * (PI/180.0); + float y0=-45.0 * (PI/180.0); + float y1= 45.0 * (PI/180.0); + + + //------------------------ + // Read the angles from stdin + //------------------------ + FLT hmdAngles[4][NUM_HMD]; + + // Read the hmd angles + OGLockMutex(read_mutex); + for (i=0; i<4; i++) { + for (j=0; j<NUM_HMD; j++) { + hmdAngles[i][j] = read_hmdAngles[i][j]; + } + } + OGUnlockMutex(read_mutex); + + // Draw the hmd bearing angles + nPoints=0; + for (i=0; i<NUM_HMD; i++) { + const double dist=10.0; + + int sweepx = (LH_ID==0) ? SWEEP_LX : SWEEP_RX; + int sweepy = (LH_ID==0) ? SWEEP_LY : SWEEP_RY; + + // If the left lighthouse sees it + if (read_hmdAngleViewed[sweepx][i] >= read_frameno-6 && + read_hmdAngleViewed[sweepy][i] >= read_frameno-6 && + hmdAngles[sweepx][i]!=-9999.0 && hmdAngles[sweepy][i]!=-9999.0) + { + S_in[0][nPoints] = hmdAngles[sweepy][i]; + S_in[1][nPoints] = hmdAngles[sweepx][i]; + X_in[0][nPoints] = hmd_pos[i][0]; + X_in[1][nPoints] = hmd_pos[i][1]; + X_in[2][nPoints] = hmd_pos[i][2]; +printf("i %d S %f %f X %f %f %f frno %d %d currfr %d\n", + i, S_in[0][nPoints], S_in[1][nPoints], + X_in[0][nPoints], X_in[1][nPoints], X_in[2][nPoints], + read_hmdAngleViewed[sweepx][i], read_hmdAngleViewed[sweepy][i], read_frameno); + nPoints++; + } + } + + read_frameno++; + + //-------------------------------------------------- + // Run the "OrthoSolve" and then the "AffineSolve" + //-------------------------------------------------- + + // Run OrthoSolve + OrthoSolve( + Tortho, // OUTPUT: 4x4 transformation matrix + S_out, // OUTPUT: array of output screenspace points + S_in, // INPUT: array of screenspace points + X_in, // INPUT: array of offsets + nPoints); + printf( "POS: %f %f %f\n", Tortho[0][3], Tortho[1][3], Tortho[2][3]); + + + //------------------------ + // Draw the inputs + //------------------------ + glPointSize(3.0); + + // Draw the input points + glColor3f(1.0,0.5,0.5); + glBegin(GL_POINTS); + for (i=0; i<nPoints; i++) { + glVertex2f( (S_in[0][i]-x0)/(x1-x0), (S_in[1][i]-y0)/(y1-y0) ); + } + glEnd(); + + // Draw the output points + glColor3f(0.5,0.5,1.0); + glBegin(GL_POINTS); + for (i=0; i<nPoints; i++) { + glVertex2f( (S_out[0][i]-x0)/(x1-x0), (S_out[1][i]-y0)/(y1-y0) ); + } + glEnd(); + + + // + // We're Done + // + glutSwapBuffers ( ); // Swap The Buffers To Not Be Left With A Clear Screen +} + +/* + * resize() is called when we change the screen size + */ +void resize(int width, int height) // Resize And Initialize The GL Window +{ + glViewport(0,0,width,height); // Reset The Current Viewport + + glMatrixMode(GL_PROJECTION); // Select The Projection Matrix + glLoadIdentity(); // Reset The Projection Matrix + + // Uncomment For a 3D perspective + //gluPerspective(45.0f,(float)width/(float)height,0.1f,1000.0f); + // Uncomment for a 2D perspective + glOrtho(0.0, 1.0, 0.0, 1.0, -10.0, 10.0); + + glMatrixMode(GL_MODELVIEW); // Select The Modelview Matrix + glLoadIdentity(); // Reset The Modelview Matrix +} + +/* + * These functions are called whenever a key is pressed + */ +void keyboardDown ( unsigned char key, int x, int y ) // Create Keyboard Function +{ + keys[key] = 1; +} +void keyboardUp ( unsigned char key, int x, int y ) +{ + keys[key] = 0; +} + +void specialKeyDown ( int key, int x, int y ) // Create Special Function (required for arrow keys) +{ + if (key<256) { + sp_keys[key] = 1; + } +} +void specialKeyUp (int key, int x, int y) +{ + if (key<256) { + sp_keys[key] = 0; + } +} + +int main ( int argc, char** argv ) // Create Main Function For Bringing It All Together +{ + glutInit ( &argc, argv ); // Erm Just Write It =) + glutInitDisplayMode ( GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE ); // Display Mode + glutInitWindowSize ( WIDTH, HEIGHT ); // If glutFullScreen wasn't called this is the window size + glutCreateWindow ( "OpenGL" ); // Window Title (argv[0] for current directory as title) + if (FULLSCREEN) { + glutFullScreen ( ); // Put Into Full Screen + } + glutDisplayFunc ( draw ); // Matching Earlier Functions To Their Counterparts + glutIdleFunc ( draw ); + glutReshapeFunc ( resize ); + glutKeyboardFunc ( keyboardDown ); + glutKeyboardUpFunc ( keyboardUp ); + glutSpecialFunc ( specialKeyDown ); + glutSpecialUpFunc ( specialKeyUp ); + init (); + glutMainLoop ( ); // Initialize The Main Loop + return 0; +} + + + +void DrawGrid( + float minX, float maxX, + float minY, float maxY, + float minZ, float maxZ, + float stepX, float stepY, float stepZ) +{ + float x,y,z; + + glBegin(GL_LINES); + + // X grid stripes + for (y=minY; y<maxY; y+=stepY) { + for (z=minZ; z<maxZ; z+=stepZ) { + glVertex3f(minX, y, z); + glVertex3f(maxX, y, z); + } + } + + // Y grid stripes + for (x=minX; x<maxX; x+=stepX) { + for (z=minZ; z<maxZ; z+=stepZ) { + glVertex3f(x, minY, z); + glVertex3f(x, maxY, z); + } + } + + // Z grid stripes + for (y=minY; y<maxY; y+=stepY) { + for (x=minX; x<maxX; x+=stepX) { + glVertex3f(x, y, minZ); + glVertex3f(x, y, maxZ); + } + } + + glEnd(); +} + + +void DrawCoordinateSystem( + float x, float y, float z, + float qx, float qy, float qz, float qr) +{ + FLT i0[3],j0[3],k0[3]; + FLT i[3],j[3],k[3]; + FLT q[4]; + + i0[0]=1.0; i0[1]=0.0; i0[2]=0.0; + j0[0]=0.0; j0[1]=1.0; j0[2]=0.0; + k0[0]=0.0; k0[1]=0.0; k0[2]=1.0; + q [0]=qr; q [1]=qx; q [2]=qy; q [3]=qz; + + quatrotatevector(i, q, i0); + quatrotatevector(j, q, j0); + quatrotatevector(k, q, k0); + + glBegin(GL_LINES); + glColor3f(1, 0, 0); glVertex3f(x,z,y); glVertex3f(x+i[0],z+i[2],y+i[1]); + glColor3f(0, 1, 0); glVertex3f(x,z,y); glVertex3f(x+j[0],z+j[2],y+j[1]); + glColor3f(0, 0, 1); glVertex3f(x,z,y); glVertex3f(x+k[0],z+k[2],y+k[1]); + glEnd(); +} + + diff --git a/dave/dclapack_test.c b/dave/dclapack_test.c index b0667ef..3f48b08 100644 --- a/dave/dclapack_test.c +++ b/dave/dclapack_test.c @@ -1,6 +1,6 @@ #define FLOAT float #define ORDER 50 -#include "dclapack.h" +#include "../redist/dclapack.h" #include <stdio.h> #include <stdlib.h> diff --git a/dave/fileutil.c b/dave/fileutil.c new file mode 100644 index 0000000..04dc241 --- /dev/null +++ b/dave/fileutil.c @@ -0,0 +1,133 @@ +#include "fileutil.h" +#include <stdio.h> +#include <stdlib.h> + +#define PI 3.14159265358979323846264 + + +og_mutex_t read_mutex; +og_thread_t read_thread; +double read_hmdAngles[NUM_SWEEP][NUM_HMD]; +int read_hmdAngleViewed[NUM_SWEEP][NUM_HMD]; +int read_frameno=0; + +static FILE *fopen_orDie(const char *path, const char *flag) +{ + FILE *f = fopen(path, flag); + if (f == NULL) { + printf("ERROR could not oepn %s for %s\n", path, flag); + exit(1); + } + return f; +} + +static void SeekToken(FILE *f, const char *keyword) +{ + char token[4096]; + do { + fscanf(f, "%s", token); + } while( strcmp(token,keyword)!=0 && !feof(f) ); +} + +void LoadLighthousePos( + const char *path, + float *x, float *y, float *z, + float *qi, float *qj, float *qk, float *qreal) +{ + FILE *f = fopen_orDie(path,"r"); + SeekToken(f, "POS:"); + fscanf(f, "%f %f %f\n", x, y, z); + SeekToken(f, "QUAT:"); + fscanf(f, "%f %f %f %f\n", qreal, qi, qj, qk); + fclose (f); +} + +void LoadHmdProcessedDataAngles( + const char *path, + double angles[NUM_SWEEP][NUM_HMD]) +{ + int i,j; + char type[256]; + char sweep[256]; + int id; + int nSweep; + double ang; + double d1,d2,d3; // revisit these numbers later + + // Initialize all of the angles to -9999 + for (i=0; i<NUM_SWEEP; i++) { + for (j=0; j<NUM_HMD; j++) { + angles[i][j] = -9999.0; // Initially no value + } + } + + FILE *f = fopen_orDie(path, "r"); + + while (!feof(f)) + { + // Read the line from the file + int rt=fscanf(f, "%s %s %d %d %lf %lf %lf %lf", + &type, &sweep, &id, &nSweep, &ang, + &d1,&d2,&d3); + + if (rt<8) { break; } + + // Only hmd points + if ( strcmp(type,"HMD")!=0 ) { continue; } + + // Which sweep is it? + int sweepId=-1; + if ( strcmp(sweep,"LX")==0 ) { sweepId=SWEEP_LX; } + else if ( strcmp(sweep,"LY")==0 ) { sweepId=SWEEP_LY; } + else if ( strcmp(sweep,"RX")==0 ) { sweepId=SWEEP_RX; } + else if ( strcmp(sweep,"RY")==0 ) { sweepId=SWEEP_RY; } + else { continue; } + + // Convert the angle from ticks to radians + angles[sweepId][id] = (PI / 400000.0) * ( ang-200000.0 ); + } + + fclose(f); +} + +void *ThreadReadHmtAngles(void *junk) +{ + char house[256]; + char xy[256]; + char hmd[256]; + double ts; + int id; + int syncid; + int timeInSweep; + int length; + //lighthouse sweep hmdOrwmd timestamp id syncid timeinsweep length + + while(1) { + int rt=scanf("%s %s %s %lf %d %d %d %d", house, xy, hmd, &ts, &id, &syncid, &timeInSweep, &length); + if (rt==8) + { + //int rt=scanf("%s %s %s %lf %d %d %d %d", house, xy, hmd, &ts, &id, &syncid, &timeInSweep, &length); + //printf( "%s %s %s %f %d %d %d %d\n", house, xy, hmd, ts,id, syncid, timeInSweep, length ); + + if( id < 0 ) continue; + int sweepId=0; + if ( house[0]=='R' ) { sweepId+=2; } + if ( xy[0] =='Y' ) { sweepId++; } + double angle = (PI / 400000.0) * ( (double)timeInSweep-200000.0 ); + + if ( strcmp(hmd,"HMD")==0 ) { id += 0; } + else if ( strcmp(hmd,"WM0")==0 ) { id += 32; } + else if ( strcmp(hmd,"WM1")==0 ) { id += 56; } + else { continue; } + + if ( id<0 || id >NUM_HMD) { continue; } + + OGLockMutex (read_mutex); + read_hmdAngles[sweepId][id] = angle; + OGUnlockMutex(read_mutex); + read_hmdAngleViewed[sweepId][id] = read_frameno; + } + } +} + + diff --git a/dave/fileutil.h b/dave/fileutil.h new file mode 100644 index 0000000..e5da244 --- /dev/null +++ b/dave/fileutil.h @@ -0,0 +1,35 @@ +#ifndef _fileutil_h_ +#define _fileutil_h_ + +#include <pthread.h> +#include "os_generic.h" + +void LoadLighthousePos( + const char *path, + float *x, float *y, float *z, + float *qi, float *qj, float *qk, float *qreal); + + +// first 32 are hmd, next 24 wm0 next 24 wm1 +#define NUM_HMD 80 +#define NUM_SWEEP 4 +#define SWEEP_LX 0 +#define SWEEP_LY 1 +#define SWEEP_RX 2 +#define SWEEP_RY 3 +void LoadHmdProcessedDataAngles( + const char *path, + double angle[NUM_SWEEP][NUM_HMD]); + + +extern og_mutex_t read_mutex; +extern og_thread_t read_thread; +extern double read_hmdAngles[NUM_SWEEP][NUM_HMD]; +extern int read_hmdAngleViewed[NUM_SWEEP][NUM_HMD]; +extern int read_frameno; +void *ThreadReadHmtAngles(void *junk); + + +#endif // __fileutil_h_ + + diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index e81e154..80f65a9 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -265,7 +265,8 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); // FLT ydist1 = 1.0 / uhat_len; //0.25*PI / uhat_len; // FLT ydist2 = 1.0 / rhat_len; //0.25*PI / rhat_len; FLT ydist = 1.0 / urhat_len; - //printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist); + printf("ydist %f\n", ydist); +// printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist); //-------------------- // Rescale the axies to be of the proper length @@ -282,7 +283,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); if( x_y != x_y ) x_y = 0; if( y_y != y_y ) y_y = 0; if( z_y != z_y ) z_y = 0; -/* + // Exhaustively flip the minus sign of the z axis until we find the right one . . . FLT bestErr = 9999.0; FLT xy_dot2 = x[0][0]*y[0][0] + x[2][0]*y[2][0]; @@ -302,7 +303,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); FLT cx,cy,cz; CrossProduct(cx,cy,cz,x[0][0],x_y,x[2][0],y[0][0],y_y,y[2][0]); FLT hand = cx*z[0][0] + cy*z_y + cz*z[2][0]; - printf("err %f hand %f\n", err, hand); +// printf("err %f hand %f\n", err, hand); // If we are the best right-handed frame so far //if (hand > 0 && err < bestErr) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; } @@ -313,9 +314,9 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); } x_y = -x_y; } - printf("bestErr %f\n", bestErr); -*/ +// printf("bestErr %f\n", bestErr); +/* //------------------------- // A test version of the rescaling to the proper length //------------------------- @@ -338,7 +339,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); if( y_y != y_y ) y_y = 0; if( z_y != z_y ) z_y = 0; - printf( "---> %f %f %f\n", x_y, y_y, z_y ); +// printf( "---> %f %f %f\n", x_y, y_y, z_y ); // Exhaustively flip the minus sign of the z axis until we find the right one . . . FLT bestErr = 9999.0; @@ -359,7 +360,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); FLT cx,cy,cz; CrossProduct(cx,cy,cz,x2[0][0],x_y,x2[2][0],y2[0][0],y_y,y2[2][0]); FLT hand = cx*z2[0][0] + cy*z_y + cz*z2[2][0]; - printf("err %f hand %f\n", err, hand); + //printf("err %f hand %f\n", err, hand); // If we are the best right-handed frame so far if (hand > 0 && err < bestErr) { x2[1][0]=x_y; y2[1][0]=y_y; z2[1][0]=z_y; bestErr=err; } @@ -380,7 +381,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); } } ydist = bestYdist; - +*/ /* for (i=0; i<nPoints; i++) { FLT x1 = x[0][0]*X[0][i] + y[0][0]*X[1][i] + z[0][0]*X[2][i]; @@ -441,7 +442,71 @@ PRINT(ab,2,1); T[2][0]=R[2][0]; T[2][1]=R[2][1]; T[2][2]=R[2][2]; T[2][3]=trans[2]; T[3][0]=0.0; T[3][1]=0.0; T[3][2]=0.0; T[3][3]=1.0; - PRINT_MAT(T,4,4); + + FLT T2[4][4]; + + //------------------- + // Orthogonalize the matrix + //------------------- + FLT temp[4][4]; + FLT quat[4], quatNorm[4]; + FLT euler[3]; + + + //------------------- + // Orthogonalize the matrix + //------------------- + + PRINT_MAT(T,4,4); +matrix44transpose(T2, T); +//matrix44copy(T2,T); + cross3d( &T2[0][0], &T2[1][0], &T2[2][0] ); + cross3d( &T2[2][0], &T2[0][0], &T2[1][0] ); + normalize3d( &T2[0][0], &T2[0][0] ); + normalize3d( &T2[1][0], &T2[1][0] ); + normalize3d( &T2[2][0], &T2[2][0] ); +//matrix44copy(T2,T); +matrix44transpose(T, T2); + PRINT_MAT(T,4,4); + + + + //memcpy(T2,T,16*sizeof(float)); +// matrix44copy(T2,T); + matrix44transpose(T2,T); + quatfrommatrix( quat, &T2[0][0] ); + quatnormalize(quatNorm,quat); + quattoeuler(euler,quatNorm); + quattomatrix( T2, quatNorm ); + printf("rot %f %f %f len %f\n", euler[0], euler[1], euler[2], quatmagnitude(quat)); + PRINT(T,4,4); + +// matrix44copy(temp,T2); + matrix44transpose(temp,T2); + + memcpy(T2,temp,16*sizeof(float)); + for (i=0; i<3; i++) { + for (j=0; j<3; j++) { + T[i][j] = T2[j][i]; + } + } + PRINT(T2,4,4); + + +/* + CrossProduct(T[0][2],T[1][2],T[2][2], T[0][0],T[1][0],T[2][0], T[0][1],T[1][1],T[2][1]); + CrossProduct(T[0][0],T[1][0],T[2][0], T[0][1],T[1][1],T[2][1], T[0][2],T[1][2],T[2][2]); + CrossProduct(T[0][1],T[1][1],T[2][1], T[0][2],T[1][2],T[2][2], T[0][0],T[1][0],T[2][0]); + float xlen = sqrt(T[0][0]*T[0][0] + T[1][0]*T[1][0] + T[2][0]*T[2][0]); + float ylen = sqrt(T[0][1]*T[0][1] + T[1][1]*T[1][1] + T[2][1]*T[2][1]); + float zlen = sqrt(T[0][2]*T[0][2] + T[1][0]*T[1][2] + T[2][2]*T[2][2]); + T[0][0]/=xlen; T[1][0]/=xlen; T[2][0]/=xlen; + T[0][1]/=ylen; T[1][1]/=ylen; T[2][1]/=ylen; + T[0][2]/=zlen; T[1][2]/=zlen; T[2][2]/=zlen; +*/ + + + // PRINT_MAT(T,4,4); //------------------- // Plot the output points //------------------- @@ -453,7 +518,7 @@ PRINT(ab,2,1); S_out[1][i] = atan2(Tz, Ty); // vert //S_out[0][i] = Tx; //S_out[1][i] = Tz; - printf("point %i Txyz %f %f %f in %f %f out %f %f morph %f %f\n", i, Tx,Ty,Tz, S_in[0][i], S_in[1][i], S_out[0][i], S_out[1][i], S_morph[0][i], S_morph[1][i]); +// printf("point %i Txyz %f %f %f in %f %f out %f %f morph %f %f\n", i, Tx,Ty,Tz, S_in[0][i], S_in[1][i], S_out[0][i], S_out[1][i], S_morph[0][i], S_morph[1][i]); } } diff --git a/src/survive_cal.h b/src/survive_cal.h index c64b8f9..45b77f6 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -37,7 +37,7 @@ void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t t #define MIN_PTS_BEFORE_CAL 24 -#define DRPTS 1024 //Number of samples required in collection phase. +#define DRPTS 32 //Number of samples required in collection phase. #define POSE_OBJECTS 3 diff --git a/src/survive_data.c b/src/survive_data.c index 8c5c646..0873f7f 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -119,7 +119,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) { int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz. int base_station = is_new_pulse; - printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); + //printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length ); } } |