aboutsummaryrefslogtreecommitdiff
path: root/tools/planetest/camfind.c
diff options
context:
space:
mode:
Diffstat (limited to 'tools/planetest/camfind.c')
-rw-r--r--tools/planetest/camfind.c248
1 files changed, 169 insertions, 79 deletions
diff --git a/tools/planetest/camfind.c b/tools/planetest/camfind.c
index 6a6650b..1e9b2b9 100644
--- a/tools/planetest/camfind.c
+++ b/tools/planetest/camfind.c
@@ -7,125 +7,174 @@
#define PTS 32
#define MAX_CHECKS 20000
-double hmd_points[PTS*3];
-double hmd_point_angles[PTS*2];
+FLT hmd_points[PTS*3];
+FLT hmd_norms[PTS*3];
+FLT hmd_point_angles[PTS*2];
int hmd_point_counts[PTS*2];
int LoadData( char Camera );
//Values used for RunTest()
-float LighthousePos[3] = { 0, 0, 0 };
-float LighthouseQuat[4] = { 1, 0, 0, 0 };
+FLT LighthousePos[3] = { 0, 0, 0 };
+FLT LighthouseQuat[4] = { 1, 0, 0, 0 };
//Actual values
-float xyzypr[6] = { 0, 0, 0, 0, 0, 0 };
+FLT xyzypr[6] = { 0, 2, 2, 0, 0, 0 };
-float RunOpti( int axis );
-float RunTest( int print );
+FLT RunOpti( int axis );
+FLT RunTest( int print );
void PrintOpti();
int main()
{
int i;
+ //Load either 'L' (LH1) or 'R' (LH2) data.
if( LoadData( 'R' ) ) return 5;
int opti = 0;
int cycle = 0;
int axis = 0;
- float dx, dy, dz;
+ FLT dx, dy, dz;
- float bestxyzypr[6];
+ FLT bestxyzypr[6];
memcpy( bestxyzypr, xyzypr, sizeof( xyzypr ) );
- float fullrange = 5;
- for( cycle = 0; cycle < 4; cycle ++ )
+ FLT fullrange = 5; //Maximum search space for positions. (Relative to HMD)
+ for( cycle = 0; cycle < 20; cycle ++ )
{
-// for( axis = 0; axis < 3; axis++ )
-// {
- float bestxyzyprrunning[6];
- float beste = 1e20;
- for( dz = -fullrange; dz < fullrange; dz += fullrange/5 )
- {
- for( dy = -fullrange; dy < fullrange; dy += fullrange/5 )
+
+ //Adjust position, one axis at a time, over and over until we zero in.
+#define FULLSEARCH
+
+#ifndef FULLSEARCH
+ for( axis = 0; axis < 3; axis++ )
+#endif
+ {
+ FLT bestxyzyprrunning[6];
+ FLT beste = 1e20;
+
+ //Try a bunch of points in this axis.
+#ifdef FULLSEARCH
+ for( dz = -fullrange; dz <= fullrange; dz += fullrange/3 )
+ for( dy = -fullrange; dy <= fullrange; dy += fullrange/3 )
+ for( dx = -fullrange; dx <= fullrange; dx += fullrange/3 )
{
- for( dx = -fullrange; dx < fullrange; dx += fullrange/5 )
+#else
+ for( dx = -fullrange; dx <= fullrange; dx += fullrange/5 )
{
+#endif
+ //Specificially adjust one axis at a time, searching for the best.
memcpy( xyzypr, bestxyzypr, sizeof( xyzypr ) );
+#ifdef FULLSEARCH
xyzypr[0] += dx;
xyzypr[1] += dy;
xyzypr[2] += dz;
+#else
+ xyzypr[axis] += dx;
+#endif
+ //if( axis == 2 && xyzypr[2] < 0 ) continue;
+
xyzypr[3] = 0;
xyzypr[4] = 0;
xyzypr[5] = 0;
- float ft;
+ FLT ft;
int reopt = 0;
- for( reopt = 0; reopt < 10; reopt++ )
- for( opti = 3; opti < 6; opti++ )
+
+ //Try refining the search for the best orientation several times.
+ for( reopt = 0; reopt < 4; reopt++ )
{
- ft = RunOpti( opti );
+ //XXX This search mechanism is insufficient :(
+ //Search through each axis every time.
+ for( opti = 3; opti < 6; opti++ )
+ {
+ ft = RunOpti( opti );
+ }
+ if( ft < beste ) { beste = ft; memcpy( bestxyzyprrunning, xyzypr, sizeof( xyzypr ) ); }
}
- if( ft < beste ) { beste = ft; memcpy( bestxyzyprrunning, xyzypr, sizeof( xyzypr ) ); }
- //printf( "%f %f %f %f\n", xyzypr[0], xyzypr[1], xyzypr[2], ft );
- }}}
- memcpy( bestxyzypr, bestxyzyprrunning, sizeof( bestxyzypr ) );
- printf( "%f %f %f %f %f %f = %f\n", bestxyzypr[0], bestxyzypr[1], bestxyzypr[2], bestxyzypr[3], bestxyzypr[4], bestxyzypr[5], beste );
-// }
-
- fullrange *= 0.3;
+ //printf( " %f %f %f %f\n", xyzypr[0], xyzypr[1], xyzypr[2], ft );
+#ifndef FULLSEARCH
+ memcpy( bestxyzypr, bestxyzyprrunning, sizeof( bestxyzypr ) );
+#endif
+ }
+#ifdef FULLSEARCH
+ memcpy( bestxyzypr, bestxyzyprrunning, sizeof( bestxyzypr ) );
+#endif
+
+ //Print out the quality of the lock this time.
+ FLT dist = sqrt(bestxyzypr[0]*bestxyzypr[0] + bestxyzypr[1]*bestxyzypr[1] + bestxyzypr[2]*bestxyzypr[2]);
+ printf( "%f %f %f (%f) %f %f %f = %f\n", bestxyzypr[0], bestxyzypr[1], bestxyzypr[2], dist, bestxyzypr[3], bestxyzypr[4], bestxyzypr[5], beste );
+ }
+/*
+ if( bestxyzypr[2] < 0 )
+ {
+ bestxyzypr[0] *= -1;
+ bestxyzypr[1] *= -1;
+ bestxyzypr[2] *= -1;
+ }
+*/
+ //Every cycle, tighten up the search area.
+ fullrange *= 0.6;
}
//Use bestxyzypr
memcpy( xyzypr, bestxyzypr, sizeof( xyzypr ) );
+
+
+ //Print out plane accuracies with these settings.
PrintOpti();
}
void PrintOpti()
{
- float xyzyprchange[6];
+ FLT xyzyprchange[6];
memcpy( xyzyprchange, xyzypr, sizeof( xyzypr ) );
quatfromeuler( LighthouseQuat, &xyzyprchange[3] );
memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) );
- float ft = RunTest(1);
+ FLT ft = RunTest(1);
+ printf( "Final RMS: %f\n", ft );
}
-float RunOpti( int axis )
+FLT RunOpti( int axis )
{
- float xyzyprchange[6];
+ FLT xyzyprchange[6];
memcpy( xyzyprchange, xyzypr, sizeof( xyzypr ) );
int i;
- float minv = 1e20;
- float fv = -1.3;
- float bv = 0;
+ FLT minv = 1e20;
+ FLT fv = -3.3;
+ FLT bv = 0;
- //Coarse linear search
- for( i = 0; i < 26; i++ )
+ //Coarse linear search first, try to figure out about where
+ for( i = 0; i < 33*2; i++ )
{
xyzyprchange[axis] = fv;
quatfromeuler( LighthouseQuat, &xyzyprchange[3] );
memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) );
- float ft = RunTest(0);
+ FLT ft = RunTest(0);
+ //printf( " %d / %f = %f\n", ft, fv );
if( ft < minv ) { minv = ft; bv = fv; }
fv += 0.1;
}
xyzyprchange[axis] = bv;
-
- //Now, do a more precise binary search.
- float jumpamount = 0.1;
- for( i = 0; i < 30; i++ )
+#if 1
+ //Now, do a more precise binary-ish search.
+ FLT jumpamount = 0.15;
+ for( i = 0; i < 20; i++ )
{
- float orig = xyzyprchange[axis];
+ FLT orig = xyzyprchange[axis];
minv = 1e20;
+ //When searching, consider 'less' 'this' and 'more'
+
fv = xyzyprchange[axis] = orig;
quatfromeuler( LighthouseQuat, &xyzyprchange[3] );
memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) );
- float ft = RunTest(0);
+ FLT ft = RunTest(0);
if( ft < minv ) { minv = ft; bv = fv; }
fv = xyzyprchange[axis] = orig + jumpamount;
@@ -142,57 +191,61 @@ float RunOpti( int axis )
xyzyprchange[axis] = bv;
- jumpamount *= 0.5;
+ //Tighten up search area. Doing by 0.5 can cause unusual instabilities.
+ jumpamount *= 0.6;
}
-
-
-
+#endif
xyzypr[axis] = bv;
return minv;
}
-float RunTest( int print )
+FLT RunTest( int print )
{
int k;
- float totprob = 0.0;
+ FLT totprob = 0.0;
int ict = 0;
for( k = 0; k < 64; k++ )
{
if( hmd_point_counts[k] == 0 ) continue;
int axis = k%2;
int pt = k/2;
- float angle = (hmd_point_angles[k] - 200000) / 200000 * 3.1415926535/2; //XXX XXX WRONG??? OR SOMETHING??? WHY DIV2 MAKE GOOD?
- if( axis == 1) angle = -angle; //Flip coordinate systems
- float thiseuler[3] = { 0, 0, 0 };
- thiseuler[axis] = angle;
-
+ FLT angle = hmd_point_angles[k];
if( print ) printf( "%d %d : angle: %f / ", axis, pt, angle );
- //axis = 0: x changes. +y is rotated plane normal.
- //axis = 1: y changes. +x is rotated plane normal.
- float planequat[4];
- quatfromeuler( planequat, thiseuler );
- quatrotateabout( planequat, LighthouseQuat, planequat ); //Order of rotations may be wrong.
- float plane_normal[3] = { 0, 0, 0 };
- plane_normal[!axis] = 1;
- quatrotatevector( plane_normal, planequat, plane_normal );
-
- //plane_normal is our normal / LighthousePos is our point.
- float w0[] = { hmd_points[pt*3+0], hmd_points[pt*3+1], hmd_points[pt*3+2] };
- //float w0[] = { 0, 0, 0 };
- float d = -(plane_normal[0] * LighthousePos[0] + plane_normal[1] * LighthousePos[1] + plane_normal[2] * LighthousePos[2]);
- float D = plane_normal[0] * w0[0] + plane_normal[1] * w0[1] + plane_normal[2] * w0[2] + d;
- //Point line distance assuming ||normal|| = 1.
- if( print ) printf( " %f %f -\n", d, D );
- totprob += (D*D);
- ict++;
- if( print )
+ //XXX TODO: This is critical. We need to properly define the planes.
+ FLT plane_normal[3] = { 0, 0, 0 };
+ if( axis == 0 )
{
- //printf( "%d %d ", pt, axis, hmd_points[ ); ///..x.x.x XXX TODO check line intersection and planequat thing
+ plane_normal[1] = cos(angle);
+ plane_normal[2] =-sin(angle);
}
+ else
+ {
+ plane_normal[0] =-cos(angle);
+ plane_normal[2] =-sin(angle);
+ }
+
+ quatrotatevector( plane_normal, LighthouseQuat, plane_normal ); //Rotate plane normal by concatenated rotation.
- //printf( " : %f %f %f %f %f %f\n", plane_normal[0], plane_normal[1], plane_normal[2], 1.0, angle, hmd_point_angles[k] );
+ //plane_normal is our normal / LighthousePos is our point.
+ FLT w0[] = { hmd_points[pt*3+0], hmd_points[pt*3+1], hmd_points[pt*3+2] };
+
+//May be able to remove this.
+// FLT w0[] = { hmd_points[pt*3+0], hmd_points[pt*3+2],-hmd_points[pt*3+1] }; //XXX WRONG Why does this produce the right answers?
+
+ //FLT w0[] = { 0, 0, 0 };
+ FLT d = -(plane_normal[0] * LighthousePos[0] + plane_normal[1] * LighthousePos[1] + plane_normal[2] * LighthousePos[2]);
+ FLT D = plane_normal[0] * w0[0] + plane_normal[1] * w0[1] + plane_normal[2] * w0[2] + d;
+ //Point line distance assuming ||normal|| = 1.
+
+ FLT delta[] = { LighthousePos[0]-hmd_points[pt*3+0],LighthousePos[1]-hmd_points[pt*3+1],LighthousePos[2]-hmd_points[pt*3+2] };
+ FLT dot = delta[0] * hmd_norms[pt*3+0] + delta[1] * hmd_norms[pt*3+1] + delta[2] * hmd_norms[pt*3+2];
+// if( dot < -0.04 ) totprob+=10000000000;
+ if( print ) printf( " %f %f N:%f\n", d, D, dot );
+ totprob += (D*D); //Calculate RMS distance of incorrectitude.
+
+ ict++;
}
return sqrt(totprob/ict);
}
@@ -229,6 +282,37 @@ int LoadData( char Camera )
printf( "Loaded %d points\n", pt );
+ f = fopen( "hmd_normals.csv", "r" );
+ int nrm = 0;
+ if( !f ) { fprintf( stderr, "error: can't open hmd points.\n" ); return -5; }
+ while(!feof(f) && !ferror(f) && nrm < PTS)
+ {
+ float fa, fb, fc;
+ int r = fscanf( f,"%g,%g,%g\n", &fa, &fb, &fc );
+ hmd_norms[nrm*3+0] = fa;
+ hmd_norms[nrm*3+1] = fb;
+ hmd_norms[nrm*3+2] = fc;
+ nrm++;
+ if( r != 3 )
+ {
+ fprintf( stderr, "Not enough entries on line %d\n", nrm );
+ return -8;
+ }
+ }
+ if( nrm < PTS )
+ {
+ fprintf( stderr, "Not enough points.\n" );
+ return -9;
+ }
+ if( nrm != pt )
+ {
+ fprintf( stderr, "point/normal counts disagree.\n" );
+ return -9;
+ }
+ fclose( f );
+ printf( "Loaded %d norms\n", nrm );
+
+
int xck = 0;
f = fopen( "third_test_with_time_lengths.csv", "r" );
if( !f ) { fprintf( stderr, "Error: can't open two lighthouses test data.\n" ); return -11; }
@@ -290,6 +374,12 @@ int LoadData( char Camera )
hmd_point_angles[i*2+0]/=hmd_point_counts[i*2+0];
hmd_point_angles[i*2+1]/=hmd_point_counts[i*2+1];
+ hmd_point_angles[i*2+0] = (hmd_point_angles[i*2+0] - 200000) / 200000 * 3.1415926535/2; // Is it really 800,000 ticks per revolution?
+ hmd_point_angles[i*2+1] = (hmd_point_angles[i*2+1] - 200000) / 200000 * 3.1415926535/2; // Is it really 800,000 ticks per revolution?
}
+
+
+
+
return 0;
}