From dde0e82eb7a5a5500e27071e344e8afe4e336049 Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Sat, 11 Mar 2017 22:45:31 -0500
Subject: Update with almost working poser information stuff. This has been
 long stream to live. Goobye.

---
 src/poser_daveortho.c | 457 ++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 457 insertions(+)
 create mode 100644 src/poser_daveortho.c

(limited to 'src/poser_daveortho.c')

diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c
new file mode 100644
index 0000000..80ddd90
--- /dev/null
+++ b/src/poser_daveortho.c
@@ -0,0 +1,457 @@
+#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, &T[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]);
+    }
+
+}
+
-- 
cgit v1.2.3