From 8701607e71c70e668f5e906a787b1fab325c0fb8 Mon Sep 17 00:00:00 2001 From: ultramn Date: Wed, 8 Mar 2017 21:52:49 -0800 Subject: added affinesolve makefile --- dave/Makefile | 1 + 1 file changed, 1 insertion(+) (limited to 'dave/Makefile') diff --git a/dave/Makefile b/dave/Makefile index ed916f4..af29e1d 100644 --- a/dave/Makefile +++ b/dave/Makefile @@ -1,5 +1,6 @@ all: # gcc -O3 -o kalman_filter kalman_filter.c main.c gcc -O3 -o dclapack_test dclapack_test.c + gcc -O3 -o AffineSolve AffineSolve.c clean: rm -f kalman_filter dclapack_test -- cgit v1.3.1 From 48aa2b2a13daf5a007deb3fedf0da4e49aae0fd5 Mon Sep 17 00:00:00 2001 From: ultramn Date: Wed, 8 Mar 2017 21:54:22 -0800 Subject: Added -lm --- dave/Makefile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'dave/Makefile') diff --git a/dave/Makefile b/dave/Makefile index af29e1d..fc7dde5 100644 --- a/dave/Makefile +++ b/dave/Makefile @@ -1,6 +1,6 @@ all: # gcc -O3 -o kalman_filter kalman_filter.c main.c - gcc -O3 -o dclapack_test dclapack_test.c - gcc -O3 -o AffineSolve AffineSolve.c + gcc -O3 -o dclapack_test dclapack_test.c -lm + gcc -O3 -o AffineSolve AffineSolve.c -lm clean: rm -f kalman_filter dclapack_test -- cgit v1.3.1 From 6362b6ee00ef5e394b636e2bb63d5e20c18c01f8 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Thu, 9 Mar 2017 02:38:10 -0500 Subject: switch to atan2 instead of asin. Take out of performance check and put into test mode again. --- dave/AffineSolve.c | 9 +++++---- dave/Makefile | 3 ++- dave/dclapack_test | Bin 12836 -> 12960 bytes 3 files changed, 7 insertions(+), 5 deletions(-) (limited to 'dave/Makefile') diff --git a/dave/AffineSolve.c b/dave/AffineSolve.c index ad9ccb5..e848ade 100644 --- a/dave/AffineSolve.c +++ b/dave/AffineSolve.c @@ -314,6 +314,7 @@ PRINT(ab,2,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 //------------------- @@ -321,11 +322,11 @@ PRINT(ab,2,1); float 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]; float 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]; float 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] = asin(Tx / Ty); // horiz - S_out[1][i] = asin(Tz / Ty); // vert + 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]); + 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]); } // printf("xbar %f %f %f\n", xbar[0], xbar[1], xbar[2]); @@ -603,7 +604,7 @@ int main() //-------------------------------------------------- int loop; - for (loop=0; loop<1000000; loop++) + for (loop=0; loop<1; loop++) { // Run OrthoSolve OrthoSolve( diff --git a/dave/Makefile b/dave/Makefile index fc7dde5..7738850 100644 --- a/dave/Makefile +++ b/dave/Makefile @@ -1,6 +1,7 @@ all: # gcc -O3 -o kalman_filter kalman_filter.c main.c gcc -O3 -o dclapack_test dclapack_test.c -lm - gcc -O3 -o AffineSolve AffineSolve.c -lm + gcc -O0 -g -o AffineSolve AffineSolve.c -lm #-Wall + clean: rm -f kalman_filter dclapack_test diff --git a/dave/dclapack_test b/dave/dclapack_test index 7789e78..bac05e7 100755 Binary files a/dave/dclapack_test and b/dave/dclapack_test differ -- cgit v1.3.1 From 6541d1e63358324400ab446a4d1457941a0e90cf Mon Sep 17 00:00:00 2001 From: cnlohr 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(-) (limited to 'dave/Makefile') 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 #include #include "dclapack.h" - -#define LH_ID 0 +#include +#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 #include +#include 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.3.1