diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/poser_daveortho.c | 457 | ||||
-rw-r--r-- | src/survive_cal.c | 150 | ||||
-rw-r--r-- | src/survive_cal.h | 4 |
3 files changed, 603 insertions, 8 deletions
diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c new file mode 100644 index 0000000..80ddd90 --- /dev/null +++ b/src/poser_daveortho.c @@ -0,0 +1,457 @@ +#include "survive_cal.h" +#include <math.h> +#include <string.h> +#include "linmath.h" +#include <survive.h> +#include <stdio.h> +#include <stdlib.h> +#include <dclapack.h> +#include <linmath.h> + +static int LH_ID; + +void OrthoSolve( + FLT T[4][4], // OUTPUT: 4x4 transformation matrix + FLT S_out[2][SENSORS_PER_OBJECT], // OUTPUT: array of screenspace points + FLT S_in[2][SENSORS_PER_OBJECT], // INPUT: array of screenspace points + FLT X_in[3][SENSORS_PER_OBJECT], // INPUT: array of offsets + int nPoints); + + +typedef struct +{ + int something; + //Stuff +} DummyData; + +int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) +{ + PoserType pt = pd->pt; + SurviveContext * ctx = so->ctx; + DummyData * dd = so->PoserData; + + if( !dd ) so->PoserData = dd = malloc( sizeof( DummyData ) ); + + switch( pt ) + { + case POSERDATA_IMU: + { + PoserDataIMU * imu = (PoserDataIMU*)pd; + //printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, imu->accel[0], imu->accel[1], imu->accel[2], imu->gyro[0], imu->gyro[1], imu->gyro[2] ); + break; + } + case POSERDATA_LIGHT: + { + PoserDataLight * l = (PoserDataLight*)pd; + //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode ); + break; + } + case POSERDATA_FULL_SCENE: + { + PoserDataFullScene * fs = (PoserDataFullScene*)pd; + + for( LH_ID = 0; LH_ID < 2; LH_ID++ ) + { + int i; + int max_hits = 0; + FLT S_in[2][SENSORS_PER_OBJECT]; + FLT X_in[3][SENSORS_PER_OBJECT]; + for( i = 0; i < SENSORS_PER_OBJECT; i++ ) + { + //Load all our valid points into something the LHFinder can use. + if( fs->lengths[i][LH_ID][0] > 0 ) + { + S_in[0][max_hits] = fs->angles[i][LH_ID][0]; + S_in[1][max_hits] = fs->angles[i][LH_ID][1]; + X_in[0][max_hits] = so->sensor_locations[i*3+0]; + X_in[1][max_hits] = so->sensor_locations[i*3+1]; + X_in[2][max_hits] = so->sensor_locations[i*3+2]; + max_hits++; + } + + } + FLT tOut[4][4]; + FLT S_out[2][SENSORS_PER_OBJECT]; + OrthoSolve( tOut, S_out, S_in, X_in, max_hits ); + + //Now, we need to solve where we are as a function of where + //the lighthouses are. + FLT quat[4]; + FLT posoff[3] = { tOut[0][3], tOut[1][3], tOut[2][3] }; + FLT MT[4][4]; + //matrix44transpose( MT, &T[0][0] ); + matrix44copy( &MT[0][0], &tOut[0][0] ); + + quatfrommatrix( quat, &MT[0][0] ); + //printf( "QUAT: %f %f %f %f = %f\n", quat[0], quat[1], quat[2], quat[3], quatmagnitude(quat) ); + //quat[2] -= 0.005; //fixes up lh0 in test data set. + quatnormalize( quat, quat ); + printf( "QUAT: %f %f %f %f = %f [%f %f %f]\n", quat[0], quat[1], quat[2], quat[3], quatmagnitude(quat), posoff[0], posoff[1], posoff[2] ); + + for( i = 0; i < max_hits;i++ ) + { + FLT pt[3] = { X_in[0][i], X_in[1][i], X_in[2][i] }; + quatrotatevector( pt, quat, pt ); + add3d( pt, pt, posoff ); + printf( "OUT %f %f %f ANGLE %f %f AOUT %f %f\n", + pt[0], pt[1], pt[2], + S_in[0][i], S_in[1][i], atan2( pt[0], pt[1] ), atan2( pt[2], pt[1] ) ); + } + + so->FromLHPose[LH_ID].Pos[0] = posoff[0]; + so->FromLHPose[LH_ID].Pos[1] = posoff[1]; + so->FromLHPose[LH_ID].Pos[2] = posoff[2]; + so->FromLHPose[LH_ID].Rot[0] = quat[0]; + so->FromLHPose[LH_ID].Rot[1] = quat[1]; + so->FromLHPose[LH_ID].Rot[2] = quat[2]; + so->FromLHPose[LH_ID].Rot[3] = quat[3]; + } + + break; + } + case POSERDATA_DISASSOCIATE: + { + free( dd ); + so->PoserData = 0; + //printf( "Need to disassociate.\n" ); + break; + } + } + +} + + +REGISTER_LINKTIME( PoserDaveOrtho ); + + + + + +#define PRINT_MAT(A,M,N) { \ + int m,n; \ + printf(#A "\n"); \ + for (m=0; m<M; m++) { \ + for (n=0; n<N; n++) { \ + printf("%f\t", A[m][n]); \ + } \ + printf("\n"); \ + } \ +} + +#define CrossProduct(ox,oy,oz,a,b,c,x,y,z) { \ + ox=(b)*(z)-(c)*(y); \ + oy=(c)*(x)-(a)*(z); \ + oz=(a)*(y)-(b)*(x); } + +void OrthoSolve( + FLT T[4][4], // OUTPUT: 4x4 transformation matrix + FLT S_out[2][SENSORS_PER_OBJECT], // OUTPUT: array of screenspace points + FLT S_in[2][SENSORS_PER_OBJECT], // INPUT: array of screenspace points + FLT X_in[3][SENSORS_PER_OBJECT], // INPUT: array of offsets + int nPoints) +{ + int i,j,k; + FLT R[3][3]; // OUTPUT: 3x3 rotation matrix + FLT trans[3]; // INPUT: x,y,z translation vector + + //-------------------- + // Remove the center of the HMD offsets, and the screen space + //-------------------- + FLT xbar[3] = {0.0, 0.0, 0.0}; + FLT sbar[2] = {0.0, 0.0}; + FLT S[2][SENSORS_PER_OBJECT]; + FLT X[3][SENSORS_PER_OBJECT]; + FLT inv_nPoints = 1.0 / nPoints; + for (i=0; i<nPoints; i++) { + xbar[0] += X_in[0][i]; + xbar[1] += X_in[1][i]; + xbar[2] += X_in[2][i]; + sbar[0] += S_in[0][i]; + sbar[1] += S_in[1][i]; + } + for (j=0; j<3; j++) { xbar[j] *= inv_nPoints; } + for (j=0; j<2; j++) { sbar[j] *= inv_nPoints; } + for (i=0; i<nPoints; i++) { + X[0][i] = X_in[0][i] - xbar[0]; + X[1][i] = X_in[1][i] - xbar[1]; + X[2][i] = X_in[2][i] - xbar[2]; + S[0][i] = S_in[0][i] - sbar[0]; + S[1][i] = S_in[1][i] - sbar[1]; + } + + //-------------------- + // Solve for the morph matrix + // S = M X + // thus + // (SX^t)(XX^t)^-1 = M + //-------------------- + FLT Xt[SENSORS_PER_OBJECT][3]; + FLT XXt[3][3]; + FLT invXXt[3][3]; + FLT SXt[2][3]; + FLT M[2][3]; // Morph matrix! (2 by 3) + TRANSP(X,Xt,3,nPoints); + MUL(X,Xt,XXt,3,nPoints,3); + MUL(S,Xt,SXt,2,nPoints,3); + INV(XXt,invXXt,3); + MUL(SXt,invXXt,M,2,3,3); +//PRINT(M,2,3); + +// Double checking work +FLT S_morph[2][SENSORS_PER_OBJECT]; +MUL(M,X,S_morph,2,3,nPoints); +for (i=0; i<nPoints; i++) { S_morph[0][i]+=sbar[0]; S_morph[1][i]+=sbar[1]; } + + //-------------------- + // Solve for the non-trivial vector + // uf -- vector that goes into the camera + //-------------------- + FLT uM[3][3] = { + { M[0][0], M[0][1], M[0][2] }, + { M[1][0], M[1][1], M[1][2] }, + { 3.14567, -1.2345, 4.32567 } }; // Morph matrix with appended row +//PRINT(uM,3,3); +// ToDo: Pick a number for the bottom that is NOT linearly separable with M[0] and M[1] + FLT B[3][1] = { {0.0}, {0.0}, {1.0} }; + FLT inv_uM[3][3]; + FLT uf[3][1]; + INV(uM,inv_uM,3); + MUL(inv_uM,B,uf,3,3,1); + + //-------------------- + // Solve for unit length vector + // f that goes into the camera + //-------------------- + FLT uf_len = sqrt( uf[0][0]*uf[0][0] + uf[1][0]*uf[1][0] + uf[2][0]*uf[2][0] ); + FLT f[3][1] = { {uf[0][0]/uf_len}, {uf[1][0]/uf_len}, {uf[2][0]/uf_len} }; +//PRINT(uf,3,1); +//PRINT(f,3,1); + +//FLT check[3][1]; +//MUL(uM,uf,check,3,3,1); +//PRINT(check,3,1); + + //-------------------- + // take cross products to get vectors u,r + //-------------------- + FLT u[3][1], r[3][1]; + CrossProduct(u[0][0],u[1][0],u[2][0],f[0][0],f[1][0],f[2][0],1.0,0.0,0.0); + FLT inv_ulen = 1.0 / sqrt( u[0][0]*u[0][0] + u[1][0]*u[1][0] + u[2][0]*u[2][0] ); + u[0][0]*=inv_ulen; u[1][0]*=inv_ulen; u[2][0]*=inv_ulen; + CrossProduct(r[0][0],r[1][0],r[2][0],f[0][0],f[1][0],f[2][0],u[0][0],u[1][0],u[2][0]); +//PRINT(u,3,1); +//PRINT(r,3,1); + + //-------------------- + // Use morph matrix to get screen space + // uhat,rhat + //-------------------- + FLT uhat[2][1], rhat[2][1], fhat[2][1]; + MUL(M,f,fhat,2,3,1); + MUL(M,u,uhat,2,3,1); + MUL(M,r,rhat,2,3,1); + FLT fhat_len = sqrt( fhat[0][0]*fhat[0][0] + fhat[1][0]*fhat[1][0] ); + FLT uhat_len = sqrt( uhat[0][0]*uhat[0][0] + uhat[1][0]*uhat[1][0] ); + FLT rhat_len = sqrt( rhat[0][0]*rhat[0][0] + rhat[1][0]*rhat[1][0] ); + FLT urhat_len = 0.5 * (uhat_len + rhat_len); +/* +printf("fhat %f %f (len %f)\n", fhat[0][0], fhat[1][0], fhat_len); +printf("uhat %f %f (len %f)\n", uhat[0][0], uhat[1][0], uhat_len); +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); + + //-------------------- + // Rescale the axies to be of the proper length + //-------------------- + FLT x[3][1] = { {M[0][0]*ydist}, {0.0}, {M[1][0]*ydist} }; + FLT y[3][1] = { {M[0][1]*ydist}, {0.0}, {M[1][1]*ydist} }; + FLT z[3][1] = { {M[0][2]*ydist}, {0.0}, {M[1][2]*ydist} }; + // we know the distance into (or out of) the camera for the z axis, + // but we don't know which direction . . . + FLT x_y = sqrt(1.0 - x[0][0]*x[0][0] - x[2][0]*x[2][0]); + FLT y_y = sqrt(1.0 - y[0][0]*y[0][0] - y[2][0]*y[2][0]); + FLT z_y = sqrt(1.0 - z[0][0]*z[0][0] - z[2][0]*z[2][0]); + + 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]; + FLT yz_dot2 = y[0][0]*z[0][0] + y[2][0]*z[2][0]; + FLT zx_dot2 = z[0][0]*x[0][0] + z[2][0]*x[2][0]; + for (i=0;i<2;i++) { + for (j=0;j<2;j++) { + for(k=0;k<2;k++) { + + // Calculate the error term + FLT xy_dot = xy_dot2 + x_y*y_y; + FLT yz_dot = yz_dot2 + y_y*z_y; + FLT zx_dot = zx_dot2 + z_y*x_y; + FLT err = _ABS(xy_dot) + _ABS(yz_dot) + _ABS(zx_dot); + + // Calculate the handedness + 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); + + // 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; } + if ( i == 0 && j == 1 && k == 0) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; } + z_y = -z_y; + } + y_y = -y_y; + } + x_y = -x_y; + } + printf("bestErr %f\n", bestErr); +*/ + + //------------------------- + // A test version of the rescaling to the proper length + //------------------------- + FLT ydist2 = ydist; + FLT bestBestErr = 9999.0; + FLT bestYdist = 0; + //for (ydist2=ydist-0.1; ydist2<ydist+0.1; ydist2+=0.0001) + { + FLT x2[3][1] = { {M[0][0]*ydist2}, {0.0}, {M[1][0]*ydist2} }; + FLT y2[3][1] = { {M[0][1]*ydist2}, {0.0}, {M[1][1]*ydist2} }; + FLT z2[3][1] = { {M[0][2]*ydist2}, {0.0}, {M[1][2]*ydist2} }; + + // we know the distance into (or out of) the camera for the z axis, + // but we don't know which direction . . . + FLT x_y = sqrt(1.0 - x2[0][0]*x2[0][0] - x2[2][0]*x2[2][0]); + FLT y_y = sqrt(1.0 - y2[0][0]*y2[0][0] - y2[2][0]*y2[2][0]); + FLT z_y = sqrt(1.0 - z2[0][0]*z2[0][0] - z2[2][0]*z2[2][0]); + + if( x_y != x_y ) x_y = 0; + 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 ); + + // Exhaustively flip the minus sign of the z axis until we find the right one . . . + FLT bestErr = 9999.0; + FLT xy_dot2 = x2[0][0]*y2[0][0] + x2[2][0]*y2[2][0]; + FLT yz_dot2 = y2[0][0]*z2[0][0] + y2[2][0]*z2[2][0]; + FLT zx_dot2 = z2[0][0]*x2[0][0] + z2[2][0]*x2[2][0]; + for (i=0;i<2;i++) { + for (j=0;j<2;j++) { + for(k=0;k<2;k++) { + + // Calculate the error term + FLT xy_dot = xy_dot2 + x_y*y_y; + FLT yz_dot = yz_dot2 + y_y*z_y; + FLT zx_dot = zx_dot2 + z_y*x_y; + FLT err = _ABS(xy_dot) + _ABS(yz_dot) + _ABS(zx_dot); + + // Calculate the handedness + 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); + + // 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; } + z_y = -z_y; + } + y_y = -y_y; + } + x_y = -x_y; + } + printf("ydist2 %f bestErr %f\n",ydist2,bestErr); + + if (bestErr < bestBestErr) { + memcpy(x,x2,3*sizeof(FLT)); + memcpy(y,y2,3*sizeof(FLT)); + memcpy(z,z2,3*sizeof(FLT)); + bestBestErr = bestErr; + bestYdist = ydist2; + } + } + 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]; + FLT y1 = x[1][0]*X[0][i] + y[1][0]*X[1][i] + z[1][0]*X[2][i]; + FLT z1 = x[2][0]*X[0][i] + y[2][0]*X[1][i] + z[2][0]*X[2][i]; + printf("x1z1 %f %f y1 %f\n", x1, z1, y1); + } +*/ +/* + //-------------------- + // Combine uhat and rhat to figure out the unit x-vector + //-------------------- + FLT xhat[2][1] = { {0.0}, {1.0} }; + FLT urhat[2][2] = { + {uhat[0][0], uhat[1][0]}, + {rhat[0][0], rhat[1][0]} }; + FLT inv_urhat[2][2]; + FLT ab[2][1]; + INV(urhat,inv_urhat,2); + MUL(inv_urhat,xhat,ab,2,2,1); +PRINT(ab,2,1); + FLT a = ab[0][0], b = ab[1][0]; + + //------------------- + // calculate the xyz coordinate system + //------------------- + FLT y[3][1] = { {f[0][0]}, {f[1][0]}, {f[2][0]} }; + FLT x[3][1] = { {a*u[0][0] + b*r[0][0]}, {a*u[1][0] + b*r[1][0]}, {a*u[2][0] + b*r[2][0]} }; + FLT inv_xlen = 1.0 / sqrt( x[0][0]*x[0][0] + x[1][0]*x[1][0] + x[2][0]*x[2][0] ); + x[0][0]*=inv_xlen; x[1][0]*=inv_xlen; x[2][0]*=inv_xlen; + FLT z[3][1]; + CrossProduct(z[0][0],z[1][0],z[2][0],x[0][0],x[1][0],x[2][0],y[0][0],y[1][0],y[2][0]); +*/ + // Store into the rotation matrix + for (i=0; i<3; i++) { R[i][0] = x[i][0]; R[i][1] = y[i][0]; R[i][2] = z[i][0]; } +//PRINT(R,3,3); + + //------------------- + // Calculate the translation of the centroid + //------------------- + trans[0]=tan(sbar[0]); trans[1]=1.0; trans[2]=tan(sbar[1]); + FLT inv_translen = ydist / sqrt( trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2] ); + trans[0]*=inv_translen; trans[1]*=inv_translen; trans[2]*=inv_translen; + + //------------------- + // Add in the centroid point + //------------------- + trans[0] -= xbar[0]*R[0][0] + xbar[1]*R[0][1] + xbar[2]*R[0][2]; + trans[1] -= xbar[0]*R[1][0] + xbar[1]*R[1][1] + xbar[2]*R[1][2]; + trans[2] -= xbar[0]*R[2][0] + xbar[1]*R[2][1] + xbar[2]*R[2][2]; + FLT transdist = sqrt( trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2] ); + + //------------------- + // Pack into the 4x4 transformation matrix + //------------------- + T[0][0]=R[0][0]; T[0][1]=R[0][1]; T[0][2]=R[0][2]; T[0][3]=trans[0]; + T[1][0]=R[1][0]; T[1][1]=R[1][1]; T[1][2]=R[1][2]; T[1][3]=trans[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); + //------------------- + // Plot the output points + //------------------- + for (i=0; i<nPoints; i++) { + FLT Tx = T[0][0]*X_in[0][i] + T[0][1]*X_in[1][i] + T[0][2]*X_in[2][i] + T[0][3]; + FLT Ty = T[1][0]*X_in[0][i] + T[1][1]*X_in[1][i] + T[1][2]*X_in[2][i] + T[1][3]; + FLT Tz = T[2][0]*X_in[0][i] + T[2][1]*X_in[1][i] + T[2][2]*X_in[2][i] + T[2][3]; + S_out[0][i] = atan2(Tx, Ty); // horiz + 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]); + } + +} + diff --git a/src/survive_cal.c b/src/survive_cal.c index 06914eb..fd2f806 100644 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -14,6 +14,7 @@ #include <string.h> #include <sys/stat.h> #include <sys/types.h> +#include <linmath.h> #include "survive_config.h" @@ -22,6 +23,9 @@ #define NEEDED_TIMES_OF_COMMON 5 #define DRPTS_NEEDED_FOR_AVG ((int)(DRPTS*3/4)) + + + static void handle_calibration( struct SurviveCalData *cd ); static void reset_calibration( struct SurviveCalData * cd ); @@ -109,6 +113,23 @@ void survive_cal_install( struct SurviveContext * ctx ) return; } + + + const char * DriverName; + const char * PreferredPoser = config_read_str( ctx->global_config_values, "ConfigPoser", "PoserDaveOrtho" ); + PoserCB PreferredPoserCB = 0; + const char * FirstPoser = 0; + printf( "Available posers:\n" ); + i = 0; + while( ( DriverName = GetDriverNameMatching( "Poser", i++ ) ) ) + { + PoserCB p = GetDriver( DriverName ); + if( !PreferredPoserCB ) PreferredPoserCB = p; + int ThisPoser = strcmp( DriverName, PreferredPoser ) == 0; + if( ThisPoser ) PreferredPoserCB = p; + } + cd->ConfigPoserFn = PreferredPoserCB; + printf( "Got config poser: %p\n", cd->ConfigPoserFn ); ootx_packet_clbk = ootx_packet_clbk_d; ctx->calptr = cd; @@ -201,7 +222,7 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin if( cd->peak_counts >= PTS_BEFORE_COMMON ) { - int tfc = cd->times_found_common; +/* int tfc = cd->times_found_common; if( cd->found_common ) { if( tfc >= NEEDED_TIMES_OF_COMMON ) @@ -223,7 +244,12 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin SV_INFO( "Stage 2 bad - redoing. %d %d %d", cd->peak_counts, cd->found_common, tfc ); reset_calibration( cd ); cd->times_found_common = 0; - } + }*/ + + SV_INFO( "Stage 2 moving to stage 3. %d %d", cd->peak_counts, cd->found_common ); + reset_calibration( cd ); + cd->stage = 3; + cd->found_common = 1; } break; @@ -410,8 +436,10 @@ static void handle_calibration( struct SurviveCalData *cd ) } fclose( hists ); fclose( ptinfo ); - +/* //Comb through data and make sure we still have a sensor on a WM that + //We don't do this anymore. + int bcp_senid = 0; int bcp_count = 0; for( sen = 0; sen < MAX_SENSORS_TO_CAL; sen++ ) @@ -434,13 +462,121 @@ static void handle_calibration( struct SurviveCalData *cd ) reset_calibration( cd ); return; } - cd->senid_of_checkpt = bcp_senid; +*/ + + int i, j; + PoserDataFullScene fsd; + fsd.pt = POSERDATA_FULL_SCENE; + for( j = 0; j < NUM_LIGHTHOUSES; j++ ) + for( i = 0; i < SENSORS_PER_OBJECT; i++ ) + { + int gotdata = 0; - if( survive_cal_lhfind( cd ) == 0 ) + int dataindex = i*(2*NUM_LIGHTHOUSES)+j*2+0; + + if( cd->ctsweeps[dataindex+0] < DRPTS_NEEDED_FOR_AVG || + cd->ctsweeps[dataindex+1] < DRPTS_NEEDED_FOR_AVG ) + { + fsd.lengths[i][j][0] = -1; + fsd.lengths[i][j][1] = -1; + continue; + } + fsd.lengths[i][j][0] = cd->avglens[dataindex+0]; + fsd.lengths[i][j][1] = cd->avglens[dataindex+1]; + fsd.angles[i][j][0] = cd->avgsweeps[dataindex+0]; + fsd.angles[i][j][1] = cd->avgsweeps[dataindex+1]; + } + + cd->ConfigPoserFn( cd->hmd, (PoserData*)&fsd ); + if( 1 ) { - SV_INFO( "Stage 4 succeeded." ); - cd->stage = 5; + static int notfirstcal = 0; + SV_INFO( "Stage 4 succeeded. Inverting %d", notfirstcal ); + + if( !notfirstcal ) + { + // XXX This part is /all/ wrong. + // XXX This part is /all/ wrong. + // XXX This part is /all/ wrong. + + //OK! We've arrived. Now, we have to get the LH's pose from. + int lh; + for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ ) + { + SurvivePose * objfromlh = &cd->hmd->FromLHPose[lh]; + SurvivePose * lhp = &ctx->bsd[lh].Pose; + + + lhp->Pos[0] = objfromlh->Pos[0]; + lhp->Pos[1] = objfromlh->Pos[1]; + lhp->Pos[2] = objfromlh->Pos[2]; + + lhp->Rot[0] = objfromlh->Rot[0]*-1; + lhp->Rot[1] = objfromlh->Rot[1]; + lhp->Rot[2] = objfromlh->Rot[2]; + lhp->Rot[3] = objfromlh->Rot[3]; + + //Write lhp from the inverse of objfromlh + quatrotatevector( lhp->Pos, lhp->Rot, lhp->Pos ); + + + fprintf( stderr, "%f %f %f\n", objfromlh->Pos[0], objfromlh->Pos[1], objfromlh->Pos[2] ); + fprintf( stderr, "%f %f %f %f\n", objfromlh->Rot[0], objfromlh->Rot[1], objfromlh->Rot[2], objfromlh->Rot[3] ); + fprintf( stderr, "%f %f %f\n", lhp->Pos[0], lhp->Pos[1], lhp->Pos[2] ); + /* + -0.074179 2.793859 0.519508 + -0.092802 0.087361 0.872115 0.472409 + 0.379494 1.906039 2.074617 + + 0.695987 3.346798 -0.776169 + -0.258207 0.966008 -0.008604 0.009116 + 0.614156 -2.521008 2.356952 + + */ + + printf( "\n" ); + + } + notfirstcal = 1; + } + else + { + for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ ) + { + SurvivePose * objfromlh = &cd->hmd->FromLHPose[lh]; + SurvivePose * lhp = &ctx->bsd[lh].Pose; + + FLT pos[3] = { objfromlh->Pos[0], + objfromlh->Pos[1], + objfromlh->Pos[2] }; + + FLT rot[4] = { objfromlh->Rot[0]*-1, + objfromlh->Rot[1], + objfromlh->Rot[2], + objfromlh->Rot[3] }; + + fprintf( stderr, "====> %f %f %f ", + pos[0], pos[1], pos[2] ); + + quatrotatevector( pos, rot, pos ); + pos[0] -= lhp->Pos[0]; + pos[1] -= lhp->Pos[1]; + pos[2] -= lhp->Pos[2]; + + + quatrotatevector( lhp->Pos, objfromlh->Rot, lhp->Pos ); + + fprintf( stderr, "====> %f %f %f ", + pos[0], pos[1], pos[2] ); + + } + } + fprintf( stderr, "\n" ); + + + reset_calibration( cd ); +// cd->stage = 5; } else { diff --git a/src/survive_cal.h b/src/survive_cal.h index dd2a1e2..d21ba36 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -33,7 +33,7 @@ void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timein void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); #define MAX_SENSORS_TO_CAL 96 -#define DRPTS 1024 +#define DRPTS 128 #define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2) struct SurviveCalData { @@ -61,6 +61,8 @@ struct SurviveCalData SurviveObject * hmd; + PoserCB ConfigPoserFn; + //Stage: // 0: Idle // 1: Collecting OOTX data. |