aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/poser_charlesslow.c343
-rw-r--r--src/poser_daveortho.c460
-rwxr-xr-xsrc/survive.c2
-rwxr-xr-xsrc/survive_cal.c168
-rw-r--r--src/survive_cal.h6
-rw-r--r--src/survive_cal_lhfind.c13
-rw-r--r--src/survive_config.c2
-rw-r--r--src/survive_data.c1
-rwxr-xr-xsrc/survive_vive.c4
9 files changed, 981 insertions, 18 deletions
diff --git a/src/poser_charlesslow.c b/src/poser_charlesslow.c
new file mode 100644
index 0000000..3cc7d9d
--- /dev/null
+++ b/src/poser_charlesslow.c
@@ -0,0 +1,343 @@
+#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;
+
+typedef struct
+{
+ int something;
+ //Stuff
+} DummyData;
+
+
+static FLT RunOpti( SurviveObject * so, PoserDataFullScene * fs, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat );
+
+int PoserCharlesSlow( 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;
+
+ int lh, cycle;
+ FLT dz, dy, dx;
+ for( lh = 0; lh < 2; lh++ )
+ {
+ FLT beste = 1e20;
+
+ FLT LighthousePos[3];
+ FLT LighthouseQuat[4];
+
+ LighthousePos[0] = 0;
+ LighthousePos[1] = 0;
+ LighthousePos[2] = 0;
+ LighthouseQuat[0] = 1;
+ LighthouseQuat[1] = 0;
+ LighthouseQuat[2] = 0;
+ LighthouseQuat[3] = 0;
+
+ FLT bestxyz[3];
+ memcpy( bestxyz, LighthousePos, sizeof( LighthousePos ) );
+
+ //STAGE1 1: Detemine vectoral position from lighthouse to target. Does not determine lighthouse-target distance.
+ //This also is constantly optimizing the lighthouse quaternion for optimal spotting.
+ FLT fullrange = 5; //Maximum search space for positions. (Relative to HMD)
+
+
+ //Sweep whole area 30 times
+ for( cycle = 0; cycle < 30; cycle ++ )
+ {
+
+ //Adjust position, one axis at a time, over and over until we zero in.
+ {
+ FLT bestxyzrunning[3];
+ beste = 1e20;
+
+ FILE * f;
+ if( cycle == 0 )
+ {
+ char filename[1024];
+ sprintf( filename, "calinfo/%d_lighthouse.dat", lh );
+ f = fopen( filename, "wb" );
+ }
+
+ //We split the space into this many groups (times 2) and
+ //if we're on the first cycle, we want to do a very linear
+ //search. As we refine our search we can then use a more
+ //binary search technique.
+ FLT splits = 4;
+ if( cycle == 0 ) splits = 32;
+ if( cycle == 1 ) splits = 13;
+ if( cycle == 2 ) splits = 10;
+ if( cycle == 3 ) splits = 8;
+ if( cycle == 4 ) splits = 5;
+
+ //Wwe search throug the whole space.
+ for( dz = 0; dz < fullrange; dz += fullrange/splits )
+ for( dy = -fullrange; dy < fullrange; dy += fullrange/splits )
+ for( dx = -fullrange; dx < fullrange; dx += fullrange/splits )
+ {
+ //Specificially adjust one axis at a time, searching for the best.
+ memcpy( LighthousePos, bestxyz, sizeof( LighthousePos ) );
+ LighthousePos[0] += dx; //These are adjustments to the "best" from last frame.
+ LighthousePos[1] += dy;
+ LighthousePos[2] += dz;
+
+ FLT ft;
+ //Try refining the search for the best orientation several times.
+ ft = RunOpti(so, fs, lh, 0, LighthousePos, LighthouseQuat);
+ if( cycle == 0 )
+ {
+ float sk = ft*10.;
+ if( sk > 1 ) sk = 1;
+ uint8_t cell = (1.0 - sk) * 255;
+ FLT epsilon = 0.1;
+
+ if( dz == 0 ) { /* Why is dz special? ? */
+ if ( dx > -epsilon && dx < epsilon )
+ cell = 255;
+ if ( dy > -epsilon && dy < epsilon )
+ cell = 128;
+ }
+
+ fprintf( f, "%c", cell );
+ }
+
+ if( ft < beste ) { beste = ft; memcpy( bestxyzrunning, LighthousePos, sizeof( LighthousePos ) ); }
+ }
+
+ if( cycle == 0 )
+ {
+ fclose( f );
+ }
+ memcpy( bestxyz, bestxyzrunning, sizeof( bestxyz ) );
+
+ //Print out the quality of the lock this time.
+ FLT dist = sqrt(bestxyz[0]*bestxyz[0] + bestxyz[1]*bestxyz[1] + bestxyz[2]*bestxyz[2]);
+ printf( "%f %f %f (%f) = %f\n", bestxyz[0], bestxyz[1], bestxyz[2], dist, beste );
+ }
+
+ //Every cycle, tighten up the search area.
+ fullrange *= 0.25;
+ }
+
+ if( beste > 0.1 )
+ {
+ //Error too high
+ SV_ERROR( "LH: %d / Best E %f Error too high\n", lh, beste );
+ return -1;
+ }
+
+ RunOpti(so, fs, lh, 1, LighthousePos, LighthouseQuat);
+
+ ctx->bsd[lh].PositionSet = 1;
+ copy3d( ctx->bsd[lh].Pose.Pos, LighthousePos );
+ quatcopy( ctx->bsd[lh].Pose.Rot, LighthouseQuat );
+#define ALT_COORDS
+#ifdef ALT_COORDS
+ so->FromLHPose[lh].Pos[0] = LighthousePos[0];
+ so->FromLHPose[lh].Pos[1] = LighthousePos[1];
+ so->FromLHPose[lh].Pos[2] = LighthousePos[2];
+ so->FromLHPose[lh].Rot[0] =-LighthouseQuat[0];
+ so->FromLHPose[lh].Rot[1] = LighthouseQuat[1];
+ so->FromLHPose[lh].Rot[2] = LighthouseQuat[2];
+ so->FromLHPose[lh].Rot[3] = LighthouseQuat[3];
+
+ quatrotatevector( so->FromLHPose[lh].Pos, so->FromLHPose[lh].Rot, so->FromLHPose[lh].Pos );
+#else
+ so->FromLHPose[lh].Pos[0] = LighthousePos[0];
+ so->FromLHPose[lh].Pos[1] = LighthousePos[1];
+ so->FromLHPose[lh].Pos[2] = LighthousePos[2];
+ so->FromLHPose[lh].Rot[0] = LighthouseQuat[0];
+ so->FromLHPose[lh].Rot[1] = LighthouseQuat[1];
+ so->FromLHPose[lh].Rot[2] = LighthouseQuat[2];
+ so->FromLHPose[lh].Rot[3] = LighthouseQuat[3];
+#endif
+ }
+
+ break;
+ }
+ case POSERDATA_DISASSOCIATE:
+ {
+ free( dd );
+ so->PoserData = 0;
+ //printf( "Need to disassociate.\n" );
+ break;
+ }
+ }
+
+}
+
+
+REGISTER_LINKTIME( PoserCharlesSlow );
+
+
+
+static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat )
+{
+ int i, p;
+ FLT UsToTarget[3];
+ FLT LastUsToTarget[3];
+ FLT mux = .9;
+ quatsetnone( LighthouseQuat );
+ FLT * hmd_points = hmd->sensor_locations;
+ FLT * hmd_normals = hmd->sensor_normals;
+
+ int first = 1, second = 0;
+
+ //First check to see if this is a valid viewpoint.
+ //If a sensor is pointed away from where we are testing a possible lighthouse position.
+ //BUT We get data from that light house, then we KNOW this is not a possible
+ //lighthouse position.
+ for( p = 0; p < 32; p++ )
+ {
+ int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2;
+ if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue;
+ FLT me_to_dot[3];
+ sub3d( me_to_dot, LighthousePos, &hmd_points[p*3] );
+ float dot = dot3d( &hmd_normals[p*3], me_to_dot );
+ if( dot < -.01 ) { return 1000; }
+ }
+ int iters = 6;
+
+ //Iterate over a refinement of the quaternion that constitutes the
+ //lighthouse.
+ for( i = 0; i < iters; i++ )
+ {
+ first = 1;
+ for( p = 0; p < 32; p++ )
+ {
+ int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2;
+ if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue;
+
+ //Find out where our ray shoots forth from.
+ FLT ax = fs->angles[p][lh][0];
+ FLT ay = fs->angles[p][lh][1];
+ //NOTE: Inputs may never be output with cross product.
+ //Create a fictitious normalized ray. Imagine the lighthouse is pointed
+ //straight in the +z direction, this is the lighthouse ray to the point.
+ FLT RayShootOut[3] = { sin(ax), sin(ay), 0 };
+ RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) );
+ FLT RayShootOutWorld[3];
+
+ quatnormalize( LighthouseQuat, LighthouseQuat );
+ //Rotate that ray by the current rotation estimation.
+ quatrotatevector( RayShootOutWorld, LighthouseQuat, RayShootOut );
+
+ //Find a ray from us to the target point.
+ sub3d( UsToTarget, &hmd_points[p*3], LighthousePos );
+ if( magnitude3d( UsToTarget ) < 0.0001 ) { continue; }
+ normalize3d( UsToTarget, UsToTarget );
+
+ FLT RotatedLastUs[3];
+ quatnormalize( LighthouseQuat, LighthouseQuat );
+ quatrotatevector( RotatedLastUs, LighthouseQuat, LastUsToTarget );
+
+ //Rotate the lighthouse around this axis to point at the HMD.
+ //If it's the first time, the axis is synthesized, if it's after that, use most recent point.
+ FLT ConcatQuat[4];
+ FLT AxisToRotate[3];
+ if( first )
+ {
+ cross3d( AxisToRotate, RayShootOutWorld, UsToTarget );
+ if( magnitude3d(AxisToRotate) < 0.0001 ) break;
+ normalize3d( AxisToRotate, AxisToRotate );
+ //Don't need to worry about being negative, cross product will fix it.
+ FLT RotateAmount = anglebetween3d( RayShootOutWorld, UsToTarget );
+ quatfromaxisangle( ConcatQuat, AxisToRotate, RotateAmount );
+ quatnormalize( ConcatQuat, ConcatQuat );
+ }
+ else
+ {
+ FLT Target[3];
+ FLT Actual[3];
+
+ copy3d( AxisToRotate, LastUsToTarget );
+ //Us to target = normalized ray from us to where we should be.
+ //RayShootOut = where we would be pointing.
+ sub3d( Target, UsToTarget, AxisToRotate ); //XXX XXX XXX WARNING THIS MESSES STUFF UP.
+ sub3d( Actual, RayShootOutWorld, AxisToRotate );
+ if( magnitude3d( Actual ) < 0.0001 || magnitude3d( Target ) < 0.0001 ) { continue; }
+ normalize3d( Target, Target );
+ normalize3d( Actual, Actual );
+
+ cross3d( AxisToRotate, Actual, Target ); //XXX Check: AxisToRotate should be equal to LastUsToTarget.
+ if( magnitude3d( AxisToRotate ) < 0.000001 ) { continue; }
+ normalize3d( AxisToRotate,AxisToRotate );
+
+ //printf( "%f %f %f === %f %f %f : ", PFTHREE( AxisToRotate ), PFTHREE( LastUsToTarget ) );
+ FLT RotateAmount = anglebetween3d( Actual, Target ) * mux;
+ //printf( "FA: %f (O:%f)\n", acos( dot3d( Actual, Target ) ), RotateAmount );
+ quatfromaxisangle( ConcatQuat, AxisToRotate, RotateAmount );
+ quatnormalize( ConcatQuat, ConcatQuat );
+ }
+
+
+ quatnormalize( ConcatQuat, ConcatQuat );
+ quatnormalize( LighthouseQuat, LighthouseQuat );
+ quatrotateabout( LighthouseQuat, ConcatQuat, LighthouseQuat ); //Checked. This appears to be
+
+ mux = mux * 0.94;
+ if( second ) { second = 0; }
+ if( first ) { first = 0; second = 1; }
+ copy3d( LastUsToTarget, RayShootOutWorld );
+ }
+ }
+
+ //Step 2: Determine error.
+ float errorsq = 0.0;
+ int count = 0;
+ for( p = 0; p < 32; p++ )
+ {
+ int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2;
+ if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue;
+
+ //Find out where our ray shoots forth from.
+ FLT ax = fs->angles[p][lh][0];
+ FLT ay = fs->angles[p][lh][1];
+ FLT RayShootOut[3] = { sin(ax), sin(ay), 0 };
+ RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) );
+
+ //Rotate that ray by the current rotation estimation.
+ quatrotatevector( RayShootOut, LighthouseQuat, RayShootOut );
+
+ //Point-line distance.
+ //Line defined by LighthousePos & Direction: RayShootOut
+
+ //Find a ray from us to the target point.
+ sub3d( UsToTarget, &hmd_points[p*3], LighthousePos );
+ FLT xproduct[3];
+ cross3d( xproduct, UsToTarget, RayShootOut );
+ FLT dist = magnitude3d( xproduct );
+ errorsq += dist*dist;
+ //if( print ) printf( "%f (%d(%d/%d))\n", dist, p, cd->ctsweeps[dataindex+0], cd->ctsweeps[dataindex+1] );
+ }
+ if( print ) printf( " = %f\n", sqrt( errorsq ) );
+ return sqrt(errorsq);
+}
diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c
new file mode 100644
index 0000000..9f3b55a
--- /dev/null
+++ b/src/poser_daveortho.c
@@ -0,0 +1,460 @@
+#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, &tOut[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.c b/src/survive.c
index 09eb432..6d49d55 100755
--- a/src/survive.c
+++ b/src/survive.c
@@ -53,7 +53,7 @@ SurviveContext * survive_init( int headless )
}
i = 0;
- const char * PreferredPoser = config_read_str( ctx->global_config_values, "DefualtPoser", "PoserDummy" );
+ const char * PreferredPoser = config_read_str( ctx->global_config_values, "DefaultPoser", "PoserDummy" );
PoserCB PreferredPoserCB = 0;
const char * FirstPoser = 0;
printf( "Available posers:\n" );
diff --git a/src/survive_cal.c b/src/survive_cal.c
index ad518b3..1a3be2d 100755
--- 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,11 +23,16 @@
#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 );
void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
{
+ static uint8_t lighthouses_completed = 0;
+
SurviveContext * ctx = (SurviveContext*)(ct->user);
SurviveCalData * cd = ctx->calptr;
int id = ct->user1;
@@ -53,7 +59,11 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
b->OOTXSet = 1;
config_set_lighthouse(ctx->lh_config,b,id);
-// config_save("config.json");
+ lighthouses_completed++;
+
+ if (lighthouses_completed >= NUM_LIGHTHOUSES) {
+ config_save(ctx, "config.json");
+ }
}
int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length )
@@ -115,6 +125,23 @@ void survive_cal_install( struct SurviveContext * ctx )
SV_INFO( "HMD not found, calibrating using Tracker" );
}
+
+
+ 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;
@@ -207,7 +234,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 )
@@ -229,7 +256,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;
@@ -287,7 +319,7 @@ static void handle_calibration( struct SurviveCalData *cd )
for( axis = 0; axis < 2; axis++ )
{
int dpmax = cd->all_counts[sen][lh][axis];
- if( dpmax < 50 ) continue;
+ if( dpmax < MIN_PTS_BEFORE_CAL ) continue;
int i;
FLT sumsweepangle = 0;
@@ -416,8 +448,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++ )
@@ -440,13 +474,129 @@ 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;
+
+ int dataindex = i*(2*NUM_LIGHTHOUSES)+j*2+0;
- if( survive_cal_lhfind( cd ) == 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];
+ lhp->Rot[1] = objfromlh->Rot[1];
+ lhp->Rot[2] = objfromlh->Rot[2];
+ lhp->Rot[3] = objfromlh->Rot[3];
+
+ quatrotatevector( lhp->Pos, lhp->Rot, objfromlh->Pos );
+
+ //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] );
+
+ /*
+ -0.204066 3.238746 -0.856369
+ 0.812203 -0.264897 0.505599 0.120520
+ -0.204066 3.238746 -0.856369
+
+ 0.020036 3.162476 -0.117896
+ -0.322354 0.450869 0.346281 0.756898
+ 0.020036 3.162476 -0.117896
+ */
+
+ /* Facing up, moved -x 1m.
+ ====> 0.446818 -0.309120 -0.747630 ====> -0.222356 -0.701865 -0.558656
+ ====> -0.341064 0.099785 0.887015 ====> 0.619095 0.727263 0.029786
+ IN PLACE, but rotated 90 * up.
+ ====> 0.374516 -0.370583 -0.606996 ====> -0.120238 -0.670330 -0.426896
+ ====> -0.231758 0.070437 0.765982 ====> 0.497615 0.625761 0.078759
+ */
+
+ 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];
+ quatrotatevector( pos, lhp->Rot, objfromlh->Pos );
+
+ pos[0] -= lhp->Pos[0];
+ pos[1] -= lhp->Pos[1];
+ pos[2] -= lhp->Pos[2];
+
+ //FLT rot[4] = {
+ // [0],
+ // lhp->Rot[1],
+ // lhp->Rot[2],
+ // lhp->Rot[3] };
+ //quatrotatevector( pos, lhp->Rot, 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] );
+ fprintf( stderr, "%f, %f, %f, %f\n", lhp->Rot[0], lhp->Rot[1], lhp->Rot[2], lhp->Rot[3] );
+
+ fprintf( stderr, "====> %f %f %f\n",
+ 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..ce61962 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -33,7 +33,9 @@ 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 MIN_PTS_BEFORE_CAL 24
+#define DRPTS 32
#define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2)
struct SurviveCalData
{
@@ -61,6 +63,8 @@ struct SurviveCalData
SurviveObject * hmd;
+ PoserCB ConfigPoserFn;
+
//Stage:
// 0: Idle
// 1: Collecting OOTX data.
diff --git a/src/survive_cal_lhfind.c b/src/survive_cal_lhfind.c
index a1bb2cc..cc32154 100644
--- a/src/survive_cal_lhfind.c
+++ b/src/survive_cal_lhfind.c
@@ -129,13 +129,15 @@ int survive_cal_lhfind( SurviveCalData * cd )
fullrange *= 0.25;
}
- if( beste > 0.01 )
+ if( beste > 0.1 )
{
//Error too high
SV_ERROR( "LH: %d / Best E %f Error too high\n", lh, beste );
return -1;
}
+ RunOpti(cd, lh, 1, LighthousePos, LighthouseQuat);
+
cd->ctx->bsd[lh].PositionSet = 1;
copy3d( cd->ctx->bsd[lh].Pose.Pos, LighthousePos );
quatcopy( cd->ctx->bsd[lh].Pose.Rot, LighthouseQuat );
@@ -198,6 +200,7 @@ static FLT RunOpti( SurviveCalData * cd, int lh, int print, FLT * LighthousePos,
RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) );
FLT RayShootOutWorld[3];
+ quatnormalize( LighthouseQuat, LighthouseQuat );
//Rotate that ray by the current rotation estimation.
quatrotatevector( RayShootOutWorld, LighthouseQuat, RayShootOut );
@@ -207,6 +210,7 @@ static FLT RunOpti( SurviveCalData * cd, int lh, int print, FLT * LighthousePos,
normalize3d( UsToTarget, UsToTarget );
FLT RotatedLastUs[3];
+ quatnormalize( LighthouseQuat, LighthouseQuat );
quatrotatevector( RotatedLastUs, LighthouseQuat, LastUsToTarget );
//Rotate the lighthouse around this axis to point at the HMD.
@@ -221,6 +225,7 @@ static FLT RunOpti( SurviveCalData * cd, int lh, int print, FLT * LighthousePos,
//Don't need to worry about being negative, cross product will fix it.
FLT RotateAmount = anglebetween3d( RayShootOutWorld, UsToTarget );
quatfromaxisangle( ConcatQuat, AxisToRotate, RotateAmount );
+ quatnormalize( ConcatQuat, ConcatQuat );
}
else
{
@@ -244,9 +249,13 @@ static FLT RunOpti( SurviveCalData * cd, int lh, int print, FLT * LighthousePos,
FLT RotateAmount = anglebetween3d( Actual, Target ) * mux;
//printf( "FA: %f (O:%f)\n", acos( dot3d( Actual, Target ) ), RotateAmount );
quatfromaxisangle( ConcatQuat, AxisToRotate, RotateAmount );
+ quatnormalize( ConcatQuat, ConcatQuat );
}
- quatrotateabout( LighthouseQuat, ConcatQuat, LighthouseQuat ); //Chekcked. This appears to be
+
+ quatnormalize( ConcatQuat, ConcatQuat );
+ quatnormalize( LighthouseQuat, LighthouseQuat );
+ quatrotateabout( LighthouseQuat, ConcatQuat, LighthouseQuat ); //Checked. This appears to be
mux = mux * 0.94;
if( second ) { second = 0; }
diff --git a/src/survive_config.c b/src/survive_config.c
index 3f0f199..10da9a6 100644
--- a/src/survive_config.c
+++ b/src/survive_config.c
@@ -209,8 +209,6 @@ const FLT* config_set_float_a(config_group *cg, const char *tag, const FLT* valu
assert(ptr!=NULL);
cv->data = ptr;
- printf("float array\n");
-
memcpy(cv->data,values,sizeof(FLT)*count);
cv->type = CONFIG_FLOAT_ARRAY;
cv->elements = count;
diff --git a/src/survive_data.c b/src/survive_data.c
index 63cc5c2..6824b0f 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -12,7 +12,6 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
//int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
//if( so->codename[0] != 'H' )
- //printf( "*** %s %d %d %d %d %d\n", so->codename, le->sensor_id, le->type, le->length, le->timestamp, le->timestamp-so->tsl );
if( le->sensor_id > SENSORS_PER_OBJECT )
{
diff --git a/src/survive_vive.c b/src/survive_vive.c
index 993125a..0cae6f0 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -93,8 +93,7 @@ void survive_usb_close( SurviveContext * t );
int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0 );
int survive_usb_poll( SurviveContext * ctx );
int survive_get_config( char ** config, SurviveViveData * ctx, int devno, int interface, int send_extra_magic );
-
-
+int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen );
static void handle_transfer(struct libusb_transfer* transfer)
@@ -306,6 +305,7 @@ int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, s
SV_INFO( "All enumerated devices attached." );
+ survive_vive_send_magic(ctx, sv, 1, 0, 0 );
//libUSB initialized. Continue.
return 0;