diff options
| -rw-r--r-- | Makefile | 4 | ||||
| -rw-r--r-- | dave/AffineSolve.c | 322 | ||||
| -rw-r--r-- | include/libsurvive/poser.h | 54 | ||||
| -rw-r--r-- | include/libsurvive/survive.h (renamed from include/survive.h) | 31 | ||||
| -rw-r--r-- | include/libsurvive/survive_types.h | 29 | ||||
| -rw-r--r-- | src/survive_cal_lhfind.c | 2 | ||||
| -rw-r--r-- | src/survive_internal.h | 5 | ||||
| -rw-r--r-- | src/survive_vive.c | 22 | ||||
| -rw-r--r-- | tools/plot_lighthouse/Makefile | 2 |
9 files changed, 444 insertions, 27 deletions
@@ -1,6 +1,6 @@ all : lib data_recorder test calibrate calibrate_client -CFLAGS:=-Iinclude -I. -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE -std=gnu99 +CFLAGS:=-Iinclude/libsurvive -I. -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE -std=gnu99 LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -lm -flto -g @@ -27,7 +27,7 @@ lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survi gcc -o $@ $^ $(LDFLAGS) -shared clean : - rm -rf *.o src/*.o *~ src/*~ test data_recorder lib/libsurvive.so + rm -rf *.o src/*.o *~ src/*~ test data_recorder lib/libsurvive.so redist/*.o redist/*~ diff --git a/dave/AffineSolve.c b/dave/AffineSolve.c new file mode 100644 index 0000000..36587e6 --- /dev/null +++ b/dave/AffineSolve.c @@ -0,0 +1,322 @@ +// +// main.c +// AffineSolve +// +// Created by user on 3/2/17. +// Copyright © 2017 user. All rights reserved. +// + +#include <stdio.h> +#include <string.h> +#include <stdlib.h> +#include <math.h> + +#define LH_ID 0 +#define NUM_HMD 32 + +float hmd_pos[NUM_HMD][3]; +void ReadHmdPoints() +{ + int i; + FILE *fin = fopen("HMD_points.csv","r"); + if (fin==NULL) { + printf("ERROR: could not open HMD_points.csv for reading\n"); + exit(1); + } + + for (i=0; i<NUM_HMD; i++) { + fscanf(fin, "%f %f %f", &(hmd_pos[i][0]), &(hmd_pos[i][1]), &(hmd_pos[i][2])); + } + + fclose(fin); +} + +#define MAX_POINTS 128 +#define _ABS(a) ( (a)<=0 ? -(a) : (a) ) +#define _SIGN(a) ( (a)<=0 ? -1.0f : 1.0f ) +#define RANDF ( (float)rand() / (float)RAND_MAX ) + +#define STEP_SIZE_ROT 1.0 +#define STEP_SIZE_POS 1.0 +#define FALLOFF 0.99999 +#define NITER 2000000 +#define TOO_SMALL 0.0001 +#define ORTHOG_PENALTY 1.0 + +#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"); \ + } \ +} + +void AffineSolve( + float T[4][4], // OUTPUT: transform + float O[MAX_POINTS][4], // INPUT: points, offsets + float N[MAX_POINTS][3], // INPUT: plane normals + float D[MAX_POINTS], // INPUT: plane offsets + int nPoints, int nIter, + float stepSizeRot, float stepSizePos, float falloff, int constrain) +{ + int i,j,k,iter; + //T[3][3] = 1.0f; + + printf("iter x y z error\n"); + + float gradDot = 1.0; + float prevGradDot = 1.0; + float de_dT[3][4]; // the gradient + float conj[3][4]; // the conjugate + float errorSq=0.0; + for (iter=0; iter<nIter; iter++) + { + //---------------------------------- + // Calculate the gradient direction + //---------------------------------- + errorSq = 0.0; + memset(de_dT, 0, 3*4*sizeof(float)); + for (i=0; i<nPoints; i++) + { + // What is the plane deviation error + float Ei = -D[i]; + for (j=0; j<3; j++) { + float Tj_oi = 0.0f; + for (k=0; k<4; k++) { + Tj_oi += T[j][k] * O[i][k]; + } + Ei += N[i][j] * Tj_oi; + } +// printf("E[%d] %f\n", i, Ei); + + // Figure out contribution to the error + for (j=0; j<3; j++) { + for (k=0; k<4; k++) { + de_dT[j][k] += N[i][j] * O[i][k] * Ei; + } + } + + errorSq += Ei*Ei; + } + + printf("%d %f %f %f %f\n", iter, T[0][3], T[1][3], T[2][3], sqrt(errorSq)); + + // Constrain the gradient (such that dot products are zero) + if (constrain) + { + float T0T1 = 0.0, T1T2 = 0.0, T2T0 = 0.0; + for (k=0; k<3; k++) { + T0T1 += T[0][k] * T[1][k]; + T1T2 += T[1][k] * T[2][k]; + T2T0 += T[2][k] * T[0][k]; + } +// printf("T0T1 %f T1T2 %f T2T0 %f\n", T0T1, T1T2, T2T0); + for (k=0; k<3; k++) { + de_dT[0][k] += ORTHOG_PENALTY * 2.0 * T0T1 * T[1][k]; + de_dT[0][k] += ORTHOG_PENALTY * 2.0 * T2T0 * T[2][k]; + de_dT[1][k] += ORTHOG_PENALTY * 2.0 * T1T2 * T[2][k]; + de_dT[1][k] += ORTHOG_PENALTY * 2.0 * T0T1 * T[0][k]; + de_dT[2][k] += ORTHOG_PENALTY * 2.0 * T1T2 * T[1][k]; + de_dT[2][k] += ORTHOG_PENALTY * 2.0 * T2T0 * T[0][k]; + } + } + + // Calculate the gradient dot product + // (used by conjugate gradient method) + prevGradDot = gradDot; + gradDot = 0.0; + for (j=0; j<3; j++) { + for (k=0; k<4; k++) { + gradDot += de_dT[j][k] * de_dT[j][k]; + } + } + +// printf("Iter %d error %f gradDot %f prevGradDot %f\n", iter, sqrt(errorSq), gradDot, prevGradDot); + + //---------------------------------- + // Calculate the conjugate direction + //---------------------------------- +// if (iter==0) { + // First iteration, just use the gradient + for (j=0; j<3; j++) { + for (k=0; k<4; k++) { + conj[j][k] = -de_dT[j][k]; + } + } +/* } else { + // Calculate "beta" for Fletcher Reeves method + float beta = gradDot / prevGradDot; +//printf("gradDot %f prevGradDot %f beta %f\n", gradDot, prevGradDot, beta); + + // Update the conjugate + for (j=0; j<3; j++) { + for (k=0; k<4; k++) { + conj[j][k] = beta*conj[j][k] - de_dT[j][k]; + } + } + } +*/ + +// PRINT_MAT(de_dT,4,4); +// exit(1); + + //---------------------------------- + // How large is the gradient ? + //---------------------------------- + + double gradSizeRot = 0.0; + double gradSizePos = 0.0; + for (j=0; j<3; j++) { + for (k=0; k<3; k++) { + gradSizeRot += _ABS(conj[j][k]); + } + gradSizePos += _ABS(conj[j][k]); + } + if (gradSizeRot <= TOO_SMALL && gradSizePos <= TOO_SMALL) { break; } // Quit, we've totally converged + + //---------------------------------- + // Descend in the gradient direction + //---------------------------------- + if (gradSizeRot > TOO_SMALL) { + float scaleRot = stepSizeRot / gradSizeRot; + for (j=0; j<3; j++) { + for (k=0; k<3; k++) { + T[j][k] += scaleRot * conj[j][k]; + } + } + stepSizeRot *= falloff; + } + + if (gradSizePos > TOO_SMALL) { + float scalePos = stepSizePos / gradSizePos; + for (j=0; j<3; j++) { + T[j][3] += scalePos * conj[j][3]; + } + stepSizePos *= falloff; + } + + // Constrain the gradient (such that scaling is one) + if (constrain) + { + // Measure the scales + float len[3] = {0.0, 0.0, 0.0}; + for (j=0; j<3; j++) { + double lenSq = 0.0; + for (k=0; k<3; k++) { lenSq += (double)T[j][k] * (double)T[j][k]; } + len[j] = sqrt(lenSq); + } + + // How far off is the scale? + float xzLen = 0.5 * (len[0] + len[2]); + if (xzLen > TOO_SMALL) { + float inv_xzLen = 1.0 / xzLen; + for (j=0; j<3; j++) { + T[3][j] *= inv_xzLen; + } + } + + // Rescale the thing + for (j=0; j<3; j++) + { + if (len[j] > TOO_SMALL) { + float inv_len = 1.0 / len[j]; + for (k=0; k<3; k++) { T[j][k] *= inv_len; } + } + } + } + } + +} + +int main() +{ + int i,j,k; + float Tcalc[4][4]; + float O[MAX_POINTS][4]; + float N[MAX_POINTS][3]; + float D[MAX_POINTS]; + int nPoints = 0; + + // Read the hmd points + ReadHmdPoints(); + + //------------------------- + // Read the lighthouse data + //------------------------- + FILE *fin = fopen("ptinfo.csv", "r"); + if (fin==NULL) { printf("ERROR: could not open ptinfo.csv for reading\n"); exit(1); } + while (!feof(fin)) + { + // Read the angle + int sen,lh,axis,count; + float angle, avglen, stddevang, stddevlen; + float max_outlier_length, max_outlier_angle; + int rt = fscanf( fin, "%d %d %d %d %f %f %f %f %f %f\n", + &sen, &lh, &axis, &count, + &angle, &avglen, &stddevang, &stddevlen, + &max_outlier_length, &max_outlier_angle); + if (rt != 10) { break; } + + if (lh == LH_ID && sen < NUM_HMD) { + // Set the offset + O[nPoints][0] = hmd_pos[sen][0]; + O[nPoints][1] = hmd_pos[sen][1]; + O[nPoints][2] = hmd_pos[sen][2]; + O[nPoints][3] = 1.0; + + // Calculate the plane equation + if (axis == 1) { // Horizontal + N[nPoints][0] = -cos(angle); + N[nPoints][1] = -sin(angle); + N[nPoints][2] = 0.0; + D[nPoints] = 0.0; + } else { // Vertical + N[nPoints][0] = 0.0; + N[nPoints][1] = -sin(angle); + N[nPoints][2] = cos(angle); + D[nPoints] = 0.0; + } + + printf("pt %d O %.3f %.3f %.3f %.3f N %.3f %.3f %.3f D %.3f\n", + nPoints, + O[nPoints][0], O[nPoints][1], O[nPoints][2], O[nPoints][3], + N[nPoints][0], N[nPoints][1], N[nPoints][2], + D[nPoints]); + + nPoints++; + } + } + fclose(fin); + + printf("nPoints %d\n", nPoints); + + // Run the calculation for Tcalc + int run; + //for (run=0; run<100; run++) { + + // Initialize Tcalc to the identity matrix + //memcpy(Tcalc, Torig, 4*4*sizeof(float)); + memset(Tcalc, 0, 4*4*sizeof(float)); + for (i=0; i<4; i++) { Tcalc[i][i] = 1.0f; } + + // Solve it! + AffineSolve( + Tcalc, // OUTPUT: transform + O, // INPUT: points, offsets + N, // INPUT: plane normals + D, // INPUT: plane offsets + nPoints, NITER, + STEP_SIZE_ROT, STEP_SIZE_POS, FALLOFF, + 1); + //} + + PRINT_MAT(Tcalc,4,4); + + + // insert code here... + printf("Hello, World!\n"); + return 0; +} diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h new file mode 100644 index 0000000..4894acf --- /dev/null +++ b/include/libsurvive/poser.h @@ -0,0 +1,54 @@ +#ifndef _LSPOSER_H +#define _LSPOSER_H + +#include "survive_types.h" + +typedef enum PoserType_t +{ + POSERDATA_NONE = 0, + POSERDATA_IMU, + POSERDATA_LIGHT, //Single lighting event. + POSERDATA_FULL_SCENE, //Full, statified X, Y sweep data for both lighthouses. +} PoserType; + +struct PoserData +{ + PoserType pt; + uint8_t data[0]; +}; + +struct PoserDataIMU +{ + PoserType pt; + uint8_t datamask; //0 = accel present, 1 = gyro present, 2 = mag present. + FLT accel[3]; + FLT gyro[3]; + FLT mag[3]; +}; + +struct PoserDataLight +{ + PoserType pt; + int sensor_id; + int acode; //OOTX Code associated with this sweep. base_station = acode >> 2; axis = acode & 1; + uint32_t timecode; //In object-local ticks. + FLT length; //In seconds + FLT angle; //In radians from center of lighthouse. +}; + +struct PoserDataFullScene +{ + PoserType pt; + + //If "lengths[...]" < 0, means not a valid piece of sweep information. + FLT lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; + FLT angles [SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; //2 Axes + + struct PoserDataIMU lastimu; +}; + +//When you register your posers using the internal system, +typedef int (*PoserCB)( struct SurviveObject * so, struct PoserData * pd ); + + +#endif diff --git a/include/survive.h b/include/libsurvive/survive.h index 51910a6..536c7fb 100644 --- a/include/survive.h +++ b/include/libsurvive/survive.h @@ -2,24 +2,13 @@ #define _SURVIVE_H #include <stdint.h> - -#ifndef FLT -#ifdef USE_DOUBLE -#define FLT double -#else -#define FLT float -#endif -#endif +#include "poser.h" struct SurviveContext; //DANGER: This structure may be redefined. Note that it is logically split into 64-bit chunks //for optimization on 32- and 64-bit systems. -//Careful with this, you can't just add another one right now, would take minor changes in survive_data.c and the cal tools. -//It will also require a recompile. TODO: revisit this and correct the comment once fixed. -#define NUM_LIGHTHOUSES 2 - struct SurviveObject { struct SurviveContext * ctx; @@ -36,9 +25,17 @@ struct SurviveObject int8_t ison:1; int8_t additional_flags:6; + //Pose Information, also "resolver" field. + FLT PoseConfidence; //0..1 + FLT Position[3]; + FLT Rotation[4]; + void * PoserData; //Initialized to zero, configured by poser, can be anything the poser wants. + PoserCB * PoserFn; + + //Device-specific information about the location of the sensors. This data will be used by the poser. + int8_t nr_locations; FLT * sensor_locations; FLT * sensor_normals; - int8_t nr_locations; //Timing sensitive data (mostly for disambiguation) int32_t timebase_hz; //48,000,000 for normal vive hardware. (checked) @@ -51,7 +48,7 @@ struct SurviveObject int32_t pulse_synctime_slack; //5,000 for normal vive hardware. (guessed) //Flood info, for calculating which laser is currently sweeping. - int8_t oldcode; + int8_t oldcode; int8_t sync_set_number; //0 = master, 1 = slave, -1 = fault. int8_t did_handle_ootx; //If unset, will send lightcap data for sync pulses next time a sensor is hit. uint32_t last_time[NUM_LIGHTHOUSES]; @@ -60,15 +57,11 @@ struct SurviveObject uint32_t last_lighttime; //May be a 24- or 32- bit number depending on what device. + //Debug int tsl; }; -typedef void (*text_feedback_func)( struct SurviveContext * ctx, const char * fault ); -typedef void (*light_process_func)( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); -typedef void (*imu_process_func)( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ); -typedef void (*angle_process_func)( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); - struct SurviveContext * survive_init( int headless ); //For any of these, you may pass in 0 for the function pointer to use default behavior. diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h new file mode 100644 index 0000000..593819f --- /dev/null +++ b/include/libsurvive/survive_types.h @@ -0,0 +1,29 @@ +#ifndef _SURVIVE_TYPES_H +#define _SURVIVE_TYPES_H + +#ifndef FLT +#ifdef USE_DOUBLE +#define FLT double +#else +#define FLT float +#endif +#endif + + +//Careful with this, you can't just add another one right now, would take minor changes in survive_data.c and the cal tools. +//It will also require a recompile. TODO: revisit this and correct the comment once fixed. +#define NUM_LIGHTHOUSES 2 + +#define INTBUFFSIZE 64 +#define SENSORS_PER_OBJECT 32 + +struct SurviveObject; +struct SurviveContext; + +typedef void (*text_feedback_func)( struct SurviveContext * ctx, const char * fault ); +typedef void (*light_process_func)( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); +typedef void (*imu_process_func)( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id ); +typedef void (*angle_process_func)( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); + +#endif + diff --git a/src/survive_cal_lhfind.c b/src/survive_cal_lhfind.c index e1e5fc9..93d9dc0 100644 --- a/src/survive_cal_lhfind.c +++ b/src/survive_cal_lhfind.c @@ -129,7 +129,7 @@ int survive_cal_lhfind( struct SurviveCalData * cd ) fullrange *= 0.25; } - if( beste > 0.005 ) + if( beste > 0.01 ) { //Error too high SV_ERROR( "LH: %d / Best E %f Error too high\n", lh, beste ); diff --git a/src/survive_internal.h b/src/survive_internal.h index 5962623..9d04d93 100644 --- a/src/survive_internal.h +++ b/src/survive_internal.h @@ -1,4 +1,4 @@ -//<>< (C) 2016 C. N. Lohr, MOSTLY Under MIT/x11 License. +//<>< (C) 2016-2017 C. N. Lohr, MOSTLY Under MIT/x11 License. // //Based off of https://github.com/collabora/OSVR-Vive-Libre // Originally Copyright 2016 Philipp Zabel @@ -27,8 +27,6 @@ //XXX TODO This one needs to be rewritten. #define SV_KILL() exit(0) -#define INTBUFFSIZE 64 -#define SENSORS_PER_OBJECT 32 struct SurviveContext; struct SurviveUSBInterface; @@ -36,7 +34,6 @@ struct SurviveUSBInterface; typedef int (*DeviceDriverCb)( struct SurviveContext * ctx, void * driver ); typedef int (*DeviceDriverMagicCb)( struct SurviveContext * ctx, void * driver, int magic_code, void * data, int datalen ); - //This is defined in survive.h struct SurviveObject; struct SurviveCalData; diff --git a/src/survive_vive.c b/src/survive_vive.c index a402153..47886c9 100644 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -20,6 +20,7 @@ #include <unistd.h> //sleep if I ever use it. #include <errno.h> #include <string.h> +#include <sys/stat.h> struct SurviveViveData; @@ -976,6 +977,27 @@ static int LoadConfig( struct SurviveViveData * sv, struct SurviveObject * so, i //TODO: Cleanup any remaining USB stuff. return 1; } + + char fname[20]; + mkdir( "calinfo", 0755 ); + + sprintf( fname, "calinfo/%s_points.csv", so->codename ); + FILE * f = fopen( fname, "w" ); + int j; + for( j = 0; j < so->nr_locations; j++ ) + { + fprintf( f, "%f %f %f\n", so->sensor_locations[j*3+0], so->sensor_locations[j*3+1], so->sensor_locations[j*3+2] ); + } + fclose( f ); + + sprintf( fname, "calinfo/%s_normals.csv", so->codename ); + f = fopen( fname, "w" ); + for( j = 0; j < so->nr_locations; j++ ) + { + fprintf( f, "%f %f %f\n", so->sensor_normals[j*3+0], so->sensor_normals[j*3+1], so->sensor_normals[j*3+2] ); + } + fclose( f ); + return 0; } diff --git a/tools/plot_lighthouse/Makefile b/tools/plot_lighthouse/Makefile index db382ea..38eece0 100644 --- a/tools/plot_lighthouse/Makefile +++ b/tools/plot_lighthouse/Makefile @@ -6,7 +6,7 @@ endif # Darwin is Mac OSX !! ifeq ($(UNAME), Darwin) -CFLAGS:= -w -framework OpenGL -framework GLUT +CFLAGS:= -I../../redist -w -framework OpenGL -framework GLUT endif all: |