From 6b6ee8c04aaa13a188cae4ef86106a6cdcd54571 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Tue, 13 Dec 2016 23:39:09 -0500 Subject: Closing in. Average off by 3mm... BUT something wrong with targeting math. --- tools/planetest/camfind.c | 75 +++++++++++++++++++++++++++++++++-------------- 1 file changed, 53 insertions(+), 22 deletions(-) diff --git a/tools/planetest/camfind.c b/tools/planetest/camfind.c index c1fba0e..0e199b1 100644 --- a/tools/planetest/camfind.c +++ b/tools/planetest/camfind.c @@ -2,6 +2,7 @@ #include #include "linmath.h" #include +#include #define PTS 32 #define MAX_CHECKS 20000 @@ -12,27 +13,28 @@ int hmd_point_counts[PTS*2]; int LoadData( char Camera ); //Values used for RunTest() -float LighthousePos[3] = { 0, 0,0 }; +float LighthousePos[3] = { 0, 0, 0 }; float LighthouseQuat[4] = { 1, 0, 0, 0 }; //Actual values -float xyzypr[6] = { 0, 0, 1, 0, 0, 0 }; +float xyzypr[6] = { 0, 0, 0, 0, 0, 0 }; float RunOpti( int axis ); -float RunTest(); +float RunTest( int print ); +void PrintOpti(); int main() { int i; - if( LoadData( 'L' ) ) return 5; + if( LoadData( 'R' ) ) return 5; int opti = 0; int cycle = 0; int axis = 0; - float dx; + float dx, dy, dz; float bestxyzypr[6]; memcpy( bestxyzypr, xyzypr, sizeof( xyzypr ) ); @@ -40,30 +42,52 @@ int main() float fullrange = 5; for( cycle = 0; cycle < 10; cycle ++ ) { - for( axis = 0; axis < 3; axis++ ) - { +// for( axis = 0; axis < 3; axis++ ) +// { float bestxyzyprrunning[6]; float beste = 1e20; - for( dx = -fullrange; dx < fullrange; dx += fullrange/100 ) + for( dz = -fullrange; dz < fullrange; dz += fullrange/5 ) + { + for( dy = -fullrange; dy < fullrange; dy += fullrange/5 ) + { + for( dx = -fullrange; dx < fullrange; dx += fullrange/5 ) { memcpy( xyzypr, bestxyzypr, sizeof( xyzypr ) ); - xyzypr[axis] += dx; + xyzypr[0] += dx; + xyzypr[1] += dy; + xyzypr[2] += dz; xyzypr[3] = 0; xyzypr[4] = 0; xyzypr[5] = 0; float ft; + int reopt = 0; + for( reopt = 0; reopt < 10; reopt++ ) for( opti = 3; opti < 6; opti++ ) { ft = RunOpti( opti ); } if( ft < beste ) { beste = ft; memcpy( bestxyzyprrunning, xyzypr, sizeof( xyzypr ) ); } - } - printf( "%f %f %f %f %f %f = %f\n", xyzypr[0], xyzypr[1], xyzypr[2], xyzypr[3], xyzypr[4], xyzypr[5], beste ); + //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.2; + fullrange *= 0.3; } + + //Use bestxyzypr + memcpy( xyzypr, bestxyzypr, sizeof( xyzypr ) ); + PrintOpti(); +} + +void PrintOpti() +{ + float xyzyprchange[6]; + memcpy( xyzyprchange, xyzypr, sizeof( xyzypr ) ); + quatfromeuler( LighthouseQuat, &xyzyprchange[3] ); + memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) ); + float ft = RunTest(1); } @@ -76,13 +100,14 @@ float RunOpti( int axis ) float fv = -1.3; float bv = 0; + //Coarse linear search for( i = 0; i < 26; i++ ) { xyzyprchange[axis] = fv; quatfromeuler( LighthouseQuat, &xyzyprchange[3] ); memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) ); - float ft = RunTest(); + float ft = RunTest(0); if( ft < minv ) { minv = ft; bv = fv; } fv += 0.1; } @@ -100,19 +125,19 @@ float RunOpti( int axis ) fv = xyzyprchange[axis] = orig; quatfromeuler( LighthouseQuat, &xyzyprchange[3] ); memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) ); - float ft = RunTest(); + float ft = RunTest(0); if( ft < minv ) { minv = ft; bv = fv; } fv = xyzyprchange[axis] = orig + jumpamount; quatfromeuler( LighthouseQuat, &xyzyprchange[3] ); memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) ); - ft = RunTest(); + ft = RunTest(0); if( ft < minv ) { minv = ft; bv = fv; } fv = xyzyprchange[axis] = orig - jumpamount; quatfromeuler( LighthouseQuat, &xyzyprchange[3] ); memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) ); - ft = RunTest(); + ft = RunTest(0); if( ft < minv ) { minv = ft; bv = fv; } xyzyprchange[axis] = bv; @@ -127,24 +152,25 @@ float RunOpti( int axis ) } -float RunTest() +float RunTest( int print ) { int k; float 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; - if( axis == 0 ) angle = -angle; + if( axis == 0) angle = -angle; //Flip coordinate systems float thiseuler[3] = { 0, 0, 0 }; thiseuler[axis] = 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, planequat, LighthouseQuat ); //Order of rotations may be wrong. + 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 ); @@ -155,11 +181,16 @@ float RunTest() 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. - totprob += D*D; + totprob += sqrt(D*D); + ict++; + if( print ) + { + //printf( "%d %d ", pt, axis, hmd_points[ ); ///..x.x.x XXX TODO check line intersection and planequat thing + } //printf( " : %f %f %f %f %f %f\n", plane_normal[0], plane_normal[1], plane_normal[2], 1.0, angle, hmd_point_angles[k] ); } - return totprob; + return totprob/ict; } -- cgit v1.2.3