From 3da18b263cb0cf1814cef374771d242c9591ed2d Mon Sep 17 00:00:00 2001 From: cnlohr Date: Tue, 13 Dec 2016 20:56:35 -0500 Subject: Add charles' camfind (DOES NOT WORK!!!) --- tools/planetest/Makefile | 10 ++ tools/planetest/camfind.c | 259 ++++++++++++++++++++++++++++++++++++ tools/planetest/linmath.c | 326 ++++++++++++++++++++++++++++++++++++++++++++++ tools/planetest/linmath.h | 59 +++++++++ 4 files changed, 654 insertions(+) create mode 100644 tools/planetest/Makefile create mode 100644 tools/planetest/camfind.c create mode 100644 tools/planetest/linmath.c create mode 100644 tools/planetest/linmath.h (limited to 'tools') diff --git a/tools/planetest/Makefile b/tools/planetest/Makefile new file mode 100644 index 0000000..0e55f12 --- /dev/null +++ b/tools/planetest/Makefile @@ -0,0 +1,10 @@ +all : camfind + +CFLAGS:=-g -O4 +LDFLAGS:=$(CFLAGS) -lm + +camfind : camfind.o linmath.o + gcc -o $@ $^ $(LDFLAGS) + +clean : + rm -rf *.o *~ camfind diff --git a/tools/planetest/camfind.c b/tools/planetest/camfind.c new file mode 100644 index 0000000..c1fba0e --- /dev/null +++ b/tools/planetest/camfind.c @@ -0,0 +1,259 @@ +#include +#include +#include "linmath.h" +#include + +#define PTS 32 +#define MAX_CHECKS 20000 + +double hmd_points[PTS*3]; +double hmd_point_angles[PTS*2]; +int hmd_point_counts[PTS*2]; +int LoadData( char Camera ); + +//Values used for RunTest() +float LighthousePos[3] = { 0, 0,0 }; +float LighthouseQuat[4] = { 1, 0, 0, 0 }; + +//Actual values +float xyzypr[6] = { 0, 0, 1, 0, 0, 0 }; + + +float RunOpti( int axis ); +float RunTest(); + +int main() +{ + int i; + + if( LoadData( 'L' ) ) return 5; + + int opti = 0; + int cycle = 0; + int axis = 0; + + float dx; + + float bestxyzypr[6]; + memcpy( bestxyzypr, xyzypr, sizeof( xyzypr ) ); + + float fullrange = 5; + for( cycle = 0; cycle < 10; cycle ++ ) + { + for( axis = 0; axis < 3; axis++ ) + { + float bestxyzyprrunning[6]; + float beste = 1e20; + for( dx = -fullrange; dx < fullrange; dx += fullrange/100 ) + { + memcpy( xyzypr, bestxyzypr, sizeof( xyzypr ) ); + xyzypr[axis] += dx; + xyzypr[3] = 0; + xyzypr[4] = 0; + xyzypr[5] = 0; + float ft; + for( opti = 3; opti < 6; opti++ ) + { + ft = RunOpti( opti ); + } + if( ft < beste ) { beste = ft; memcpy( bestxyzyprrunning, xyzypr, sizeof( xyzypr ) ); } + } + printf( "%f %f %f %f %f %f = %f\n", xyzypr[0], xyzypr[1], xyzypr[2], xyzypr[3], xyzypr[4], xyzypr[5], beste ); + memcpy( bestxyzypr, bestxyzyprrunning, sizeof( bestxyzypr ) ); + } + + fullrange *= 0.2; + } +} + + +float RunOpti( int axis ) +{ + float xyzyprchange[6]; + memcpy( xyzyprchange, xyzypr, sizeof( xyzypr ) ); + int i; + float minv = 1e20; + float fv = -1.3; + float bv = 0; + + //Coarse linear search + for( i = 0; i < 26; i++ ) + { + xyzyprchange[axis] = fv; + quatfromeuler( LighthouseQuat, &xyzyprchange[3] ); + memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) ); + float ft = RunTest(); + if( ft < minv ) { minv = ft; bv = fv; } + fv += 0.1; + } + + xyzyprchange[axis] = bv; + + //Now, do a more precise binary search. + float jumpamount = 0.1; + for( i = 0; i < 30; i++ ) + { + float orig = xyzyprchange[axis]; + + minv = 1e20; + + fv = xyzyprchange[axis] = orig; + quatfromeuler( LighthouseQuat, &xyzyprchange[3] ); + memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) ); + float ft = RunTest(); + if( ft < minv ) { minv = ft; bv = fv; } + + fv = xyzyprchange[axis] = orig + jumpamount; + quatfromeuler( LighthouseQuat, &xyzyprchange[3] ); + memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) ); + ft = RunTest(); + if( ft < minv ) { minv = ft; bv = fv; } + + fv = xyzyprchange[axis] = orig - jumpamount; + quatfromeuler( LighthouseQuat, &xyzyprchange[3] ); + memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) ); + ft = RunTest(); + if( ft < minv ) { minv = ft; bv = fv; } + + xyzyprchange[axis] = bv; + + jumpamount *= 0.5; + } + + + + xyzypr[axis] = bv; + return minv; +} + + +float RunTest() +{ + int k; + float totprob = 0.0; + for( k = 0; k < 64; k++ ) + { + if( hmd_point_counts[k] == 0 ) continue; + int axis = k%2; + int pt = k/2; + float angle = (hmd_point_angles[k] - 200000) / 200000 * 3.1415926535; + if( axis == 0 ) angle = -angle; + float thiseuler[3] = { 0, 0, 0 }; + thiseuler[axis] = angle; + //axis = 0: x changes. +y is rotated plane normal. + //axis = 1: y changes. +x is rotated plane normal. + float planequat[4]; + quatfromeuler( planequat, thiseuler ); + quatrotateabout( planequat, planequat, LighthouseQuat ); //Order of rotations may be wrong. + float plane_normal[3] = { 0, 0, 0 }; + plane_normal[!axis] = 1; + quatrotatevector( plane_normal, planequat, plane_normal ); + + //plane_normal is our normal / LighthousePos is our point. + float w0[] = { hmd_points[pt*3+0], hmd_points[pt*3+1], hmd_points[pt*3+2] }; + //float w0[] = { 0, 0, 0 }; + float d = plane_normal[0] * LighthousePos[0] + plane_normal[1] * LighthousePos[1] + plane_normal[2] * LighthousePos[2]; + float D = plane_normal[0] * w0[0] + plane_normal[1] * w0[1] + plane_normal[2] * w0[2] - d; //Point line distance assuming ||normal|| = 1. + + totprob += D*D; + + //printf( " : %f %f %f %f %f %f\n", plane_normal[0], plane_normal[1], plane_normal[2], 1.0, angle, hmd_point_angles[k] ); + } + return totprob; +} + + +int LoadData( char Camera ) +{ + int calpts[MAX_CHECKS*4]; //X (0) or Y (1), ID, offset + int calptscount; + + FILE * f = fopen( "correct_hmd_points.csv", "r" ); + int pt = 0; + if( !f ) { fprintf( stderr, "error: can't open hmd points.\n" ); return -5; } + while(!feof(f) && !ferror(f) && pt < PTS) + { + float fa, fb, fc; + int r = fscanf( f,"%g,%g,%g\n", &fa, &fb, &fc ); + hmd_points[pt*3+0] = fa; + hmd_points[pt*3+1] = fb; + hmd_points[pt*3+2] = fc; + pt++; + if( r != 3 ) + { + fprintf( stderr, "Not enough entries on line %d\n", pt ); + return -8; + } + } + if( pt < PTS ) + { + fprintf( stderr, "Not enough points.\n" ); + return -9; + } + fclose( f ); + printf( "Loaded %d points\n", pt ); + + + int xck = 0; + f = fopen( "second_full_test.csv", "r" ); + if( !f ) { fprintf( stderr, "Error: can't open two lighthouses test data.\n" ); return -11; } + while( !feof(f) && !ferror(f) ) + { + char * lineptr; + size_t n; + lineptr = 0; + n = 0; + ssize_t r = getline( &lineptr, &n, f ); + char lf[10]; + char xory[10]; + char dev[10]; + int timestamp; + int sensorid; + int offset; + int code; + int rk = sscanf( lineptr, "%9s %9s %9s %d %d %d %d\n", lf, xory, dev, ×tamp, &sensorid, &code, &offset ); + if( lf[0] == Camera && rk == 7 ) + { + //calpts + if( xory[0] == 'X' ) + { + calpts[xck*3+0] = 0; + } + else if( xory[0] == 'Y' ) + { + calpts[xck*3+0] = 1; + } + else + { + printf( "Confusing line\n" ); + continue; + } + + calpts[xck*3+1] = sensorid; + calpts[xck*3+2] = offset; + xck++; + } + if( lineptr ) free( lineptr ); + } + printf( "Cal points: %d\n", xck ); + calptscount = xck; + + int i; + for( i = 0; i < calptscount; i++ ) + { + int isy = calpts[i*3+0]; + int pt = calpts[i*3+1]; + int ofs = calpts[i*3+2]; + hmd_point_counts[pt*2+isy]++; + hmd_point_angles[pt*2+isy]+=ofs; + } + for( i = 0; i < 32; i++ ) + { + if( hmd_point_counts[i*2+0] < 100 ) hmd_point_counts[i*2+0] = 0; + if( hmd_point_counts[i*2+1] < 100 ) hmd_point_counts[i*2+1] = 0; + + hmd_point_angles[i*2+0]/=hmd_point_counts[i*2+0]; + hmd_point_angles[i*2+1]/=hmd_point_counts[i*2+1]; + } + return 0; +} diff --git a/tools/planetest/linmath.c b/tools/planetest/linmath.c new file mode 100644 index 0000000..dec8c70 --- /dev/null +++ b/tools/planetest/linmath.c @@ -0,0 +1,326 @@ +//Copyright 2013 <>< C. N. Lohr. This file licensed under the terms of the MIT license. + +#include "linmath.h" +#include + +void cross3d( float * out, const float * a, const float * b ) +{ + out[0] = a[1]*b[2] - a[2]*b[1]; + out[1] = a[2]*b[0] - a[0]*b[2]; + out[2] = a[0]*b[1] - a[1]*b[0]; +} + +void sub3d( float * out, const float * a, const float * b ) +{ + out[0] = a[0] - b[0]; + out[1] = a[1] - b[1]; + out[2] = a[2] - b[2]; +} + +void add3d( float * out, const float * a, const float * b ) +{ + out[0] = a[0] + b[0]; + out[1] = a[1] + b[1]; + out[2] = a[2] + b[2]; +} + +void scale3d( float * out, const float * a, float scalar ) +{ + out[0] = a[0] * scalar; + out[1] = a[1] * scalar; + out[2] = a[2] * scalar; +} + +void normalize3d( float * out, const float * in ) +{ + float r = 1./sqrtf( in[0] * in[0] + in[1] * in[1] + in[2] * in[2] ); + out[0] = in[0] * r; + out[1] = in[1] * r; + out[2] = in[2] * r; +} + +float dot3d( const float * a, const float * b ) +{ + return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; +} + +int compare3d( const float * a, const float * b, float epsilon ) +{ + if( !a || !b ) return 0; + if( a[2] - b[2] > epsilon ) return 1; + if( b[2] - a[2] > epsilon ) return -1; + if( a[1] - b[1] > epsilon ) return 1; + if( b[1] - a[1] > epsilon ) return -1; + if( a[0] - b[0] > epsilon ) return 1; + if( b[0] - a[0] > epsilon ) return -1; + return 0; +} + +void copy3d( float * out, const float * in ) +{ + out[0] = in[0]; + out[1] = in[1]; + out[2] = in[2]; +} + + + +/////////////////////////////////////QUATERNIONS////////////////////////////////////////// +//Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman) +//Under the mit/X11 license. + + + + +void quatsetnone( float * q ) +{ + q[0] = 0; q[1] = 0; q[2] = 0; q[3] = 1; +} + +void quatcopy( float * qout, const float * qin ) +{ + qout[0] = qin[0]; + qout[1] = qin[1]; + qout[2] = qin[2]; + qout[3] = qin[3]; +} + +void quatfromeuler( float * q, const float * euler ) +{ + float X = euler[0]/2.0f; //roll + float Y = euler[1]/2.0f; //pitch + float Z = euler[2]/2.0f; //yaw + + float cx = cosf(X); + float sx = sinf(X); + float cy = cosf(Y); + float sy = sinf(Y); + float cz = cosf(Z); + float sz = sinf(Z); + + //Correct according to + //http://en.wikipedia.org/wiki/Conversion_between_MQuaternions_and_Euler_angles + q[0] = cx*cy*cz+sx*sy*sz;//q1 + q[1] = sx*cy*cz-cx*sy*sz;//q2 + q[2] = cx*sy*cz+sx*cy*sz;//q3 + q[3] = cx*cy*sz-sx*sy*cz;//q4 + quatnormalize( q, q ); +} + +void quattoeuler( float * euler, const float * q ) +{ + //According to http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles (Oct 26, 2009) + euler[0] = atan2( 2 * (q[0]*q[1] + q[2]*q[3]), 1 - 2 * (q[1]*q[1] + q[2]*q[2] ) ); + euler[1] = asin( 2 * (q[0] *q[2] - q[3]*q[1] ) ); + euler[2] = atan2( 2 * (q[0]*q[3] + q[1]*q[2]), 1 - 2 * (q[2]*q[2] + q[3]*q[3] ) ); +} + +void quatfromaxisangle( float * q, const float * axis, float radians ) +{ + float v[3]; + normalize3d( v, axis ); + + float sn = sin(radians/2.0f); + q[0] = cos(radians/2.0f); + q[1] = sn * v[0]; + q[2] = sn * v[1]; + q[3] = sn * v[2]; + + quatnormalize( q, q ); +} + +float quatmagnitude( const float * q ) +{ + return sqrt((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3])); +} + +float quatinvsqmagnitude( const float * q ) +{ + return 1./((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3])); +} + + +void quatnormalize( float * qout, const float * qin ) +{ + float imag = quatinvsqmagnitude( qin ); + quatscale( qout, qin, imag ); +} + +void quattomatrix( float * matrix44, const float * qin ) +{ + float q[4]; + quatnormalize( q, qin ); + + //Reduced calulation for speed + float xx = 2*q[0]*q[0]; + float xy = 2*q[0]*q[1]; + float xz = 2*q[0]*q[2]; + float xw = 2*q[0]*q[3]; + + float yy = 2*q[1]*q[1]; + float yz = 2*q[1]*q[2]; + float yw = 2*q[1]*q[3]; + + float zz = 2*q[2]*q[2]; + float zw = 2*q[2]*q[3]; + + //opengl major + matrix44[0] = 1-yy-zz; + matrix44[1] = xy-zw; + matrix44[2] = xz+yw; + matrix44[3] = 0; + + matrix44[4] = xy+zw; + matrix44[5] = 1-xx-zz; + matrix44[6] = yz-xw; + matrix44[7] = 0; + + matrix44[8] = xz-yw; + matrix44[9] = yz+xw; + matrix44[10] = 1-xx-yy; + matrix44[11] = 0; + + matrix44[12] = 0; + matrix44[13] = 0; + matrix44[14] = 0; + matrix44[15] = 1; +} + +void quatgetconjugate( float * qout, const float * qin ) +{ + qout[0] = qin[0]; + qout[1] = -qin[1]; + qout[2] = -qin[2]; + qout[3] = -qin[3]; +} + +void quatgetreciprocal( float * qout, const float * qin ) +{ + float m = quatinvsqmagnitude(qin); + quatgetconjugate( qout, qin ); + quatscale( qout, qout, m ); +} + +void quatsub( float * qout, const float * a, const float * b ) +{ + qout[0] = a[0] - b[0]; + qout[1] = a[1] - b[1]; + qout[2] = a[2] - b[2]; + qout[3] = a[3] - b[3]; +} + +void quatadd( float * qout, const float * a, const float * b ) +{ + qout[0] = a[0] + b[0]; + qout[1] = a[1] + b[1]; + qout[2] = a[2] + b[2]; + qout[3] = a[3] + b[3]; +} + +void quatrotateabout( float * qout, const float * a, const float * b ) +{ + float q1[4]; + float q2[4]; + + quatnormalize( q1, a ); + quatnormalize( q2, b ); + + 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]); + qout[3] = (q1[0]*q2[3])+(q1[1]*q2[2])-(q1[2]*q2[1])+(q1[3]*q2[0]); +} + +void quatscale( float * qout, const float * qin, float s ) +{ + qout[0] = qin[0] * s; + qout[1] = qin[1] * s; + qout[2] = qin[2] * s; + qout[3] = qin[3] * s; +} + + +float quatinnerproduct( const float * qa, const float * qb ) +{ + return (qa[0]*qb[0])+(qa[1]*qb[1])+(qa[2]*qb[2])+(qa[3]*qb[3]); +} + +void quatouterproduct( float * outvec3, float * qa, float * qb ) +{ + outvec3[0] = (qa[0]*qb[1])-(qa[1]*qb[0])-(qa[2]*qb[3])+(qa[3]*qb[2]); + outvec3[1] = (qa[0]*qb[2])+(qa[1]*qb[3])-(qa[2]*qb[0])-(qa[3]*qb[1]); + outvec3[2] = (qa[0]*qb[3])-(qa[1]*qb[2])+(qa[2]*qb[1])-(qa[3]*qb[0]); +} + +void quatevenproduct( float * q, float * qa, float * qb ) +{ + q[0] = (qa[0]*qb[0])-(qa[1]*qb[1])-(qa[2]*qb[2])-(qa[3]*qb[3]); + q[1] = (qa[0]*qb[1])+(qa[1]*qb[0]); + q[2] = (qa[0]*qb[2])+(qa[2]*qb[0]); + q[3] = (qa[0]*qb[3])+(qa[3]*qb[0]); +} + +void quatoddproduct( float * outvec3, float * qa, float * qb ) +{ + outvec3[0] = (qa[2]*qb[3])-(qa[3]*qb[2]); + outvec3[1] = (qa[3]*qb[1])-(qa[1]*qb[3]); + outvec3[2] = (qa[1]*qb[2])-(qa[2]*qb[1]); +} + +void quatslerp( float * q, const float * qa, const float * qb, float t ) +{ + float an[4]; + float bn[4]; + quatnormalize( an, qa ); + quatnormalize( bn, qb ); + float cosTheta = quatinnerproduct(an,bn); + float sinTheta; + + //Careful: If cosTheta is exactly one, or even if it's infinitesimally over, it'll + // cause SQRT to produce not a number, and screw everything up. + if ( 1 - (cosTheta*cosTheta) <= 0 ) + sinTheta = 0; + else + sinTheta = sqrt(1 - (cosTheta*cosTheta)); + + float Theta = acos(cosTheta); //Theta is half the angle between the 2 MQuaternions + + if(fabs(Theta) < DEFAULT_EPSILON ) + quatcopy( q, qa ); + else if(fabs(sinTheta) < DEFAULT_EPSILON ) + { + quatadd( q, qa, qb ); + quatscale( q, q, 0.5 ); + } + else + { + float aside[4]; + float bside[4]; + quatscale( bside, qb, sin( t * Theta ) ); + quatscale( aside, qa, sin((1-t)*Theta) ); + quatadd( q, aside, bside ); + quatscale( q, q, 1./sinTheta ); + } +} + +void quatrotatevector( float * vec3out, const float * quat, const float * vec3in ) +{ + float tquat[4]; + float vquat[4]; + float qrecp[4]; + vquat[0] = 0; + vquat[1] = vec3in[0]; + vquat[2] = vec3in[1]; + vquat[3] = vec3in[2]; + + quatrotateabout( tquat, quat, vquat ); + quatgetreciprocal( qrecp, quat ); + quatrotateabout( vquat, tquat, qrecp ); + + vec3out[0] = vquat[1]; + vec3out[1] = vquat[2]; + vec3out[2] = vquat[3]; +} + + + diff --git a/tools/planetest/linmath.h b/tools/planetest/linmath.h new file mode 100644 index 0000000..344a547 --- /dev/null +++ b/tools/planetest/linmath.h @@ -0,0 +1,59 @@ +//Copyright 2013 <>< C. N. Lohr. This file licensed under the terms of the MIT license. + +#ifndef _LINMATH_H +#define _LINMATH_H + +//Yes, I know it's kind of arbitrary. +#define DEFAULT_EPSILON 0.001 + + +//NOTE: Inputs may never be output with cross product. +void cross3d( float * out, const float * a, const float * b ); + +void sub3d( float * out, const float * a, const float * b ); + +void add3d( float * out, const float * a, const float * b ); + +void scale3d( float * out, const float * a, float scalar ); + +void normalize3d( float * out, const float * in ); + +float dot3d( const float * a, const float * b ); + +//Returns 0 if equal. If either argument is null, 0 will ALWAYS be returned. +int compare3d( const float * a, const float * b, float epsilon ); + +void copy3d( float * out, const float * in ); + + + + +//Quaternion things... + +void quatsetnone( float * q ); +void quatcopy( float * qout, const float * qin ); +void quatfromeuler( float * q, const float * euler ); +void quattoeuler( float * euler, const float * q ); +void quatfromaxisangle( float * q, const float * axis, float radians ); +float quatmagnitude( const float * q ); +float quatinvsqmagnitude( const float * q ); +void quatnormalize( float * qout, const float * qin ); //Safe for in to be same as out. +void quattomatrix( float * matrix44, const float * q ); +void quatgetconjugate( float * qout, const float * qin ); +void quatgetreciprocal( float * qout, const float * qin ); +void quatsub( float * qout, const float * a, const float * b ); +void quatadd( float * qout, const float * a, const float * b ); +void quatrotateabout( float * qout, const float * a, const float * b ); //same as quat multiply, not piecewise multiply. +void quatscale( float * qout, const float * qin, float s ); +float quatinnerproduct( const float * qa, const float * qb ); +void quatouterproduct( float * outvec3, float * qa, float * qb ); +void quatevenproduct( float * q, float * qa, float * qb ); +void quatoddproduct( float * outvec3, float * qa, float * qb ); +void quatslerp( float * q, const float * qa, const float * qb, float t ); +void quatrotatevector( float * vec3out, const float * quat, const float * vec3in ); + + +#endif + + + -- cgit v1.2.3