From b754712675b5d644ae950006afe0d1ceba0e1899 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 12 Mar 2017 03:29:24 -0400 Subject: update with another test. --- redist/linmath.c | 24 ++++++++++++++++++++++++ redist/linmath.h | 2 +- redist/lintest.c | 48 ++++++++++++++++++++++++++++++++++++++++++++++-- src/poser_daveortho.c | 6 +++--- src/survive_cal.c | 41 ++++++++++++++++++++++------------------- src/survive_cal.h | 4 +++- 6 files changed, 99 insertions(+), 26 deletions(-) diff --git a/redist/linmath.c b/redist/linmath.c index 37d8e7a..41fa18f 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -516,3 +516,27 @@ void matrix44copy(FLT * mout, const FLT * minm ) memcpy( mout, minm, sizeof( FLT ) * 16 ); } +void matrix44transpose(FLT * mout, const FLT * minm ) +{ + mout[0] = minm[0]; + mout[1] = minm[4]; + mout[2] = minm[8]; + mout[3] = minm[12]; + + mout[4] = minm[1]; + mout[5] = minm[5]; + mout[6] = minm[9]; + mout[7] = minm[13]; + + mout[8] = minm[2]; + mout[9] = minm[6]; + mout[10] = minm[10]; + mout[11] = minm[14]; + + mout[12] = minm[3]; + mout[13] = minm[7]; + mout[14] = minm[11]; + mout[15] = minm[15]; + +} + diff --git a/redist/linmath.h b/redist/linmath.h index aeaca46..66a38ed 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -110,7 +110,7 @@ Matrix3x3 inverseM33(const Matrix3x3 mat); void matrix44copy(FLT * mout, const FLT * minm ); - +void matrix44transpose(FLT * mout, const FLT * minm ); #endif diff --git a/redist/lintest.c b/redist/lintest.c index a767670..933d2e5 100644 --- a/redist/lintest.c +++ b/redist/lintest.c @@ -3,7 +3,51 @@ int main() { - + +#ifdef NONTRANSPOSED_DAVE + FLT pLH1[3] = {-0.275796, 3.105430, -0.868643}; + FLT qLH1[4] = {0.803743, -0.347009, 0.475276, 0.087718}; + FLT pNLH1[3] = { 0.122180, 2.772487, -1.511608 }; //1M +x + FLT qNLH1[4] = { 0.796702, -0.389329, 0.457024, -0.069414 }; + + FLT pLH2[3] = {0.120546, 3.252532, -0.098940}; + FLT qLH2[4] = {-0.220026, 0.460836, 0.328534, 0.794534 }; + FLT pNLH2[3] = {-0.183505, 3.356293, 0.695688, }; + FLT qNLH2[4] = {-0.237438, 0.405213, 0.270438, 0.840410 }; +#else + + FLT pLH1[3] = {-0.321299, 3.130532, -0.786460}; + FLT qLH1[4] = {0.794180, 0.336117, -0.485668, -0.142934}; + FLT pNLH1[3] = { 0.113572, 2.791495, -1.495652 }; //1M +x + FLT qNLH1[4] = { 0.807419, 0.372818, -0.451339, 0.073308 }; + + FLT pLH2[3] = {0.153580, 3.251673, -0.190491}; + FLT qLH2[4] = {0.217017, 0.482214, 0.306568, 0.791448 }; + FLT pNLH2[3] = {-0.175330, 3.351943, 0.669623 }; + FLT qNLH2[4] = {0.257241, 0.394159, 0.292555, 0.832392 }; +#endif + + FLT pOut1[3]; + FLT pOut2[3]; + +// qLH1[1]*=-1; +// qLH2[0]*=-1; + + sub3d( pOut1, pLH1, pNLH1 ); + sub3d( pOut2, pLH2, pNLH2 ); + + printf( "%f %f %f\n", PFTHREE( pOut1 ) ); + printf( "%f %f %f\n", PFTHREE( pOut2 ) ); + + quatrotatevector( pOut1, qLH1, pOut1 ); + quatrotatevector( pOut2, qLH2, pOut2 ); + + printf( "%f %f %f\n", PFTHREE( pOut1 ) ); + printf( "%f %f %f\n", PFTHREE( pOut2 ) ); + + return -1; + +/* FLT e[3] = { 1,1,3.14 }; FLT q[4]; FLT m[16]; @@ -39,7 +83,7 @@ int main() quatrotatevector( p, q, p ); printf( "%f %f %f\n", PFTHREE( p ) ); - +*/ //Try setting up a pose. // FLT mypose[7] = { 0, 0, 10, q[0], q[1], q[2], q[3] ); // ApplyPoseToPoint( FLT * pout, const FLT * pin, const FLT * pose ); diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index 80ddd90..beba7ad 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -74,13 +74,13 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) 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 + //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] ); + 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) ); diff --git a/src/survive_cal.c b/src/survive_cal.c index fd2f806..59dd919 100644 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -307,7 +307,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; @@ -512,27 +512,34 @@ static void handle_calibration( struct SurviveCalData *cd ) lhp->Pos[1] = objfromlh->Pos[1]; lhp->Pos[2] = objfromlh->Pos[2]; - lhp->Rot[0] = objfromlh->Rot[0]*-1; + lhp->Rot[0] = objfromlh->Rot[0]; 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 ); + //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.204066 3.238746 -0.856369 + 0.812203 -0.264897 0.505599 0.120520 + -0.204066 3.238746 -0.856369 - 0.695987 3.346798 -0.776169 - -0.258207 0.966008 -0.008604 0.009116 - 0.614156 -2.521008 2.356952 + 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" ); @@ -551,21 +558,17 @@ static void handle_calibration( struct SurviveCalData *cd ) 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]; + FLT rot[4] = { + lhp->Rot[0], + lhp->Rot[1], + lhp->Rot[2], + lhp->Rot[3] }; - quatrotatevector( lhp->Pos, objfromlh->Rot, lhp->Pos ); + quatrotatevector( pos, rot, pos ); fprintf( stderr, "====> %f %f %f ", pos[0], pos[1], pos[2] ); diff --git a/src/survive_cal.h b/src/survive_cal.h index d21ba36..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 128 + +#define MIN_PTS_BEFORE_CAL 24 +#define DRPTS 32 #define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2) struct SurviveCalData { -- cgit v1.2.3