From ee12999bcb71f9b4b3c48bd0c1874f7ac251f355 Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Tue, 20 Mar 2018 21:34:42 -0400
Subject: significant improvements to Orthodave.

---
 README.md                  |   2 +-
 include/libsurvive/poser.h |   1 +
 src/poser_daveortho.c      | 138 ++++++++++++++++++++++++++++++++++++---------
 src/survive_process.c      |  18 +++++-
 4 files changed, 131 insertions(+), 28 deletions(-)

diff --git a/README.md b/README.md
index 6639946..ddd2c2d 100644
--- a/README.md
+++ b/README.md
@@ -319,7 +319,7 @@ Given an HMD:
  * pointed FACE DOWN at the lighthouse
  * 1M above the lighthouse
 
-   Will produce a pose of [[0, 0, -1] [0, 0, 0, 1]* ].  NOTE: The quaternion is inverted.  This is the pose of the OBJECT in WORLD space. For our example, the lighthouse is at 0,0,0 in world space.  In practicality, the lighthouse will be at some other place in the scene.
+   Will produce a pose of [[0, 0, -1] [0, 1, 0, 0] ].  NOTE: The quaternion is inverted, so rotation axis is arbitrary, point is w is 0.  This is the pose of the OBJECT in WORLD space. For our example, the lighthouse is at 0,0,0 in world space.  In practicality, the lighthouse will be at some other place in the scene.
 
    The idea is you should be able to take a coordinate local to the HMD and then use ```ApplyPoseToPoint``` to transform that point into world space, by applying the output of the poser.
 
diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h
index 6c74c52..ccd36ce 100644
--- a/include/libsurvive/poser.h
+++ b/include/libsurvive/poser.h
@@ -15,6 +15,7 @@ typedef enum PoserType_t
 	POSERDATA_LIGHT,		//Single lighting event.  
 	POSERDATA_FULL_SCENE, 	//Full, statified X, Y sweep data for both lighthouses.
 	POSERDATA_DISASSOCIATE, //If you get this, it doesn't contain data.  It just tells you to please disassociate from the current SurviveObject and delete your poserdata.
+	POSERDATA_SYNC,			//Sync pulse.
 } PoserType;
 
 typedef void (*poser_raw_pose_func)(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose, void *user);
diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c
index 2bf57c6..0e88a4e 100644
--- a/src/poser_daveortho.c
+++ b/src/poser_daveortho.c
@@ -8,6 +8,8 @@
 #include <dclapack.h>
 #include <linmath.h>
 
+//Dave talks about this poser here: https://www.youtube.com/watch?v=nSbEltdH9vM&feature=youtu.be&t=2h29m47s
+
 static int LH_ID;
 
 void OrthoSolve(
@@ -20,7 +22,9 @@ void OrthoSolve(
 
 typedef struct
 {
-	int something;
+	int nextaxis;
+	float   lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2];
+	float   angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2];
 	//Stuff
 } DummyData;
 
@@ -30,7 +34,7 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd )
 	SurviveContext * ctx = so->ctx;
 	DummyData * dd = so->PoserData;
 
-	if( !dd ) so->PoserData = dd = malloc( sizeof( DummyData ) );
+	if( !dd ) so->PoserData = dd = calloc( sizeof( DummyData ), 1 );
 
 	switch( pt )
 	{
@@ -43,7 +47,77 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd )
 	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 );
+		dd->lengths[l->sensor_id][l->lh][dd->nextaxis] = l->length;
+		dd->angles [l->sensor_id][l->lh][dd->nextaxis] = l->angle;
+		break;
+	}
+	case POSERDATA_SYNC:
+	{
+		PoserDataLight * l = (PoserDataLight*)pd;
+		int lhid = l->lh;
+
+		dd->nextaxis = l->acode & 1;
+
+		if( dd->nextaxis == 0 && l->lh == 0 &&  so->codename[0] == 'H' )
+		{
+			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( dd->lengths[i][lhid][0] > 0 && dd->lengths[i][lhid][1] > 0  )
+				{
+					S_in[0][max_hits] = dd->angles[i][lhid][0];
+					S_in[1][max_hits] = dd->angles[i][lhid][1];
+					dd->lengths[i][lhid][0] = -1;
+					dd->lengths[i][lhid][1] = -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++;
+				}
+
+			}
+			if( 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] );
+			
+if(0)
+				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] ) );
+				}
+			}
+		}
+
+		//if( so->codename[0] == 'H' ) printf( "LIG:%s %d @ %d rad, %f s (AX %d) (TC %d)\n", so->codename, l->sensor_id, l->lh, l->length, dd->nextaxis, l->timecode );
+
 		break;
 	}
 	case POSERDATA_FULL_SCENE:
@@ -148,7 +222,7 @@ REGISTER_LINKTIME( PoserDaveOrtho );
 
 void OrthoSolve(
     FLT T[4][4],               // OUTPUT: 4x4 transformation matrix
-    FLT S_out[2][SENSORS_PER_OBJECT],  // OUTPUT:  array of screenspace points
+    FLT S_out[2][SENSORS_PER_OBJECT],  // OUTPUT:  array of screenspace points  (May not be populated!!!)
     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)
@@ -437,12 +511,26 @@ PRINT(ab,2,1);
     FLT transdist = sqrt( trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2] );
 
     //-------------------
-    // Pack into the 4x4 transformation matrix
+    // Pack into the 4x4 transformation matrix, and put into correct "world"-space. I.e. where is the object, relative to the lighthouse.
     //-------------------
+
+#if 0
+	//Don't do any transformation to the correct space.
     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;
+#else
+
+	//XXX XXX XXX FOR ANYONE LOOKING TO CONTROL THE COORDINATE FRAME, THESE ARE THE LINES!!!
+    T[0][0]=-R[0][0]; T[0][1]=-R[0][1]; T[0][2]=-R[0][2]; T[0][3]=-trans[0];
+    T[2][0]=-R[1][0]; T[2][1]=-R[1][1]; T[2][2]=-R[1][2]; T[2][3]=-trans[1];
+    T[1][0]=R[2][0]; T[1][1]=R[2][1]; T[1][2]=R[2][2]; T[1][3]=trans[2];
+    T[3][0]=0.0;     T[3][1]=0.0;     T[3][2]=0.0;     T[3][3]=1.0;
+
+#endif
+
+
 
 
 	FLT T2[4][4];
@@ -450,47 +538,44 @@ PRINT(ab,2,1);
 	//-------------------
 	// Orthogonalize the matrix
 	//-------------------
-	FLT temp[4][4];
-	FLT quat[4], quatNorm[4];
-	FLT euler[3];
+	//FLT temp[4][4];
+	//FLT quat[4], quatNorm[4];
+	//FLT euler[3];
 
 
 	//-------------------
 	// Orthogonalize the matrix
 	//-------------------
-	PRINT_MAT(T,4,4);
+	//PRINT_MAT(T,4,4);
 
 #if 1
 //	matrix44transpose(T2, T);  //Transpose so we are
 	matrix44copy((FLT*)T2,(FLT*)T);
-	cross3d( &T2[1][0], &T2[0][0], &T2[2][0] );
 	cross3d( &T2[2][0], &T2[1][0], &T2[0][0] ); //Replace axes in-place.
-	matrix44copy((FLT*)T,(FLT*)T2);
+	cross3d( &T2[1][0], &T2[0][0], &T2[2][0] );
+//	matrix44copy((FLT*)T,(FLT*)T2);
 //	matrix44transpose(T, T2);
+	normalize3d( &T[0][0], &T2[0][0] );
+	normalize3d( &T[1][0], &T2[1][0] );
+	normalize3d( &T[2][0], &T2[2][0] );
 
-#endif
 
+#else
 	normalize3d( &T[0][0], &T[0][0] );
 	normalize3d( &T[1][0], &T[1][0] );
 	normalize3d( &T[2][0], &T[2][0] );
-	//Change handedness
+#endif
 
-	T[1][0]*=-1;
-	T[1][1]*=-1;
-	T[1][2]*=-1;
+	//Fix handedness for rotations...
+	T[0][0]*=-1;
+	T[0][1]*=-1;
+	T[0][2]*=-1;
+
+//	PRINT_MAT(T,4,4);
 
-/*
-	//Check Orthogonality.  Yep. It's orthogonal.
-	FLT tmp[3];
-	cross3d( tmp, &T[0][0], &T[1][0] );
-	printf( "M3: %f\n", magnitude3d( tmp ) );
-	cross3d( tmp, &T[2][0], &T[1][0] );
-	printf( "M3: %f\n", magnitude3d( tmp ) );
-	cross3d( tmp, &T[2][0], &T[0][0] );
-	printf( "M3: %f\n", magnitude3d( tmp ) );
-*/
 
 //	PRINT_MAT(T,4,4);
+#if 0
 
 #if 1
 
@@ -554,6 +639,7 @@ PRINT(ab,2,1);
         //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]);
     }
+#endif
 
 }
 
diff --git a/src/survive_process.c b/src/survive_process.c
index b697f4a..f0928b4 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -19,7 +19,23 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode
 	}
 
 	//We don't use sync times, yet.
-	if( acode < -1 ) return;
+	if( sensor_id <= -1 )
+	{
+		if( so->PoserFn ) 
+		{
+			PoserDataLight l = {
+				.hdr = { .pt = POSERDATA_SYNC, },
+				.sensor_id = sensor_id,
+				.acode = acode,
+				.timecode = timecode,
+				.length = length,
+				.angle = 0,
+				.lh = lh,
+			};
+			so->PoserFn( so, (PoserData *)&l );
+		}
+		return;
+	}
 
 	if( base_station > NUM_LIGHTHOUSES ) return;
 
-- 
cgit v1.2.3


From 9378980248f137368cbf80d1dec8af7eda209655 Mon Sep 17 00:00:00 2001
From: jdavidberger <j.david.berger@gmail.com>
Date: Tue, 20 Mar 2018 22:50:16 -0600
Subject: Update README.md

Added EPNP and SBA posers information
---
 README.md | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/README.md b/README.md
index ddd2c2d..b329730 100644
--- a/README.md
+++ b/README.md
@@ -117,6 +117,8 @@ Poser | [poser_daveortho.c](src/poser_daveortho.c) | A very fast system using or
 Poser | [poser_dummy.c](src/poser_dummy.c) | Template for posers | [@cnlohr](https://github.com/cnlohr)
 Poser | [poser_octavioradii.c](src/poser_octavioradii.c) | A potentially very fast poser that works by finding the best fit of the distances from the lighthouse to each sensor that matches the known distances between sensors, given the known angles of a lighthouse sweep.  Incomplete- distances appear to be found correctly, but more work needed to turn this into a pose. | [@mwturvey](https://github.com/mwturvey) and [@octavio2895](https://github.com/octavio2895)
 Poser | [poser_turveytori.c](src/poser_turveytori.c) | A moderately fast, fairly high precision poser that works by determine the angle at the lighthouse between many sets of two sensors.  Using the inscirbed angle theorom, each set defines a torus of possible locations of the lighthouse.  Multiple sets define multiple tori, and this poser finds most likely location of the lighthouse using least-squares distance.   Best suited for calibration, but is can be used for real-time tracking on a powerful system.  | [@mwturvey](https://github.com/mwturvey)
+Poser | [poser_epnp.c](src/poser_epnp.c) | Reasonably fast and accurate calibration and tracker that uses the [EPNP algorithm](https://en.wikipedia.org/wiki/Perspective-n-Point#EPnP) to solve the perspective and points problem. Suitable for fast tracking, but does best with >5-6 sensor readings. | [@jdavidberger](https://github.com/jdavidberger)
+Poser | [poser_sba.c](src/poser_sba.c) | Reasonably fast and accurate calibration and tracker but is dependent on a 'seed' poser to give it an initial estimate. This then performs [bundle adjustment](https://en.wikipedia.org/wiki/Bundle_adjustment) to minimize reprojection error given both ligthhouse readings. This has the benefit of greatly increasing accuracy by incorporating all the light data that is available. Set 'SBASeedPoser' config option to specify the seed poser; default is EPNP. | [@jdavidberger](https://github.com/jdavidberger)
 Disambiguator | [survive_data.c](src/survive_data.c) (currently #ifdefed out) | The old disambiguator - very fast, but slightly buggy. | [@cnlohr](https://github.com/cnlohr)
 Disambiguator | [survive_data.c](src/survive_data.c) (current disambiguator) | More complicated but much more robust disambiguator | [@mwturvey](https://github.com/mwturvey)
 Dismabiguator | superceded disambiguator | A more sophisticated disambiguator, development abandoned.  Removed from tree. |  [@jpicht](https://github.com/jpicht)
-- 
cgit v1.2.3


From 96e55cd00b3c68ac8adae8a54aaca2f57e4329c0 Mon Sep 17 00:00:00 2001
From: jdavidberger <j.david.berger@gmail.com>
Date: Tue, 20 Mar 2018 22:52:43 -0600
Subject: Update README.md

Made playback / record instructions match new argument functionality
---
 README.md | 12 ++++++++++--
 1 file changed, 10 insertions(+), 2 deletions(-)

diff --git a/README.md b/README.md
index b329730..518a985 100644
--- a/README.md
+++ b/README.md
@@ -274,12 +274,20 @@ libsurvive has an integrated tool that allows you to record and playback streams
 
 ```
 make
-./data_recorder my_playback_file
+./data_recorder -o my_playback_file
 ```
 
 This gives you a file -- my_playback_file -- with all the device configurations and events file you need to replay it.
 
-To actually replay it, put that directory path in the 'PlaybackFile' configuration value in config.json and run libsurvive as usual. Note that this will purposefully stop the USB devices from loading as to not confuse the library with inconsistent data.
+You can also just let it stream to standard output, but this tends to be a lot of information. 
+
+To actually replay it, put that directory path in the 'playbackfile' configuration value in config.json and run libsurvive as usual. Note that this will purposefully stop the USB devices from loading as to not confuse the library with inconsistent data.
+
+You can also replay it just with command line options:
+
+```
+./calibrate --playbackfile my_playback_file
+```
 
 ## Playback speed
 
-- 
cgit v1.2.3


From 7f4ac3104156a25b39495af0e3146d3542283d53 Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Wed, 21 Mar 2018 01:53:22 -0400
Subject: Update dave's ortho and survive_cal to allow calling of one item even
 if multiple are connected.  Something may still be flipped.

---
 src/poser_daveortho.c | 177 ++++++++++++++++++++++++++++++++------------------
 src/survive_cal.c     |  36 +++++-----
 2 files changed, 132 insertions(+), 81 deletions(-)

diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c
index 0e88a4e..e2e8d2e 100644
--- a/src/poser_daveortho.c
+++ b/src/poser_daveortho.c
@@ -10,7 +10,6 @@
 
 //Dave talks about this poser here: https://www.youtube.com/watch?v=nSbEltdH9vM&feature=youtu.be&t=2h29m47s
 
-static int LH_ID;
 
 void OrthoSolve(
     FLT T[4][4],               // OUTPUT: 4x4 transformation matrix
@@ -47,8 +46,11 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd )
 	case POSERDATA_LIGHT:
 	{
 		PoserDataLight * l = (PoserDataLight*)pd;
-		dd->lengths[l->sensor_id][l->lh][dd->nextaxis] = l->length;
-		dd->angles [l->sensor_id][l->lh][dd->nextaxis] = l->angle;
+		if( l->length > dd->lengths[l->sensor_id][l->lh][dd->nextaxis] )
+		{
+			dd->lengths[l->sensor_id][l->lh][dd->nextaxis] = l->length;
+			dd->angles [l->sensor_id][l->lh][dd->nextaxis] = l->angle;
+		}
 		break;
 	}
 	case POSERDATA_SYNC:
@@ -58,7 +60,7 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd )
 
 		dd->nextaxis = l->acode & 1;
 
-		if( dd->nextaxis == 0 && l->lh == 0 &&  so->codename[0] == 'H' )
+		if( dd->nextaxis == 0 &&  so->codename[0] == 'H' )
 		{
 			int i;
 			int max_hits = 0;
@@ -80,40 +82,41 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd )
 				}
 
 			}
-			if( max_hits )
+			if( max_hits > 2 )
 			{
+				//if( lhid == 0 ) { printf( "\033[H\033[2J" ); }
+				//printf( "%d\n", 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] };
+
+				SurvivePose objpose; 
 				FLT MT[4][4];
 
+				objpose.Pos[0] = tOut[0][3];
+				objpose.Pos[1] = tOut[1][3];
+				objpose.Pos[2] = tOut[2][3];
+
 				//matrix44transpose( MT, &tOut[0][0] );
 				matrix44copy( &MT[0][0], &tOut[0][0] );
 
-				quatfrommatrix( quat, &MT[0][0] );
-
+				quatfrommatrix( objpose.Rot, &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] );
-			
-if(0)
-				for( i = 0; i < max_hits;i++ )
+				quatnormalize( objpose.Rot, objpose.Rot );
+
+				SurvivePose poseout;
+				InvertPose(poseout.Pos, objpose.Pos);
+				if( 0 )
 				{
-					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] ) );
+					printf( "INQUAT: %f %f %f %f = %f [%f %f %f]\n", objpose.Rot[0], objpose.Rot[1], objpose.Rot[2], objpose.Rot[3], quatmagnitude(objpose.Rot), objpose.Pos[0], objpose.Pos[1], objpose.Pos[2] );
+					printf( "OUQUAT: %f %f %f %f = %f [%f %f %f]\n", poseout.Rot[0], poseout.Rot[1], poseout.Rot[2], poseout.Rot[3], quatmagnitude(poseout.Rot), poseout.Pos[0], poseout.Pos[1], poseout.Pos[2] );
 				}
-			}
+			}	
 		}
 
 		//if( so->codename[0] == 'H' ) printf( "LIG:%s %d @ %d rad, %f s (AX %d) (TC %d)\n", so->codename, l->sensor_id, l->lh, l->length, dd->nextaxis, l->timecode );
@@ -123,6 +126,8 @@ if(0)
 	case POSERDATA_FULL_SCENE:
 	{
 		PoserDataFullScene * fs = (PoserDataFullScene*)pd;
+		int LH_ID;
+		printf( "PDFS\n" );
 
 		for( LH_ID = 0; LH_ID < 2; LH_ID++ )
 		{
@@ -150,38 +155,53 @@ if(0)
 
 						//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 quat[4];
+			//FLT posoff[3] = { tOut[0][3], tOut[1][3], tOut[2][3] };
+			SurvivePose objpose; 
 			FLT MT[4][4];
 
+			objpose.Pos[0] = tOut[0][3];
+			objpose.Pos[1] = tOut[1][3];
+			objpose.Pos[2] = tOut[2][3];
+
 			//matrix44transpose( MT, &tOut[0][0] );
 			matrix44copy( &MT[0][0], &tOut[0][0] );
 
-			quatfrommatrix( quat, &MT[0][0] );
-
+			quatfrommatrix( objpose.Rot, &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] );
+			quatnormalize( objpose.Rot, objpose.Rot );
+			printf( "QUAT: %f %f %f %f = %f [%f %f %f]\n", objpose.Rot[0], objpose.Rot[1], objpose.Rot[2], objpose.Rot[3], quatmagnitude(objpose.Rot), objpose.Pos[0], objpose.Pos[1], objpose.Pos[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] ) );
-			}
+			if( 0 );
+				for( i = 0; i < max_hits;i++ )
+				{
+					FLT pt[3] = { X_in[0][i], X_in[1][i], X_in[2][i] };
+					quatrotatevector( pt, objpose.Rot, pt );
+					add3d( pt, pt, objpose.Pos );
+					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] = objpose.Pos[0];
+				so->FromLHPose[LH_ID].Pos[1] = objpose.Pos[1];
+				so->FromLHPose[LH_ID].Pos[2] = objpose.Pos[2];
+				so->FromLHPose[LH_ID].Rot[0] = objpose.Rot[0];
+				so->FromLHPose[LH_ID].Rot[1] = objpose.Rot[1];
+				so->FromLHPose[LH_ID].Rot[2] = objpose.Rot[2];
+				so->FromLHPose[LH_ID].Rot[3] = objpose.Rot[3];
+			*/
+
+
+			SurvivePose poseout;
+			InvertPose(poseout.Pos, objpose.Pos);
+			printf( "INQUAT: %f %f %f %f = %f [%f %f %f]\n", objpose.Rot[0], objpose.Rot[1], objpose.Rot[2], objpose.Rot[3], quatmagnitude(objpose.Rot), objpose.Pos[0], objpose.Pos[1], objpose.Pos[2] );
 
-			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];
+			PoserData_lighthouse_pose_func(&fs->hdr, so, LH_ID, &poseout);
+			printf( "OUQUAT: %f %f %f %f = %f [%f %f %f]\n", poseout.Rot[0], poseout.Rot[1], poseout.Rot[2], poseout.Rot[3], quatmagnitude(poseout.Rot), poseout.Pos[0], poseout.Pos[1], poseout.Pos[2] );
 		}
 
 		break;
@@ -340,8 +360,8 @@ 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("ydist %f\n", ydist);
+    FLT ydist  = 1.0 / urhat_len;		//TRICKY XXX Dave operates with "y forward" for some reason...
+//    printf("ydist %f\n", ydist);
 //    printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist);
 
     //--------------------
@@ -382,8 +402,8 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
 //                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; }
+                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 == 0 && 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;
@@ -522,10 +542,27 @@ PRINT(ab,2,1);
     T[3][0]=0.0;     T[3][1]=0.0;     T[3][2]=0.0;     T[3][3]=1.0;
 #else
 
+
+/* WRONG: This transposes.
+    T[0][0]=-R[0][0]; T[0][1]=-R[0][1]; T[0][2]=-R[0][2]; T[0][3]=-trans[0];
+    T[2][0]= R[1][0]; T[2][1]= R[1][1]; T[2][2]= R[1][2]; T[2][3]=-trans[1];
+    T[1][0]= R[2][0]; T[1][1]= R[2][1]; T[1][2]= R[2][2]; T[1][3]=trans[2];
+    T[3][0]=0.0;     T[3][1]=0.0;     T[3][2]=0.0;     T[3][3]=1.0;
+*/
+
+
 	//XXX XXX XXX FOR ANYONE LOOKING TO CONTROL THE COORDINATE FRAME, THESE ARE THE LINES!!!
+
+
+/** This is probably all wonky and backwards  but "looks" right except with multiple transforms, perhaps it's transposed.
+    T[0][0]=-R[0][0]; T[0][1]= R[0][1]; T[0][2]= R[0][2]; T[0][3]=-trans[0];
+    T[2][0]=-R[1][0]; T[2][1]=-R[1][1]; T[2][2]= R[1][2]; T[2][3]=-trans[1];//This Z axis has problems with getting magnitudes. We can re=make it from the other two axes.
+    T[1][0]=-R[2][0]; T[1][1]= R[2][1]; T[1][2]=-R[2][2]; T[1][3]= trans[2];
+    T[3][0]=0.0;     T[3][1]=0.0;     T[3][2]=0.0;     T[3][3]=1.0;
+*/
     T[0][0]=-R[0][0]; T[0][1]=-R[0][1]; T[0][2]=-R[0][2]; T[0][3]=-trans[0];
-    T[2][0]=-R[1][0]; T[2][1]=-R[1][1]; T[2][2]=-R[1][2]; T[2][3]=-trans[1];
-    T[1][0]=R[2][0]; T[1][1]=R[2][1]; T[1][2]=R[2][2]; T[1][3]=trans[2];
+    T[1][0]= R[2][0]; T[1][1]= R[2][1]; T[1][2]= R[2][2]; T[2][3]=-trans[1];
+    T[2][0]= R[1][0]; T[2][1]= R[1][1]; T[2][2]= R[1][2]; T[1][3]= trans[2];
     T[3][0]=0.0;     T[3][1]=0.0;     T[3][2]=0.0;     T[3][3]=1.0;
 
 #endif
@@ -533,8 +570,6 @@ PRINT(ab,2,1);
 
 
 
-	FLT T2[4][4];
-
 	//-------------------
 	// Orthogonalize the matrix
 	//-------------------
@@ -546,33 +581,47 @@ PRINT(ab,2,1);
 	//-------------------
 	// Orthogonalize the matrix
 	//-------------------
-	//PRINT_MAT(T,4,4);
 
-#if 1
+	cross3d( &T[2][0], &T[0][0], &T[1][0] ); 	//Generate Z from X/Y because Z is kinda rekt.
+
+	//cross3d( &T[1][0], &T[2][0], &T[0][0] );	//Renormalize rotations...
+	//cross3d( &T[0][0], &T[1][0], &T[2][0] );	//Renormalize rotations...
+
+
+	//XXX XXX TODO
+	//We could further normalize things...
+
+
+
+#if 0
 //	matrix44transpose(T2, T);  //Transpose so we are
 	matrix44copy((FLT*)T2,(FLT*)T);
-	cross3d( &T2[2][0], &T2[1][0], &T2[0][0] ); //Replace axes in-place.
-	cross3d( &T2[1][0], &T2[0][0], &T2[2][0] );
+	cross3d( &T2[1][0], &T2[0][0], &T2[2][0] ); //Replace axes in-place.
+	cross3d( &T2[2][0], &T2[1][0], &T2[0][0] ); 
 //	matrix44copy((FLT*)T,(FLT*)T2);
 //	matrix44transpose(T, T2);
 	normalize3d( &T[0][0], &T2[0][0] );
 	normalize3d( &T[1][0], &T2[1][0] );
 	normalize3d( &T[2][0], &T2[2][0] );
-
-
 #else
-	normalize3d( &T[0][0], &T[0][0] );
-	normalize3d( &T[1][0], &T[1][0] );
-	normalize3d( &T[2][0], &T[2][0] );
+//	normalize3d( &T[0][0], &T[0][0] );
+//	normalize3d( &T[1][0], &T[1][0] );
+//	normalize3d( &T[2][0], &T[2][0] );
 #endif
 
-	//Fix handedness for rotations...
-	T[0][0]*=-1;
-	T[0][1]*=-1;
-	T[0][2]*=-1;
+//	printf( "      In Axis on headset      \n" );
+//	printf( "    x                  y                   z\n" );
+//	PRINT_MAT(T,4,4);
+
+//	PRINT_MAT(T,4,4);
 
 //	PRINT_MAT(T,4,4);
 
+	//Fix handedness for rotations...
+//	T[1][0]*=-1;
+//	T[1][1]*=-1;
+//	T[1][2]*=-1;
+
 
 //	PRINT_MAT(T,4,4);
 #if 0
diff --git a/src/survive_cal.c b/src/survive_cal.c
index cb242ae..2d97317 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -80,7 +80,7 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 	lighthouses_completed++;
 
 	if (lighthouses_completed >= NUM_LIGHTHOUSES) {
-		config_save(ctx, "config.json");
+			config_save(ctx, survive_configs( ctx, "configfile", SC_GET, "config.json" ) );
 	}
 }
 
@@ -139,8 +139,8 @@ void survive_cal_install( struct SurviveContext * ctx )
 	// setting the required trackers for calibration to be permissive to make it easier for a newbie to start-- 
 	// basically, libsurvive will detect whatever they have plugged in and start using that.  
 //	const char * RequiredTrackersForCal = config_read_str(ctx->global_config_values, "RequiredTrackersForCal", "HMD,WM0,WM1");	
-	const char * RequiredTrackersForCal = config_read_str(ctx->global_config_values, "RequiredTrackersForCal", ""); 
-	const uint32_t AllowAllTrackersForCal = config_read_uint32( ctx->global_config_values, "AllowAllTrackersForCal", 1 );
+	const char * RequiredTrackersForCal = survive_configs( ctx, "requiredtrackersforcal", SC_SETCONFIG, "" );
+	const uint32_t AllowAllTrackersForCal = survive_configi( ctx, "allowalltrackersforcal", SC_SETCONFIG, 1 );
 	size_t requiredTrackersFound = 0;
 
 	for (int j=0; j < ctx->objs_ct; j++)
@@ -334,22 +334,24 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
 			int i, j, k;
 			cd->found_common = 1;
 			for( i = 0; i < cd->numPoseObjects; i++ )
-			//for( i = 0; i < MAX_SENSORS_TO_CAL/SENSORS_PER_OBJECT; i++ )
-			for( j = 0; j < ctx->activeLighthouses; j++ )
 			{
-				int sensors_visible = 0;
-				for( k = 0; k < SENSORS_PER_OBJECT; k++ )
+				//for( i = 0; i < MAX_SENSORS_TO_CAL/SENSORS_PER_OBJECT; i++ )
+				for( j = 0; j < ctx->activeLighthouses; j++ )
 				{
-					if( cd->all_counts[k+i*SENSORS_PER_OBJECT][j][0] > NEEDED_COMMON_POINTS && 
-						cd->all_counts[k+i*SENSORS_PER_OBJECT][j][1] > NEEDED_COMMON_POINTS )
-						sensors_visible++;
-				}
-				if( sensors_visible < MIN_SENSORS_VISIBLE_PER_LH_FOR_CAL ) 
-				{
-					//printf( "Dev %d, LH %d not enough visible points found.\n", i, j );
-					reset_calibration( cd );
-					cd->found_common = 0;
-					return;
+					int sensors_visible = 0;
+					for( k = 0; k < SENSORS_PER_OBJECT; k++ )
+					{
+						if( cd->all_counts[k+i*SENSORS_PER_OBJECT][j][0] > NEEDED_COMMON_POINTS && 
+							cd->all_counts[k+i*SENSORS_PER_OBJECT][j][1] > NEEDED_COMMON_POINTS )
+							sensors_visible++;
+					}
+					if( sensors_visible < MIN_SENSORS_VISIBLE_PER_LH_FOR_CAL ) 
+					{
+						//printf( "Dev %d, LH %d not enough visible points found.\n", i, j );
+						reset_calibration( cd );
+						cd->found_common = 0;
+						return;
+					}
 				}
 			}
 			
-- 
cgit v1.2.3


From 805b5961274a50a171d0facd791429f6b8e04d63 Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Wed, 21 Mar 2018 02:30:12 -0400
Subject: Make turvey's disambiguator send out sync pulses before all sweep
 happens.

---
 src/survive_data.c | 53 +++++++++++++++++++++++++++--------------------------
 1 file changed, 27 insertions(+), 26 deletions(-)

diff --git a/src/survive_data.c b/src/survive_data.c
index befcbe7..d8a26af 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -41,6 +41,7 @@ typedef struct
 typedef struct
 {
 	double acode_offset;
+	int sent_out_ootx_bits;
 } lightcap2_global_data;
 
 typedef struct
@@ -162,6 +163,7 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so)
 {
 	lightcap2_data *lcd = so->disambiguator_data;
 
+
 	while(remove_outliers(so));
 
 	// look at all of the sensors we found, and process the ones that were hit.
@@ -218,6 +220,30 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so)
 //printf("%4d\n", lcd->sweep.sweep_len[i]);
 				int offset_from = lcd->sweep.sweep_time[i] - lcd->per_sweep.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2;
 
+
+				// first, send out the sync pulse data for the last round (for OOTX decoding
+				if( !lcd->global.sent_out_ootx_bits ) {
+					if (lcd->per_sweep.lh_max_pulse_length[0] != 0)
+					{
+						so->ctx->lightproc( so, -1,
+							handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_max_pulse_length[0]),
+							lcd->per_sweep.lh_max_pulse_length[0],
+							lcd->per_sweep.lh_start_time[0],
+							0,
+							0);
+					}
+					if (lcd->per_sweep.lh_max_pulse_length[1] != 0)
+					{
+						so->ctx->lightproc( so, -2,
+							handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_max_pulse_length[1]),
+							lcd->per_sweep.lh_max_pulse_length[1],
+							lcd->per_sweep.lh_start_time[1],
+							0,
+							1);
+					}
+					lcd->global.sent_out_ootx_bits = 1;
+				}
+
 //				if (offset_from < 380000 && offset_from > 70000)
 				{
 					//if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) 
@@ -329,32 +355,8 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
 		//this should probably be fixed. Maybe some kind of timing based guess at which lighthouse.
 
 		// looks like this is the first sync pulse.  Cool!
+		lcd->global.sent_out_ootx_bits = 0;
 
-		// first, send out the sync pulse data for the last round (for OOTX decoding
-		{
-			if (lcd->per_sweep.lh_max_pulse_length[0] != 0)
-			{
-				so->ctx->lightproc(
-					so,
-					-1,
-					handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_max_pulse_length[0]),
-					lcd->per_sweep.lh_max_pulse_length[0],
-					lcd->per_sweep.lh_start_time[0],
-					0,
-					0);
-			}
-			if (lcd->per_sweep.lh_max_pulse_length[1] != 0)
-			{
-				so->ctx->lightproc(
-					so,
-					-2,
-					handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_max_pulse_length[1]),
-					lcd->per_sweep.lh_max_pulse_length[1],
-					lcd->per_sweep.lh_start_time[1],
-					0,
-					1);
-			}
-		}
 
 		//fprintf(stderr, "************************************ Reinitializing Disambiguator!!!\n");
 		// initialize here.
@@ -384,7 +386,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
 		}
 		*/
 	}
-
 //	printf("%d %d\n", acode, lcd->per_sweep.activeLighthouse );
 }
 
-- 
cgit v1.2.3


From be3fa4562f9578472de1ded5588df8dc502898c6 Mon Sep 17 00:00:00 2001
From: Justin Berger <j.david.berger@gmail.com>
Date: Wed, 21 Mar 2018 00:53:46 -0600
Subject: Tweaked thresholds, played with single sweep solver

---
 src/poser_sba.c                  | 186 +++++++++++++++++++++++++++++++++++----
 src/survive_sensor_activations.c |   6 +-
 2 files changed, 171 insertions(+), 21 deletions(-)

diff --git a/src/poser_sba.c b/src/poser_sba.c
index 44af9c1..a29dffe 100644
--- a/src/poser_sba.c
+++ b/src/poser_sba.c
@@ -22,6 +22,12 @@ typedef struct {
 	SurviveObject *so;
 } sba_context;
 
+typedef struct {
+    sba_context hdr;
+    int acode;
+    int lh;
+} sba_context_single_sweep;
+
 void metric_function(int j, int i, double *aj, double *xij, void *adata) {
 	sba_context *ctx = (sba_context *)(adata);
 	SurviveObject *so = ctx->so;
@@ -51,6 +57,26 @@ size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, char *
 	return measCount;
 }
 
+size_t construct_input_from_scene_single_sweep(const SurviveObject *so, PoserDataLight *pdl, SurviveSensorActivations *scene,
+                                  char *vmask, double *meas, int acode, int lh) {
+    size_t rtn = 0;
+
+    for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) {
+        const uint32_t *data_timecode = scene->timecode[sensor][lh];
+        if (pdl->timecode - data_timecode[acode & 1] <= SurviveSensorActivations_default_tolerance) {
+            double *a = scene->angles[sensor][lh];
+            vmask[sensor * NUM_LIGHTHOUSES + lh] = 1;
+            meas[rtn++] = a[acode & 0x1];
+        } else {
+            vmask[sensor * NUM_LIGHTHOUSES + lh] = 0;
+
+        }
+    }
+
+    return rtn;
+}
+
+
 size_t construct_input_from_scene(const SurviveObject *so, PoserDataLight *pdl, SurviveSensorActivations *scene,
 								  char *vmask, double *meas) {
 	size_t rtn = 0;
@@ -89,6 +115,30 @@ void sba_set_position(SurviveObject *so, uint8_t lighthouse, SurvivePose *new_po
 }
 void *GetDriver(const char *name);
 
+void str_metric_function_single_sweep(int j, int i, double *bi, double *xij, void *adata) {
+    SurvivePose obj = *(SurvivePose *)bi;
+    int sensor_idx = j >> 1;
+
+    sba_context_single_sweep *ctx = (sba_context_single_sweep *)(adata);
+    SurviveObject *so = ctx->hdr.so;
+    int lh = ctx->lh;
+    int acode = ctx->acode;
+
+    assert(lh < 2);
+    assert(sensor_idx < so->sensor_ct);
+
+    quatnormalize(obj.Rot, obj.Rot);
+    FLT xyz[3];
+    ApplyPoseToPoint(xyz, obj.Pos, &so->sensor_locations[sensor_idx * 3]);
+
+    // std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl;
+    SurvivePose *camera = &so->ctx->bsd[lh].Pose;
+
+    FLT out[2];
+    survive_reproject_from_pose_with_config(so->ctx, &ctx->hdr.calibration_config, lh, camera, xyz, out);
+    *xij = out[acode];
+}
+
 void str_metric_function(int j, int i, double *bi, double *xij, void *adata) {
 	SurvivePose obj = *(SurvivePose *)bi;
 	int sensor_idx = j >> 1;
@@ -109,13 +159,116 @@ void str_metric_function(int j, int i, double *bi, double *xij, void *adata) {
 	survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, lh, camera, xyz, xij);
 }
 
+
+
+static double run_sba_find_3d_structure_single_sweep(survive_calibration_config options, PoserDataLight *pdl, SurviveObject *so,
+                                        SurviveSensorActivations *scene,
+                                                     int acode, int lh,
+                                                     int max_iterations /* = 50*/,
+                                        double max_reproj_error /* = 0.005*/) {
+    double *covx = 0;
+
+    char *vmask = alloca(sizeof(char) * so->sensor_ct);
+    double *meas = alloca(sizeof(double) * so->sensor_ct);
+    size_t meas_size = construct_input_from_scene_single_sweep(so, pdl, scene, vmask, meas, acode, lh);
+
+    static int failure_count = 500;
+    if (so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0 || meas_size < 8) {
+        if (so->ctx->bsd[0].PositionSet && so->ctx->bsd[1].PositionSet && failure_count++ == 500) {
+            SurviveContext *ctx = so->ctx;
+            SV_INFO("Can't solve for position with just %lu measurements", meas_size);
+            failure_count = 0;
+        }
+        return -1;
+    }
+    failure_count = 0;
+
+    SurvivePose soLocation = so->OutPose;
+    bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]);
+
+    {
+        const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP");
+        PoserCB driver = (PoserCB)GetDriver(subposer);
+        SurviveContext *ctx = so->ctx;
+        if (driver) {
+            PoserData hdr = pdl->hdr;
+            memset(&pdl->hdr, 0, sizeof(pdl->hdr)); // Clear callback functions
+            pdl->hdr.pt = hdr.pt;
+            pdl->hdr.rawposeproc = sba_set_position;
+
+            sba_set_position_t locations = {};
+            pdl->hdr.userdata = &locations;
+            driver(so, &pdl->hdr);
+            pdl->hdr = hdr;
+
+            if (locations.hasInfo == false) {
+
+                return -1;
+            } else if (locations.hasInfo) {
+                soLocation = locations.poses;
+            }
+        } else {
+            SV_INFO("Not using a seed poser for SBA; results will likely be way off");
+        }
+    }
+
+    double opts[SBA_OPTSSZ] = {};
+    double info[SBA_INFOSZ] = {};
+
+    sba_context_single_sweep ctx = {
+            .hdr = { options, &pdl->hdr, so },
+            .acode = acode,
+            .lh = lh
+    };
+
+    opts[0] = SBA_INIT_MU;
+    opts[1] = SBA_STOP_THRESH;
+    opts[2] = SBA_STOP_THRESH;
+    opts[3] = SBA_STOP_THRESH;
+    opts[3] = SBA_STOP_THRESH; // max_reproj_error * meas.size();
+    opts[4] = 0.0;
+
+    int status = sba_str_levmar(1, // Number of 3d points
+                                0, // Number of 3d points to fix in spot
+                                so->sensor_ct, vmask,
+                                soLocation.Pos, // Reads as the full pose though
+                                7,				// pnp -- SurvivePose
+                                meas,			// x* -- measurement data
+                                0,				// cov data
+                                1,				// mnp -- 2 points per image
+                                str_metric_function_single_sweep,
+                                0,				// jacobia of metric_func
+                                &ctx,			// user data
+                                max_iterations, // Max iterations
+                                0,				// verbosity
+                                opts,			// options
+                                info);			// info
+
+    if (status > 0) {
+        quatnormalize(soLocation.Rot, soLocation.Rot);
+        PoserData_poser_raw_pose_func(&pdl->hdr, so, 1, &soLocation);
+
+        SurviveContext *ctx = so->ctx;
+        // Docs say info[0] should be divided by meas; I don't buy it really...
+        static int cnt = 0;
+        if (cnt++ > 1000 || meas_size < 8) {
+            SV_INFO("%f original reproj error for %lu meas", (info[0] / meas_size * 2), meas_size);
+            SV_INFO("%f cur reproj error", (info[1] / meas_size * 2));
+            cnt = 0;
+        }
+    }
+
+    return info[1] / meas_size * 2;
+}
+
+
 static double run_sba_find_3d_structure(survive_calibration_config options, PoserDataLight *pdl, SurviveObject *so,
 										SurviveSensorActivations *scene, int max_iterations /* = 50*/,
 										double max_reproj_error /* = 0.005*/) {
 	double *covx = 0;
 
-	char *vmask = malloc(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES);
-	double *meas = malloc(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES);
+	char *vmask = alloca(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES);
+	double *meas = alloca(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES);
 	size_t meas_size = construct_input_from_scene(so, pdl, scene, vmask, meas);
 
 	static int failure_count = 500;
@@ -125,8 +278,6 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose
 			SV_INFO("Can't solve for position with just %lu measurements", meas_size);
 			failure_count = 0;
 		}
-		free(vmask);
-		free(meas);
 		return -1;
 	}
 	failure_count = 0;
@@ -150,8 +301,6 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose
 			pdl->hdr = hdr;
 
 			if (locations.hasInfo == false) {
-				free(vmask);
-				free(meas);
 
 				return -1;
 			} else if (locations.hasInfo) {
@@ -206,9 +355,6 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose
 		}
 	}
 
-	free(vmask);
-	free(meas);
-
 	return info[1] / meas_size * 2;
 }
 
@@ -216,8 +362,8 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd
 					  int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) {
 	double *covx = 0;
 
-	char *vmask = malloc(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES);
-	double *meas = malloc(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES);
+	char *vmask = alloca(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES);
+	double *meas = alloca(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES);
 	size_t meas_size = construct_input(so, pdfs, vmask, meas);
 
 	SurvivePose camera_params[2] = {so->ctx->bsd[0].Pose, so->ctx->bsd[1].Pose};
@@ -282,9 +428,6 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd
 	// Docs say info[0] should be divided by meas; I don't buy it really...
 	// std::cerr << info[0] / meas.size() * 2 << " original reproj error" << std::endl;
 
-	free(vmask);
-	free(meas);
-
 	{
 		SurviveContext *ctx = so->ctx;
 		// Docs say info[0] should be divided by meas; I don't buy it really...
@@ -299,11 +442,18 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
 	switch (pd->pt) {
 	case POSERDATA_LIGHT: {
 		SurviveSensorActivations *scene = &so->activations;
-
 		PoserDataLight *lightData = (PoserDataLight *)pd;
-
-		survive_calibration_config config = *survive_calibration_default_config();
-		FLT error = run_sba_find_3d_structure(config, lightData, so, scene, 50, .5);
+/*
+        static int last_acode = -1;
+        static int last_lh = -1;
+        if(last_lh != lightData->lh || last_acode != lightData->acode) {
+        */
+            survive_calibration_config config = *survive_calibration_default_config();
+            FLT error = run_sba_find_3d_structure(config, lightData, so, scene, 50, .5);
+        /*}
+        last_lh = lightData->lh;
+        last_acode = lightData->acode;
+         */
 		return 0;
 	}
 	case POSERDATA_FULL_SCENE: {
diff --git a/src/survive_sensor_activations.c b/src/survive_sensor_activations.c
index 42de833..d14bbec 100644
--- a/src/survive_sensor_activations.c
+++ b/src/survive_sensor_activations.c
@@ -3,7 +3,8 @@
 bool SurviveSensorActivations_isPairValid(const SurviveSensorActivations *self, uint32_t tolerance,
 										  uint32_t timecode_now, uint32_t idx, int lh) {
 	const uint32_t *data_timecode = self->timecode[idx][lh];
-	return !(timecode_now - data_timecode[0] > tolerance || timecode_now - data_timecode[1] > tolerance);
+	return !(timecode_now - data_timecode[0] > tolerance ||
+			 timecode_now - data_timecode[1] > tolerance);
 }
 
 void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDataLight *lightData) {
@@ -15,5 +16,4 @@ void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDa
 	*data_timecode = lightData->timecode;
 }
 
-// Roughly 31ms at a 48mhz clock rate
-uint32_t SurviveSensorActivations_default_tolerance = 500000;
\ No newline at end of file
+uint32_t SurviveSensorActivations_default_tolerance = (uint32_t) (48000000/*mhz*/ * (16.7 * 2/*ms*/) / 1000);
\ No newline at end of file
-- 
cgit v1.2.3