aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--Makefile7
-rwxr-xr-xdave/AffineSolvebin65416 -> 65424 bytes
-rw-r--r--dave/AffineSolve.c2
-rw-r--r--dave/ptinfo.csv157
-rw-r--r--include/libsurvive/survive.h5
-rw-r--r--redist/dclapack.h (renamed from dave/dclapack.h)0
-rw-r--r--redist/linmath.h1
-rw-r--r--redist/lintest.c3
-rw-r--r--src/poser_daveortho.c457
-rw-r--r--src/survive_cal.c150
-rw-r--r--src/survive_cal.h4
11 files changed, 680 insertions, 106 deletions
diff --git a/Makefile b/Makefile
index ec5d39b..14d33bb 100644
--- a/Makefile
+++ b/Makefile
@@ -5,11 +5,10 @@ CC:=gcc
CFLAGS:=-Iinclude/libsurvive -I. -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE -std=gnu99
LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -lm -flto -g
-CALS:=src/survive_cal_lhfind.o src/survive_cal.o
-POSERS:=src/poser_dummy.o
+POSERS:=src/poser_dummy.o src/poser_daveortho.o
REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o
-LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_vive.o src/survive_config.o
-LIBSURVIVE_O:=$(CALS) $(POSERS) $(REDISTS) $(LIBSURVIVE_CORE)
+LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_vive.o src/survive_config.o src/survive_cal.o
+LIBSURVIVE_O:=$(POSERS) $(REDISTS) $(LIBSURVIVE_CORE)
LIBSURVIVE_C:=$(LIBSURVIVE_O:.o=.c)
GRAPHICS_LOFI:=redist/DrawFunctions.o redist/XDriver.o
diff --git a/dave/AffineSolve b/dave/AffineSolve
index cc4d26e..98a9590 100755
--- a/dave/AffineSolve
+++ b/dave/AffineSolve
Binary files differ
diff --git a/dave/AffineSolve.c b/dave/AffineSolve.c
index 1c873d9..4fba56b 100644
--- a/dave/AffineSolve.c
+++ b/dave/AffineSolve.c
@@ -11,7 +11,7 @@
#include <math.h>
#include "dclapack.h"
#include <linmath.h>
-#define LH_ID 1
+#define LH_ID 0
#define NUM_HMD 32
#define MAX_POINTS 128
diff --git a/dave/ptinfo.csv b/dave/ptinfo.csv
index 2dbef65..f4c5ea9 100644
--- a/dave/ptinfo.csv
+++ b/dave/ptinfo.csv
@@ -1,88 +1,69 @@
-0 0 0 1024 0.054304 8.087056 7.006970 0.000005 0.000095 49.911564
-0 0 1 1024 -0.091073 7.633667 2.124995 0.000002 0.000032 28.309638
-4 0 0 1024 0.046903 7.663818 9.134277 0.000014 0.000274 87.982354
-4 0 1 1024 -0.086459 8.770345 2.726730 0.000011 0.000192 29.506709
-5 0 0 1024 0.047980 4.033020 8.087671 0.000017 0.000218 50.499310
-5 0 1 1024 -0.077373 7.318115 2.557700 0.000005 0.000082 24.065775
-6 0 0 1024 0.062184 9.521423 7.429031 0.000003 0.000035 38.676732
-6 0 1 1024 -0.073973 9.523905 1.995981 0.000002 0.000029 26.113657
-7 0 0 1024 0.056254 9.999064 7.497290 0.000003 0.000044 38.734072
-7 0 1 1024 -0.079104 9.753560 2.103774 0.000004 0.000051 33.619688
-8 1 0 1024 -0.006291 9.665955 1.987640 0.000002 0.000028 15.453922
-8 1 1 1024 -0.117330 10.177124 1.600708 0.000001 0.000018 11.038096
-9 0 0 1025 0.062143 8.118984 6.820550 0.000004 0.000055 40.521475
-9 0 1 1023 -0.085365 8.509694 1.896965 0.000003 0.000091 21719098.365862
-9 1 0 1024 0.000557 6.315043 1.998499 0.000010 0.000087 21.686142
-9 1 1 1024 -0.113388 7.437968 1.836344 0.000002 0.000035 18.467548
-10 1 0 1024 -0.009943 9.343363 1.995316 0.000002 0.000031 17.584078
-10 1 1 1024 -0.124325 9.785502 1.689753 0.000002 0.000038 11.692163
-11 1 1 1024 -0.118290 6.393616 2.064949 0.000007 0.000127 19.804200
-12 1 0 1024 -0.014712 6.424642 2.678035 0.000011 0.000116 31.494265
-12 1 1 1024 -0.109233 7.731120 2.279953 0.000007 0.000190 18.323990
-15 0 0 1024 0.060565 6.858704 7.184426 0.000008 0.000088 44.052520
-15 0 1 1024 -0.094258 6.385498 2.018832 0.000010 0.000091 29.446135
-15 1 0 1024 -0.006425 3.219747 2.549749 0.000022 0.000239 26.656846
-15 1 1 1024 -0.106455 6.234049 1.960842 0.000006 0.000076 13.899017
-17 0 0 1024 0.066623 8.772827 6.714717 0.000003 0.000034 33.014899
-17 0 1 1024 -0.060226 8.462301 2.015937 0.000002 0.000022 32.443223
-23 1 1 1024 -0.155699 4.259481 2.133325 0.000009 0.000119 14.051344
-24 1 0 1024 0.004956 2.748250 3.060253 0.000032 0.000273 24.108629
-24 1 1 1024 -0.156870 7.030355 1.539771 0.000003 0.000047 11.937263
-27 1 0 1024 -0.004080 5.997660 2.117311 0.000010 0.000096 25.358188
-27 1 1 1024 -0.157007 8.123576 1.820049 0.000003 0.000063 11.321934
-28 1 0 1024 -0.007485 3.633423 2.815398 0.000022 0.000202 29.154675
-28 1 1 1024 -0.148765 6.027649 1.957405 0.000006 0.000081 13.776313
-29 1 0 1024 -0.006364 9.275757 2.122339 0.000002 0.000026 16.821591
-29 1 1 1024 -0.140631 9.910583 1.611250 0.000002 0.000023 13.197373
-30 0 0 1024 0.072943 3.733805 6.264887 0.000006 0.000071 35.965622
-30 0 1 1024 -0.052506 4.387878 3.479865 0.000009 0.000093 44.349223
-30 1 0 1024 0.008406 5.021810 2.308727 0.000008 0.000085 15.880164
-30 1 1 1024 -0.147555 8.227193 1.637723 0.000002 0.000022 13.866484
-31 1 0 1024 0.000033 9.308573 1.886001 0.000002 0.000017 19.936102
-31 1 1 1024 -0.145700 9.870667 1.610662 0.000001 0.000017 14.105948
-32 1 0 1016 -0.101837 5.457534 2.442544 0.000008 0.000085 16.869970
-32 1 1 1018 -0.100882 6.643766 1.484100 0.000002 0.000027 17.016439
-33 1 0 1019 -0.107913 5.854473 2.030354 0.000008 0.000111 17.656553
-33 1 1 1018 -0.104750 6.514285 1.579995 0.000002 0.000026 18.046024
-34 1 0 1020 -0.108257 6.882782 1.880494 0.000003 0.000038 14.125138
-34 1 1 1018 -0.110099 6.917526 1.892384 0.000002 0.000016 13.833723
-35 1 0 1022 -0.121796 8.334801 1.807161 0.000002 0.000028 14.423969
-35 1 1 1018 -0.107517 7.105231 1.554912 0.000002 0.000022 15.379143
-36 1 0 1021 -0.111978 8.395752 1.902573 0.000002 0.000021 12.684395
-36 1 1 1018 -0.103989 7.755075 1.513899 0.000002 0.000017 15.735186
-37 1 0 1022 -0.119498 8.144121 2.128056 0.000001 0.000016 11.799663
-37 1 1 1018 -0.086472 6.760928 1.440899 0.000002 0.000032 16.451576
-38 1 0 1022 -0.126362 8.366418 1.890943 0.000002 0.000024 15.547028
-38 1 1 1018 -0.099977 7.628377 1.647596 0.000003 0.000027 13.428394
-39 1 0 1022 -0.128867 8.546498 1.952486 0.000002 0.000020 13.761684
-39 1 1 1018 -0.091108 8.340987 1.392215 0.000002 0.000030 15.791367
-41 1 0 858 -0.117675 8.663534 2.024164 0.000002 0.000016 13.786543
-41 1 1 852 -0.083109 7.402558 1.346682 0.000001 0.000017 14.923250
-42 1 0 1020 -0.110949 8.397712 2.080674 0.000002 0.000028 16.436445
-42 1 1 1017 -0.080808 7.596423 1.468899 0.000002 0.000024 14.864047
-44 0 0 1021 0.098696 8.919217 4.402947 0.000003 0.000022 29.326257
-44 0 1 1023 -0.134452 8.670434 2.040941 0.000002 0.000027 25.185552
-54 0 0 1021 0.095352 9.054522 4.727093 0.000004 0.000049 27.342838
-54 0 1 1023 -0.158791 8.836144 2.087028 0.000003 0.000027 22.937738
-68 0 0 912 -0.126928 4.094275 2.984245 0.000000 0.000005 31.298089
-68 0 1 916 0.529440 2.825646 2.494563 0.000008 0.000066 26.030924
-68 1 0 916 0.282512 3.705604 1.662663 0.000004 0.000042 16.517113
-68 1 1 916 -0.382934 2.543668 1.671447 0.000012 0.000142 16.902817
-70 1 0 919 0.273220 2.914037 1.975858 0.000007 0.000072 16.422591
-70 1 1 911 -0.380586 2.538534 2.661676 0.000018 0.000189 21.306309
-71 1 0 927 0.270648 4.495415 1.832304 0.000003 0.000023 15.326381
-71 1 1 911 -0.374629 3.509147 1.960478 0.000007 0.000067 14.023731
-72 0 0 922 -0.139030 4.265365 3.092647 0.000001 0.000008 36.803851
-72 0 1 937 0.507338 4.919313 2.656878 0.000005 0.000087 28.489847
-72 1 0 918 0.273990 4.384191 1.657099 0.000003 0.000025 16.250508
-72 1 1 901 -0.365903 4.259850 1.754957 0.000004 0.000068 12.246038
-73 0 0 909 -0.131142 4.841928 3.053060 0.000001 0.000013 35.932353
-73 0 1 939 0.512391 5.133209 2.178137 0.000004 0.000025 26.796286
-73 1 0 915 0.278769 2.556967 1.691021 0.000006 0.000050 15.318204
-74 0 0 919 -0.135500 5.774937 2.990444 0.000000 0.000004 38.680779
-74 0 1 919 0.517127 8.180404 2.131625 0.000001 0.000012 24.272923
-74 1 0 916 0.282418 1.150973 1.692552 0.000023 0.000215 15.138841
-76 0 0 916 -0.136047 5.623431 3.027347 0.000001 0.000015 40.599860
-76 0 1 913 0.537758 7.682777 2.005741 0.000001 0.000008 23.670972
-86 0 0 925 -0.141096 6.131959 3.056302 0.000001 0.000009 37.086914
-86 0 1 929 0.512093 8.391079 2.034693 0.000001 0.000008 26.888878
+0 0 0 1024 0.041750 3.356344 1.828998 0.000002 0.000021 49.839622
+0 0 1 1024 0.109967 4.153035 1.883659 0.000002 0.000017 18.750043
+0 1 1 838 -0.207362 0.514121 2.011177 0.000005 0.000146 20.057562
+4 1 0 1024 0.131400 2.764364 2.038156 0.000003 0.000066 21.627449
+4 1 1 1024 -0.201195 2.843648 1.844738 0.000003 0.000079 20.542771
+6 0 0 1024 0.023004 4.443075 1.777077 0.000001 0.000026 47.120957
+6 0 1 1024 0.113830 5.225403 1.653571 0.000001 0.000022 20.887187
+6 1 0 1024 0.146652 2.742981 2.012739 0.000001 0.000012 21.375524
+6 1 1 1024 -0.211721 2.358521 1.862361 0.000001 0.000015 20.053232
+7 1 0 1024 0.140333 3.198140 1.997439 0.000001 0.000018 18.166517
+7 1 1 1024 -0.208156 2.908244 1.892947 0.000001 0.000013 17.755360
+8 0 0 1024 0.036998 5.118429 1.829670 0.000001 0.000012 39.822422
+8 0 1 1024 0.126802 6.585714 1.734635 0.000002 0.000021 20.020662
+9 0 0 1024 0.035700 4.496134 1.846496 0.000001 0.000012 45.663926
+9 0 1 1024 0.115987 5.774577 1.682445 0.000002 0.000027 19.873340
+10 0 0 1024 0.032992 4.960815 1.840030 0.000002 0.000034 49.867023
+10 0 1 1024 0.134889 6.200765 1.801494 0.000004 0.000056 19.108826
+15 0 0 1024 0.046206 3.663981 1.957251 0.000001 0.000022 51.132112
+15 0 1 1024 0.119199 4.899963 1.761027 0.000002 0.000060 24.645105
+16 1 0 1024 0.167758 2.818949 2.055596 0.000001 0.000010 19.765362
+16 1 1 1024 -0.210253 2.650065 1.684485 0.000001 0.000010 20.046716
+17 0 0 1024 0.008123 4.042155 1.907654 0.000001 0.000016 44.942365
+17 0 1 1024 0.119126 5.346334 1.616305 0.000002 0.000038 23.873406
+17 1 0 1024 0.160551 2.538859 2.078290 0.000001 0.000011 18.503004
+17 1 1 1024 -0.212818 1.963074 1.653989 0.000001 0.000010 20.818517
+18 1 0 1024 0.169252 1.834696 2.095149 0.000003 0.000073 20.457114
+18 1 1 1024 -0.202781 1.857198 1.804126 0.000003 0.000083 25.433970
+19 1 0 1024 0.177362 2.038411 2.536636 0.000008 0.000194 22.828018
+19 1 1 1024 -0.204779 2.259420 2.005833 0.000009 0.000221 27.081790
+23 0 0 1025 -0.010332 3.968150 1.862948 0.000004 0.000068 45.777934
+23 0 1 1023 0.128696 6.093495 1.773672 0.000004 0.004550 19300251.477888
+24 0 0 993 -0.006030 4.369734 1.921640 0.000005 0.000129 48.222013
+24 0 1 992 0.138082 6.460706 1.786190 0.000005 0.000155 22.781613
+27 0 1 1024 0.147109 2.936829 2.604852 0.000022 0.000474 28.721504
+29 0 0 1024 0.016187 5.032145 1.921979 0.000003 0.000047 47.194243
+29 0 1 1024 0.140979 6.596395 1.815706 0.000005 0.000090 24.794637
+30 0 0 1024 0.000282 4.738342 1.756708 0.000001 0.000040 47.044401
+30 0 1 1024 0.128872 6.677734 1.748380 0.000002 0.000039 19.083389
+31 0 0 1024 0.007667 5.235433 1.898046 0.000001 0.000019 47.785930
+31 0 1 1024 0.137390 6.697835 1.762878 0.000002 0.000024 20.161993
+32 0 0 1019 -0.221131 4.109073 1.687081 0.000001 0.000008 39.847161
+32 0 1 1022 0.145009 4.683627 1.672801 0.000001 0.000004 18.169256
+33 0 0 1020 -0.217002 4.494730 1.552900 0.000001 0.000006 38.840936
+33 0 1 1021 0.150320 4.659607 1.476978 0.000001 0.000006 20.903208
+34 0 0 1019 -0.220660 4.394218 1.499063 0.000001 0.000007 36.572525
+34 0 1 1020 0.155998 4.846773 1.572007 0.000001 0.000008 20.786276
+35 0 0 1020 -0.207471 4.282680 1.416071 0.000000 0.000006 37.591605
+35 0 1 1020 0.158539 3.864747 1.512890 0.000001 0.000005 19.809991
+36 0 0 1020 -0.213648 1.838133 1.606175 0.000001 0.000014 42.311387
+36 0 1 1020 0.151255 1.933946 1.611633 0.000001 0.000010 20.868507
+36 1 0 1017 0.357442 1.409087 2.339893 0.000001 0.000009 25.252360
+37 0 0 1019 -0.199851 4.357601 1.549965 0.000001 0.000006 41.234504
+37 0 1 1022 0.139227 4.483753 1.584095 0.000001 0.000008 19.873120
+38 0 0 1019 -0.200025 2.547739 1.691084 0.000001 0.000006 40.679205
+38 0 1 1020 0.153593 3.013930 1.747689 0.000001 0.000006 18.072496
+39 0 1 1022 0.147554 2.405292 1.636333 0.000001 0.000009 16.334833
+40 0 1 1022 0.134857 1.219056 1.504621 0.000004 0.000039 18.954468
+40 1 0 1020 0.341554 2.105331 2.496244 0.000000 0.000007 25.552542
+40 1 1 1020 -0.141616 2.311785 2.012586 0.000001 0.000007 22.408698
+41 1 0 1020 0.344930 2.430515 2.448566 0.000000 0.000003 27.764761
+41 1 1 1021 -0.149021 2.758386 1.892542 0.000000 0.000006 25.399428
+42 0 0 1020 -0.205661 1.525102 1.704018 0.000002 0.000045 38.874329
+42 0 1 1022 0.131522 1.181813 1.789474 0.000003 0.000023 19.790870
+42 1 0 1018 0.349915 3.181974 2.424394 0.000000 0.000005 27.068381
+42 1 1 1021 -0.147299 2.959639 1.892314 0.000000 0.000004 23.454554
+54 1 0 1019 0.346022 2.557082 2.295552 0.000000 0.000006 25.170448
+54 1 1 1020 -0.141488 2.866462 2.025173 0.000001 0.000006 25.737504
+55 1 0 1018 0.350800 1.716110 2.326650 0.000001 0.000013 26.234609
+55 1 1 1021 -0.145191 0.853330 1.730182 0.000001 0.000021 26.866658
diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index b349ff1..5f2d711 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -26,8 +26,8 @@ struct SurviveObject
//Pose Information, also "poser" field.
FLT PoseConfidence; //0..1
- SurvivePose OutPose;
- SurvivePose FromLHPose[NUM_LIGHTHOUSES]; //Optionally filled out by poser, contains computed position from each lighthouse.
+ SurvivePose OutPose; //Final pose? (some day, one can dream!)
+ SurvivePose FromLHPose[NUM_LIGHTHOUSES]; //Filled out by poser, contains computed position from each lighthouse.
void * PoserData; //Initialized to zero, configured by poser, can be anything the poser wants.
PoserCB PoserFn;
@@ -92,7 +92,6 @@ struct SurviveContext
//Calibration data:
BaseStationData bsd[NUM_LIGHTHOUSES];
-
SurviveCalData * calptr; //If and only if the calibration subsystem is attached.
SurviveObject ** objs;
diff --git a/dave/dclapack.h b/redist/dclapack.h
index 4e209d3..4e209d3 100644
--- a/dave/dclapack.h
+++ b/redist/dclapack.h
diff --git a/redist/linmath.h b/redist/linmath.h
index a58b2bf..aeaca46 100644
--- a/redist/linmath.h
+++ b/redist/linmath.h
@@ -92,6 +92,7 @@ void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in );
void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest);
//Poses are Position: [x, y, z] Quaternion: [q, x, y, z]
+//XXX TODO Write these!
void ApplyPoseToPoint( FLT * pout, const FLT * pin, const FLT * pose );
void InvertPose( FLT * poseout, const FLT * pose );
diff --git a/redist/lintest.c b/redist/lintest.c
index 377824e..a767670 100644
--- a/redist/lintest.c
+++ b/redist/lintest.c
@@ -31,8 +31,7 @@ int main()
printf( "E: %f %f %f\n", e[0], e[1], e[2] );
- FLT p[3] = { 0, 0, 1 };
- printf( "%f %f %f\n", PFTHREE( p ) );
+ FLT pfromlh[3] = { 0, 1, 0 };
quatrotatevector( p, q, p );
printf( "%f %f %f\n", PFTHREE( p ) );
printf( "Flipping rotation\n" );
diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c
new file mode 100644
index 0000000..80ddd90
--- /dev/null
+++ b/src/poser_daveortho.c
@@ -0,0 +1,457 @@
+#include "survive_cal.h"
+#include <math.h>
+#include <string.h>
+#include "linmath.h"
+#include <survive.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <dclapack.h>
+#include <linmath.h>
+
+static int LH_ID;
+
+void OrthoSolve(
+ FLT T[4][4], // OUTPUT: 4x4 transformation matrix
+ FLT S_out[2][SENSORS_PER_OBJECT], // OUTPUT: array of screenspace points
+ FLT S_in[2][SENSORS_PER_OBJECT], // INPUT: array of screenspace points
+ FLT X_in[3][SENSORS_PER_OBJECT], // INPUT: array of offsets
+ int nPoints);
+
+
+typedef struct
+{
+ int something;
+ //Stuff
+} DummyData;
+
+int PoserDaveOrtho( SurviveObject * so, PoserData * pd )
+{
+ PoserType pt = pd->pt;
+ SurviveContext * ctx = so->ctx;
+ DummyData * dd = so->PoserData;
+
+ if( !dd ) so->PoserData = dd = malloc( sizeof( DummyData ) );
+
+ switch( pt )
+ {
+ case POSERDATA_IMU:
+ {
+ PoserDataIMU * imu = (PoserDataIMU*)pd;
+ //printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, imu->accel[0], imu->accel[1], imu->accel[2], imu->gyro[0], imu->gyro[1], imu->gyro[2] );
+ break;
+ }
+ case POSERDATA_LIGHT:
+ {
+ PoserDataLight * l = (PoserDataLight*)pd;
+ //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode );
+ break;
+ }
+ case POSERDATA_FULL_SCENE:
+ {
+ PoserDataFullScene * fs = (PoserDataFullScene*)pd;
+
+ for( LH_ID = 0; LH_ID < 2; LH_ID++ )
+ {
+ int i;
+ int max_hits = 0;
+ FLT S_in[2][SENSORS_PER_OBJECT];
+ FLT X_in[3][SENSORS_PER_OBJECT];
+ for( i = 0; i < SENSORS_PER_OBJECT; i++ )
+ {
+ //Load all our valid points into something the LHFinder can use.
+ if( fs->lengths[i][LH_ID][0] > 0 )
+ {
+ S_in[0][max_hits] = fs->angles[i][LH_ID][0];
+ S_in[1][max_hits] = fs->angles[i][LH_ID][1];
+ X_in[0][max_hits] = so->sensor_locations[i*3+0];
+ X_in[1][max_hits] = so->sensor_locations[i*3+1];
+ X_in[2][max_hits] = so->sensor_locations[i*3+2];
+ max_hits++;
+ }
+
+ }
+ FLT tOut[4][4];
+ FLT S_out[2][SENSORS_PER_OBJECT];
+ OrthoSolve( tOut, S_out, S_in, X_in, max_hits );
+
+ //Now, we need to solve where we are as a function of where
+ //the lighthouses are.
+ FLT quat[4];
+ FLT posoff[3] = { tOut[0][3], tOut[1][3], tOut[2][3] };
+ FLT MT[4][4];
+ //matrix44transpose( MT, &T[0][0] );
+ matrix44copy( &MT[0][0], &tOut[0][0] );
+
+ 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 [%f %f %f]\n", quat[0], quat[1], quat[2], quat[3], quatmagnitude(quat), posoff[0], posoff[1], posoff[2] );
+
+ for( i = 0; i < max_hits;i++ )
+ {
+ FLT pt[3] = { X_in[0][i], X_in[1][i], X_in[2][i] };
+ quatrotatevector( pt, quat, pt );
+ add3d( pt, pt, posoff );
+ printf( "OUT %f %f %f ANGLE %f %f AOUT %f %f\n",
+ pt[0], pt[1], pt[2],
+ S_in[0][i], S_in[1][i], atan2( pt[0], pt[1] ), atan2( pt[2], pt[1] ) );
+ }
+
+ so->FromLHPose[LH_ID].Pos[0] = posoff[0];
+ so->FromLHPose[LH_ID].Pos[1] = posoff[1];
+ so->FromLHPose[LH_ID].Pos[2] = posoff[2];
+ so->FromLHPose[LH_ID].Rot[0] = quat[0];
+ so->FromLHPose[LH_ID].Rot[1] = quat[1];
+ so->FromLHPose[LH_ID].Rot[2] = quat[2];
+ so->FromLHPose[LH_ID].Rot[3] = quat[3];
+ }
+
+ break;
+ }
+ case POSERDATA_DISASSOCIATE:
+ {
+ free( dd );
+ so->PoserData = 0;
+ //printf( "Need to disassociate.\n" );
+ break;
+ }
+ }
+
+}
+
+
+REGISTER_LINKTIME( PoserDaveOrtho );
+
+
+
+
+
+#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"); \
+ } \
+}
+
+#define CrossProduct(ox,oy,oz,a,b,c,x,y,z) { \
+ ox=(b)*(z)-(c)*(y); \
+ oy=(c)*(x)-(a)*(z); \
+ oz=(a)*(y)-(b)*(x); }
+
+void OrthoSolve(
+ FLT T[4][4], // OUTPUT: 4x4 transformation matrix
+ FLT S_out[2][SENSORS_PER_OBJECT], // OUTPUT: array of screenspace points
+ FLT S_in[2][SENSORS_PER_OBJECT], // INPUT: array of screenspace points
+ FLT X_in[3][SENSORS_PER_OBJECT], // INPUT: array of offsets
+ int nPoints)
+{
+ int i,j,k;
+ FLT R[3][3]; // OUTPUT: 3x3 rotation matrix
+ FLT trans[3]; // INPUT: x,y,z translation vector
+
+ //--------------------
+ // Remove the center of the HMD offsets, and the screen space
+ //--------------------
+ FLT xbar[3] = {0.0, 0.0, 0.0};
+ FLT sbar[2] = {0.0, 0.0};
+ FLT S[2][SENSORS_PER_OBJECT];
+ FLT X[3][SENSORS_PER_OBJECT];
+ FLT inv_nPoints = 1.0 / nPoints;
+ for (i=0; i<nPoints; i++) {
+ xbar[0] += X_in[0][i];
+ xbar[1] += X_in[1][i];
+ xbar[2] += X_in[2][i];
+ sbar[0] += S_in[0][i];
+ sbar[1] += S_in[1][i];
+ }
+ for (j=0; j<3; j++) { xbar[j] *= inv_nPoints; }
+ for (j=0; j<2; j++) { sbar[j] *= inv_nPoints; }
+ for (i=0; i<nPoints; i++) {
+ X[0][i] = X_in[0][i] - xbar[0];
+ X[1][i] = X_in[1][i] - xbar[1];
+ X[2][i] = X_in[2][i] - xbar[2];
+ S[0][i] = S_in[0][i] - sbar[0];
+ S[1][i] = S_in[1][i] - sbar[1];
+ }
+
+ //--------------------
+ // Solve for the morph matrix
+ // S = M X
+ // thus
+ // (SX^t)(XX^t)^-1 = M
+ //--------------------
+ FLT Xt[SENSORS_PER_OBJECT][3];
+ FLT XXt[3][3];
+ FLT invXXt[3][3];
+ FLT SXt[2][3];
+ FLT M[2][3]; // Morph matrix! (2 by 3)
+ TRANSP(X,Xt,3,nPoints);
+ MUL(X,Xt,XXt,3,nPoints,3);
+ MUL(S,Xt,SXt,2,nPoints,3);
+ INV(XXt,invXXt,3);
+ MUL(SXt,invXXt,M,2,3,3);
+//PRINT(M,2,3);
+
+// Double checking work
+FLT S_morph[2][SENSORS_PER_OBJECT];
+MUL(M,X,S_morph,2,3,nPoints);
+for (i=0; i<nPoints; i++) { S_morph[0][i]+=sbar[0]; S_morph[1][i]+=sbar[1]; }
+
+ //--------------------
+ // Solve for the non-trivial vector
+ // uf -- vector that goes into the camera
+ //--------------------
+ FLT uM[3][3] = {
+ { M[0][0], M[0][1], M[0][2] },
+ { M[1][0], M[1][1], M[1][2] },
+ { 3.14567, -1.2345, 4.32567 } }; // Morph matrix with appended row
+//PRINT(uM,3,3);
+// ToDo: Pick a number for the bottom that is NOT linearly separable with M[0] and M[1]
+ FLT B[3][1] = { {0.0}, {0.0}, {1.0} };
+ FLT inv_uM[3][3];
+ FLT uf[3][1];
+ INV(uM,inv_uM,3);
+ MUL(inv_uM,B,uf,3,3,1);
+
+ //--------------------
+ // Solve for unit length vector
+ // f that goes into the camera
+ //--------------------
+ FLT uf_len = sqrt( uf[0][0]*uf[0][0] + uf[1][0]*uf[1][0] + uf[2][0]*uf[2][0] );
+ FLT f[3][1] = { {uf[0][0]/uf_len}, {uf[1][0]/uf_len}, {uf[2][0]/uf_len} };
+//PRINT(uf,3,1);
+//PRINT(f,3,1);
+
+//FLT check[3][1];
+//MUL(uM,uf,check,3,3,1);
+//PRINT(check,3,1);
+
+ //--------------------
+ // take cross products to get vectors u,r
+ //--------------------
+ FLT u[3][1], r[3][1];
+ CrossProduct(u[0][0],u[1][0],u[2][0],f[0][0],f[1][0],f[2][0],1.0,0.0,0.0);
+ FLT inv_ulen = 1.0 / sqrt( u[0][0]*u[0][0] + u[1][0]*u[1][0] + u[2][0]*u[2][0] );
+ u[0][0]*=inv_ulen; u[1][0]*=inv_ulen; u[2][0]*=inv_ulen;
+ CrossProduct(r[0][0],r[1][0],r[2][0],f[0][0],f[1][0],f[2][0],u[0][0],u[1][0],u[2][0]);
+//PRINT(u,3,1);
+//PRINT(r,3,1);
+
+ //--------------------
+ // Use morph matrix to get screen space
+ // uhat,rhat
+ //--------------------
+ FLT uhat[2][1], rhat[2][1], fhat[2][1];
+ MUL(M,f,fhat,2,3,1);
+ MUL(M,u,uhat,2,3,1);
+ MUL(M,r,rhat,2,3,1);
+ FLT fhat_len = sqrt( fhat[0][0]*fhat[0][0] + fhat[1][0]*fhat[1][0] );
+ FLT uhat_len = sqrt( uhat[0][0]*uhat[0][0] + uhat[1][0]*uhat[1][0] );
+ FLT rhat_len = sqrt( rhat[0][0]*rhat[0][0] + rhat[1][0]*rhat[1][0] );
+ FLT urhat_len = 0.5 * (uhat_len + rhat_len);
+/*
+printf("fhat %f %f (len %f)\n", fhat[0][0], fhat[1][0], fhat_len);
+printf("uhat %f %f (len %f)\n", uhat[0][0], uhat[1][0], uhat_len);
+printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len);
+*/
+// FLT ydist1 = 1.0 / uhat_len; //0.25*PI / uhat_len;
+// FLT ydist2 = 1.0 / rhat_len; //0.25*PI / rhat_len;
+ FLT ydist = 1.0 / urhat_len;
+ //printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist);
+
+ //--------------------
+ // Rescale the axies to be of the proper length
+ //--------------------
+ FLT x[3][1] = { {M[0][0]*ydist}, {0.0}, {M[1][0]*ydist} };
+ FLT y[3][1] = { {M[0][1]*ydist}, {0.0}, {M[1][1]*ydist} };
+ FLT z[3][1] = { {M[0][2]*ydist}, {0.0}, {M[1][2]*ydist} };
+ // we know the distance into (or out of) the camera for the z axis,
+ // but we don't know which direction . . .
+ FLT x_y = sqrt(1.0 - x[0][0]*x[0][0] - x[2][0]*x[2][0]);
+ FLT y_y = sqrt(1.0 - y[0][0]*y[0][0] - y[2][0]*y[2][0]);
+ FLT 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 . . .
+ FLT bestErr = 9999.0;
+ FLT xy_dot2 = x[0][0]*y[0][0] + x[2][0]*y[2][0];
+ FLT yz_dot2 = y[0][0]*z[0][0] + y[2][0]*z[2][0];
+ FLT zx_dot2 = z[0][0]*x[0][0] + z[2][0]*x[2][0];
+ for (i=0;i<2;i++) {
+ for (j=0;j<2;j++) {
+ for(k=0;k<2;k++) {
+
+ // Calculate the error term
+ FLT xy_dot = xy_dot2 + x_y*y_y;
+ FLT yz_dot = yz_dot2 + y_y*z_y;
+ FLT zx_dot = zx_dot2 + z_y*x_y;
+ FLT err = _ABS(xy_dot) + _ABS(yz_dot) + _ABS(zx_dot);
+
+ // Calculate the handedness
+ FLT cx,cy,cz;
+ CrossProduct(cx,cy,cz,x[0][0],x_y,x[2][0],y[0][0],y_y,y[2][0]);
+ FLT hand = cx*z[0][0] + cy*z_y + cz*z[2][0];
+ 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 ( 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);
+*/
+
+ //-------------------------
+ // A test version of the rescaling to the proper length
+ //-------------------------
+ FLT ydist2 = ydist;
+ FLT bestBestErr = 9999.0;
+ FLT bestYdist = 0;
+ //for (ydist2=ydist-0.1; ydist2<ydist+0.1; ydist2+=0.0001)
+ {
+ FLT x2[3][1] = { {M[0][0]*ydist2}, {0.0}, {M[1][0]*ydist2} };
+ FLT y2[3][1] = { {M[0][1]*ydist2}, {0.0}, {M[1][1]*ydist2} };
+ FLT z2[3][1] = { {M[0][2]*ydist2}, {0.0}, {M[1][2]*ydist2} };
+
+ // we know the distance into (or out of) the camera for the z axis,
+ // but we don't know which direction . . .
+ FLT x_y = sqrt(1.0 - x2[0][0]*x2[0][0] - x2[2][0]*x2[2][0]);
+ FLT y_y = sqrt(1.0 - y2[0][0]*y2[0][0] - y2[2][0]*y2[2][0]);
+ FLT z_y = sqrt(1.0 - z2[0][0]*z2[0][0] - z2[2][0]*z2[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;
+
+ printf( "---> %f %f %f\n", x_y, y_y, z_y );
+
+ // Exhaustively flip the minus sign of the z axis until we find the right one . . .
+ FLT bestErr = 9999.0;
+ FLT xy_dot2 = x2[0][0]*y2[0][0] + x2[2][0]*y2[2][0];
+ FLT yz_dot2 = y2[0][0]*z2[0][0] + y2[2][0]*z2[2][0];
+ FLT zx_dot2 = z2[0][0]*x2[0][0] + z2[2][0]*x2[2][0];
+ for (i=0;i<2;i++) {
+ for (j=0;j<2;j++) {
+ for(k=0;k<2;k++) {
+
+ // Calculate the error term
+ FLT xy_dot = xy_dot2 + x_y*y_y;
+ FLT yz_dot = yz_dot2 + y_y*z_y;
+ FLT zx_dot = zx_dot2 + z_y*x_y;
+ FLT err = _ABS(xy_dot) + _ABS(yz_dot) + _ABS(zx_dot);
+
+ // Calculate the handedness
+ FLT cx,cy,cz;
+ CrossProduct(cx,cy,cz,x2[0][0],x_y,x2[2][0],y2[0][0],y_y,y2[2][0]);
+ FLT hand = cx*z2[0][0] + cy*z_y + cz*z2[2][0];
+ printf("err %f hand %f\n", err, hand);
+
+ // If we are the best right-handed frame so far
+ if (hand > 0 && err < bestErr) { x2[1][0]=x_y; y2[1][0]=y_y; z2[1][0]=z_y; bestErr=err; }
+ z_y = -z_y;
+ }
+ y_y = -y_y;
+ }
+ x_y = -x_y;
+ }
+ printf("ydist2 %f bestErr %f\n",ydist2,bestErr);
+
+ if (bestErr < bestBestErr) {
+ memcpy(x,x2,3*sizeof(FLT));
+ memcpy(y,y2,3*sizeof(FLT));
+ memcpy(z,z2,3*sizeof(FLT));
+ bestBestErr = bestErr;
+ bestYdist = ydist2;
+ }
+ }
+ ydist = bestYdist;
+
+/*
+ for (i=0; i<nPoints; i++) {
+ FLT x1 = x[0][0]*X[0][i] + y[0][0]*X[1][i] + z[0][0]*X[2][i];
+ FLT y1 = x[1][0]*X[0][i] + y[1][0]*X[1][i] + z[1][0]*X[2][i];
+ FLT z1 = x[2][0]*X[0][i] + y[2][0]*X[1][i] + z[2][0]*X[2][i];
+ printf("x1z1 %f %f y1 %f\n", x1, z1, y1);
+ }
+*/
+/*
+ //--------------------
+ // Combine uhat and rhat to figure out the unit x-vector
+ //--------------------
+ FLT xhat[2][1] = { {0.0}, {1.0} };
+ FLT urhat[2][2] = {
+ {uhat[0][0], uhat[1][0]},
+ {rhat[0][0], rhat[1][0]} };
+ FLT inv_urhat[2][2];
+ FLT ab[2][1];
+ INV(urhat,inv_urhat,2);
+ MUL(inv_urhat,xhat,ab,2,2,1);
+PRINT(ab,2,1);
+ FLT a = ab[0][0], b = ab[1][0];
+
+ //-------------------
+ // calculate the xyz coordinate system
+ //-------------------
+ FLT y[3][1] = { {f[0][0]}, {f[1][0]}, {f[2][0]} };
+ FLT x[3][1] = { {a*u[0][0] + b*r[0][0]}, {a*u[1][0] + b*r[1][0]}, {a*u[2][0] + b*r[2][0]} };
+ FLT inv_xlen = 1.0 / sqrt( x[0][0]*x[0][0] + x[1][0]*x[1][0] + x[2][0]*x[2][0] );
+ x[0][0]*=inv_xlen; x[1][0]*=inv_xlen; x[2][0]*=inv_xlen;
+ FLT z[3][1];
+ CrossProduct(z[0][0],z[1][0],z[2][0],x[0][0],x[1][0],x[2][0],y[0][0],y[1][0],y[2][0]);
+*/
+ // Store into the rotation matrix
+ for (i=0; i<3; i++) { R[i][0] = x[i][0]; R[i][1] = y[i][0]; R[i][2] = z[i][0]; }
+//PRINT(R,3,3);
+
+ //-------------------
+ // Calculate the translation of the centroid
+ //-------------------
+ trans[0]=tan(sbar[0]); trans[1]=1.0; trans[2]=tan(sbar[1]);
+ FLT inv_translen = ydist / sqrt( trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2] );
+ trans[0]*=inv_translen; trans[1]*=inv_translen; trans[2]*=inv_translen;
+
+ //-------------------
+ // Add in the centroid point
+ //-------------------
+ trans[0] -= xbar[0]*R[0][0] + xbar[1]*R[0][1] + xbar[2]*R[0][2];
+ trans[1] -= xbar[0]*R[1][0] + xbar[1]*R[1][1] + xbar[2]*R[1][2];
+ trans[2] -= xbar[0]*R[2][0] + xbar[1]*R[2][1] + xbar[2]*R[2][2];
+ FLT transdist = sqrt( trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2] );
+
+ //-------------------
+ // Pack into the 4x4 transformation matrix
+ //-------------------
+ T[0][0]=R[0][0]; T[0][1]=R[0][1]; T[0][2]=R[0][2]; T[0][3]=trans[0];
+ T[1][0]=R[1][0]; T[1][1]=R[1][1]; T[1][2]=R[1][2]; T[1][3]=trans[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
+ //-------------------
+ for (i=0; i<nPoints; i++) {
+ FLT 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];
+ FLT 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];
+ FLT 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] = 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]);
+ }
+
+}
+
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 06914eb..fd2f806 100644
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -14,6 +14,7 @@
#include <string.h>
#include <sys/stat.h>
#include <sys/types.h>
+#include <linmath.h>
#include "survive_config.h"
@@ -22,6 +23,9 @@
#define NEEDED_TIMES_OF_COMMON 5
#define DRPTS_NEEDED_FOR_AVG ((int)(DRPTS*3/4))
+
+
+
static void handle_calibration( struct SurviveCalData *cd );
static void reset_calibration( struct SurviveCalData * cd );
@@ -109,6 +113,23 @@ void survive_cal_install( struct SurviveContext * ctx )
return;
}
+
+
+ const char * DriverName;
+ const char * PreferredPoser = config_read_str( ctx->global_config_values, "ConfigPoser", "PoserDaveOrtho" );
+ PoserCB PreferredPoserCB = 0;
+ const char * FirstPoser = 0;
+ printf( "Available posers:\n" );
+ i = 0;
+ while( ( DriverName = GetDriverNameMatching( "Poser", i++ ) ) )
+ {
+ PoserCB p = GetDriver( DriverName );
+ if( !PreferredPoserCB ) PreferredPoserCB = p;
+ int ThisPoser = strcmp( DriverName, PreferredPoser ) == 0;
+ if( ThisPoser ) PreferredPoserCB = p;
+ }
+ cd->ConfigPoserFn = PreferredPoserCB;
+ printf( "Got config poser: %p\n", cd->ConfigPoserFn );
ootx_packet_clbk = ootx_packet_clbk_d;
ctx->calptr = cd;
@@ -201,7 +222,7 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
if( cd->peak_counts >= PTS_BEFORE_COMMON )
{
- int tfc = cd->times_found_common;
+/* int tfc = cd->times_found_common;
if( cd->found_common )
{
if( tfc >= NEEDED_TIMES_OF_COMMON )
@@ -223,7 +244,12 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
SV_INFO( "Stage 2 bad - redoing. %d %d %d", cd->peak_counts, cd->found_common, tfc );
reset_calibration( cd );
cd->times_found_common = 0;
- }
+ }*/
+
+ SV_INFO( "Stage 2 moving to stage 3. %d %d", cd->peak_counts, cd->found_common );
+ reset_calibration( cd );
+ cd->stage = 3;
+ cd->found_common = 1;
}
break;
@@ -410,8 +436,10 @@ static void handle_calibration( struct SurviveCalData *cd )
}
fclose( hists );
fclose( ptinfo );
-
+/*
//Comb through data and make sure we still have a sensor on a WM that
+ //We don't do this anymore.
+
int bcp_senid = 0;
int bcp_count = 0;
for( sen = 0; sen < MAX_SENSORS_TO_CAL; sen++ )
@@ -434,13 +462,121 @@ static void handle_calibration( struct SurviveCalData *cd )
reset_calibration( cd );
return;
}
-
cd->senid_of_checkpt = bcp_senid;
+*/
+
+ int i, j;
+ PoserDataFullScene fsd;
+ fsd.pt = POSERDATA_FULL_SCENE;
+ for( j = 0; j < NUM_LIGHTHOUSES; j++ )
+ for( i = 0; i < SENSORS_PER_OBJECT; i++ )
+ {
+ int gotdata = 0;
- if( survive_cal_lhfind( cd ) == 0 )
+ int dataindex = i*(2*NUM_LIGHTHOUSES)+j*2+0;
+
+ if( cd->ctsweeps[dataindex+0] < DRPTS_NEEDED_FOR_AVG ||
+ cd->ctsweeps[dataindex+1] < DRPTS_NEEDED_FOR_AVG )
+ {
+ fsd.lengths[i][j][0] = -1;
+ fsd.lengths[i][j][1] = -1;
+ continue;
+ }
+ fsd.lengths[i][j][0] = cd->avglens[dataindex+0];
+ fsd.lengths[i][j][1] = cd->avglens[dataindex+1];
+ fsd.angles[i][j][0] = cd->avgsweeps[dataindex+0];
+ fsd.angles[i][j][1] = cd->avgsweeps[dataindex+1];
+ }
+
+ cd->ConfigPoserFn( cd->hmd, (PoserData*)&fsd );
+ if( 1 )
{
- SV_INFO( "Stage 4 succeeded." );
- cd->stage = 5;
+ static int notfirstcal = 0;
+ SV_INFO( "Stage 4 succeeded. Inverting %d", notfirstcal );
+
+ if( !notfirstcal )
+ {
+ // XXX This part is /all/ wrong.
+ // XXX This part is /all/ wrong.
+ // XXX This part is /all/ wrong.
+
+ //OK! We've arrived. Now, we have to get the LH's pose from.
+ int lh;
+ for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ )
+ {
+ SurvivePose * objfromlh = &cd->hmd->FromLHPose[lh];
+ SurvivePose * lhp = &ctx->bsd[lh].Pose;
+
+
+ lhp->Pos[0] = objfromlh->Pos[0];
+ lhp->Pos[1] = objfromlh->Pos[1];
+ lhp->Pos[2] = objfromlh->Pos[2];
+
+ lhp->Rot[0] = objfromlh->Rot[0]*-1;
+ lhp->Rot[1] = objfromlh->Rot[1];
+ lhp->Rot[2] = objfromlh->Rot[2];
+ lhp->Rot[3] = objfromlh->Rot[3];
+
+ //Write lhp from the inverse of objfromlh
+ quatrotatevector( lhp->Pos, lhp->Rot, lhp->Pos );
+
+
+ fprintf( stderr, "%f %f %f\n", objfromlh->Pos[0], objfromlh->Pos[1], objfromlh->Pos[2] );
+ fprintf( stderr, "%f %f %f %f\n", objfromlh->Rot[0], objfromlh->Rot[1], objfromlh->Rot[2], objfromlh->Rot[3] );
+ fprintf( stderr, "%f %f %f\n", lhp->Pos[0], lhp->Pos[1], lhp->Pos[2] );
+ /*
+ -0.074179 2.793859 0.519508
+ -0.092802 0.087361 0.872115 0.472409
+ 0.379494 1.906039 2.074617
+
+ 0.695987 3.346798 -0.776169
+ -0.258207 0.966008 -0.008604 0.009116
+ 0.614156 -2.521008 2.356952
+
+ */
+
+ printf( "\n" );
+
+ }
+ notfirstcal = 1;
+ }
+ else
+ {
+ for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ )
+ {
+ SurvivePose * objfromlh = &cd->hmd->FromLHPose[lh];
+ SurvivePose * lhp = &ctx->bsd[lh].Pose;
+
+ FLT pos[3] = { objfromlh->Pos[0],
+ objfromlh->Pos[1],
+ objfromlh->Pos[2] };
+
+ FLT rot[4] = { objfromlh->Rot[0]*-1,
+ objfromlh->Rot[1],
+ objfromlh->Rot[2],
+ objfromlh->Rot[3] };
+
+ fprintf( stderr, "====> %f %f %f ",
+ pos[0], pos[1], pos[2] );
+
+ quatrotatevector( pos, rot, pos );
+ pos[0] -= lhp->Pos[0];
+ pos[1] -= lhp->Pos[1];
+ pos[2] -= lhp->Pos[2];
+
+
+ quatrotatevector( lhp->Pos, objfromlh->Rot, lhp->Pos );
+
+ fprintf( stderr, "====> %f %f %f ",
+ pos[0], pos[1], pos[2] );
+
+ }
+ }
+ fprintf( stderr, "\n" );
+
+
+ reset_calibration( cd );
+// cd->stage = 5;
}
else
{
diff --git a/src/survive_cal.h b/src/survive_cal.h
index dd2a1e2..d21ba36 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -33,7 +33,7 @@ void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timein
void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
#define MAX_SENSORS_TO_CAL 96
-#define DRPTS 1024
+#define DRPTS 128
#define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2)
struct SurviveCalData
{
@@ -61,6 +61,8 @@ struct SurviveCalData
SurviveObject * hmd;
+ PoserCB ConfigPoserFn;
+
//Stage:
// 0: Idle
// 1: Collecting OOTX data.