aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--dave/AffineSolve.c50
-rw-r--r--dave/Makefile2
-rw-r--r--redist/linmath.c17
-rw-r--r--redist/linmath.h2
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 );