From 6541d1e63358324400ab446a4d1457941a0e90cf Mon Sep 17 00:00:00 2001
From: cnlohr <lohr85@gmail.com>
Date: Thu, 9 Mar 2017 20:35:55 -0500
Subject: Update linmath to allow for transforming the pose solver for testing
 purposes.

---
 dave/AffineSolve.c | 50 ++++++++++++++++++++++++++++++++++++++++++++------
 dave/Makefile      |  2 +-
 redist/linmath.c   | 17 +++++++++--------
 redist/linmath.h   |  2 +-
 4 files changed, 55 insertions(+), 16 deletions(-)

diff --git a/dave/AffineSolve.c b/dave/AffineSolve.c
index f29e29e..6e17f26 100644
--- a/dave/AffineSolve.c
+++ b/dave/AffineSolve.c
@@ -10,8 +10,8 @@
 #include <stdlib.h>
 #include <math.h>
 #include "dclapack.h"
-
-#define LH_ID    0
+#include <linmath.h>
+#define LH_ID    1
 #define NUM_HMD 32
 
 #define MAX_POINTS 128
@@ -209,7 +209,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
     FLOAT ydist1 = 1.0 /  uhat_len; //0.25*PI / uhat_len;
     FLOAT ydist2 = 1.0 /  rhat_len; //0.25*PI / rhat_len;
     FLOAT ydist  = 1.0 / urhat_len;
-//    printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist);
+    //printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist);
 
     //--------------------
     // Rescale the axies to be of the proper length
@@ -223,6 +223,9 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
     FLOAT x_y = sqrt(1.0 - x[0][0]*x[0][0] - x[2][0]*x[2][0]);
     FLOAT y_y = sqrt(1.0 - y[0][0]*y[0][0] - y[2][0]*y[2][0]);
     FLOAT 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 . . .
     FLOAT bestErr = 9999.0;
@@ -243,17 +246,18 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
                 FLOAT cx,cy,cz;
                 CrossProduct(cx,cy,cz,x[0][0],x_y,x[2][0],y[0][0],y_y,y[2][0]);
                 FLOAT hand = cx*z[0][0] + cy*z_y + cz*z[2][0];
-//                printf("err %f hand %f\n", err, hand);
+                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 (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);
+    printf("bestErr %f\n", bestErr);
 /*
     for (i=0; i<nPoints; i++) {
         float x1 = x[0][0]*X[0][i] + y[0][0]*X[1][i] + z[0][0]*X[2][i];
@@ -329,6 +333,40 @@ PRINT(ab,2,1);
         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]);
     }
 
+
+
+
+	float quat[4];
+	float posoff[3] = { trans[0], trans[1], trans[2] };
+	float MT[4][4];
+	//matrix44transpose( MT, &T[0][0] );
+	matrix44copy( &MT[0][0], &T[0][0] );
+
+//QUAT: 0.657864 -0.305763 0.128486 0.265895 = 0.783252
+//QUAT: 0.657864 0.305763 -0.128486 -0.265895 = 0.783252
+
+
+	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\n", quat[0], quat[1], quat[2], quat[3], quatmagnitude(quat) );
+
+
+	for( i = 0; i <nPoints;i++ )
+	{
+		float pt[3] = { X_in[0][i], X_in[1][i], X_in[2][i] };
+		quatrotatevector( pt, quat, pt );
+		add3d( pt, pt, posoff );
+		printf( "%f %f %f OUT %f %f %f ANGLE %f %f AOUT %f %f\n", 
+			X_in[0][i], X_in[1][i], X_in[2][i], pt[0], pt[1], pt[2],
+			S_in[0][i], S_in[1][i], atan2( pt[0], pt[1] ), atan2( pt[2], pt[1] ) );
+	}
+	quattomatrix( &MT[0][0], quat );
+    PRINT_MAT(MT,4,4);
+
+
+
 //    printf("xbar %f %f %f\n", xbar[0], xbar[1], xbar[2]);
 //    printf("trans %f %f %f dist: %f\n", trans[0], trans[1], trans[2], transdist);
 }
diff --git a/dave/Makefile b/dave/Makefile
index 7738850..bf62837 100644
--- a/dave/Makefile
+++ b/dave/Makefile
@@ -1,7 +1,7 @@
 all:
 #	gcc -O3 -o kalman_filter kalman_filter.c main.c
 	gcc -O3 -o dclapack_test dclapack_test.c -lm
-	gcc  -O0 -g -o AffineSolve   AffineSolve.c -lm #-Wall
+	gcc  -O0 -g -o AffineSolve   AffineSolve.c -lm -I../redist ../redist/linmath.c #-Wall
 
 clean:
 	rm -f kalman_filter dclapack_test
diff --git a/redist/linmath.c b/redist/linmath.c
index d76fbba..72c00a4 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -3,6 +3,7 @@
 #include "linmath.h"
 #include <math.h>
 #include <float.h>
+#include <string.h>
 
 void cross3d( FLT * out, const FLT * a, const FLT * b )
 {
@@ -299,14 +300,9 @@ void quatadd( FLT * qout, const FLT * a, const FLT * b )
 	qout[3] = a[3] + b[3];
 }
 
-void quatrotateabout( FLT * qout, const FLT * a, const FLT * b )
+void quatrotateabout( FLT * qout, const FLT * q1, const FLT * q2 )
 {
-	FLT q1[4];
-	FLT q2[4];
-
-	//quatnormalize( q1, a );
-	//quatnormalize( q2, b );
-
+	//NOTE: Does not normalize
 	qout[0] = (q1[0]*q2[0])-(q1[1]*q2[1])-(q1[2]*q2[2])-(q1[3]*q2[3]);
 	qout[1] = (q1[0]*q2[1])+(q1[1]*q2[0])+(q1[2]*q2[3])-(q1[3]*q2[2]);
 	qout[2] = (q1[0]*q2[2])-(q1[1]*q2[3])+(q1[2]*q2[0])+(q1[3]*q2[1]);
@@ -396,7 +392,7 @@ void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in )
 	vquat[3] = vec3in[2];
 
 	quatrotateabout( tquat, quat, vquat );
-	quatgetreciprocal( qrecp, quat );
+	quatgetconjugate( qrecp, quat );
 	quatrotateabout( vquat, tquat, qrecp );
 
 	vec3out[0] = vquat[1];
@@ -515,6 +511,11 @@ void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest)
 
 }
 
+void matrix44copy(FLT * mout, const FLT * minm )
+{
+	memcpy( mout, minm, sizeof( FLT ) * 16 );
+}
+
 ///////////////////////////////////////Matrix Rotations////////////////////////////////////
 ////Originally from Stack Overflow 
 ////Under cc by-sa 3.0
diff --git a/redist/linmath.h b/redist/linmath.h
index ec20534..676d182 100644
--- a/redist/linmath.h
+++ b/redist/linmath.h
@@ -104,7 +104,7 @@ void rotation_between_vecs_to_m3(Matrix3x3 *m, const FLT v1[3], const FLT v2[3])
 Matrix3x3 inverseM33(const Matrix3x3 mat);
 
 
-
+void matrix44copy(FLT * mout, const FLT * minm );
 
 
 
-- 
cgit v1.2.3