From 1724abef15a4090640bd82ba408681438316de7e Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 1 Apr 2018 16:04:05 -0600 Subject: Made calibration on other posers use calibration data --- .gitignore | 1 + include/libsurvive/survive_reproject.h | 14 +++++++++++--- src/poser_charlesslow.c | 28 +++++++++++++++++++--------- src/poser_daveortho.c | 17 ++++++++++------- src/poser_epnp.c | 6 +++--- src/poser_sba.c | 2 -- src/poser_turveytori.c | 18 ++++++++++++++---- src/survive_reproject.c | 6 +++--- tools/showreproject/showreproject.cc | 2 +- 9 files changed, 62 insertions(+), 32 deletions(-) diff --git a/.gitignore b/.gitignore index 8cfd2be..f39d866 100644 --- a/.gitignore +++ b/.gitignore @@ -19,6 +19,7 @@ simple_pose_test *.csv calinfo/ *.log +*.png # Windows specific *.dll diff --git a/include/libsurvive/survive_reproject.h b/include/libsurvive/survive_reproject.h index 6546e66..e4f21d0 100644 --- a/include/libsurvive/survive_reproject.h +++ b/include/libsurvive/survive_reproject.h @@ -17,8 +17,9 @@ void survive_reproject_from_pose_with_config(const SurviveContext *ctx, struct s int lighthouse, const SurvivePose *pose, FLT *point3d, FLT *out); // This is given a lighthouse -- in the same system as stored in BaseStationData, and -// a 3d point and finds what the effective 'angle' value for a given lighthouse syste +// a 3d point and finds what the effective 'angle' value for a given lighthouse system // would be. +// // While this is typically opposite of what we want to do -- we want to find the 3d // position from a 2D coordinate, this is helpful since the minimization of reprojection // error is a core mechanism to many types of solvers. @@ -26,10 +27,17 @@ void survive_reproject_from_pose_with_config(const SurviveContext *ctx, struct s void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const survive_calibration_config *config, const SurvivePose *pose, const FLT *point3d, FLT *out); -void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, struct survive_calibration_config *config, - const FLT *in, FLT *out); +// This is given input from the light sensors and approximates the idealized version of them +// by incorporating the calibration data from the lighthouse. In theory, it's an approximation +// but in practice in converges pretty quickly and to a good degree of accuracy. +// That said, all things being equal, it is better to compare reprojection to raw incoming +// data if you are looking to minimize that error. void survive_apply_bsd_calibration(SurviveContext *ctx, int lh, const FLT *in, FLT *out); +// Same as above, but lets you specify the configuration. Used internally and also in some tools +void survive_apply_bsd_calibration_by_config(SurviveContext *ctx, int lh, struct survive_calibration_config *config, + const FLT *in, FLT *out); + #ifdef __cplusplus } #endif diff --git a/src/poser_charlesslow.c b/src/poser_charlesslow.c index bc6683a..b44225e 100644 --- a/src/poser_charlesslow.c +++ b/src/poser_charlesslow.c @@ -1,12 +1,13 @@ +#include "linmath.h" #include "survive_cal.h" +#include +#include #include -#include -#include "linmath.h" -#include #include #include -#include -#include +#include +#include +#include typedef struct { @@ -255,9 +256,13 @@ static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int pr int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2; if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue; + FLT out[2] = {}; + survive_apply_bsd_calibration(hmd->ctx, lh, fs->angles[p][lh], out); + //Find out where our ray shoots forth from. - FLT ax = fs->angles[p][lh][0]; - FLT ay = fs->angles[p][lh][1]; + FLT ax = out[0]; + FLT ay = out[1]; + //NOTE: Inputs may never be output with cross product. //Create a fictitious normalized ray. Imagine the lighthouse is pointed //straight in the +z direction, this is the lighthouse ray to the point. @@ -338,8 +343,13 @@ static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int pr if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue; //Find out where our ray shoots forth from. - FLT ax = fs->angles[p][lh][0]; - FLT ay = fs->angles[p][lh][1]; + FLT out[2] = {}; + survive_apply_bsd_calibration(hmd->ctx, lh, fs->angles[p][lh], out); + + // Find out where our ray shoots forth from. + FLT ax = out[0]; + FLT ay = out[1]; + FLT RayShootOut[3] = { sin(ax), sin(ay), 0 }; RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) ); diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index c47bceb..9cdab45 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -1,12 +1,13 @@ +#include "linmath.h" #include "survive_cal.h" +#include +#include #include -#include -#include "linmath.h" -#include #include #include -#include -#include +#include +#include +#include // Dave talks about this poser here: https://www.youtube.com/watch?v=nSbEltdH9vM&feature=youtu.be&t=2h29m47s @@ -139,8 +140,10 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) //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]; + FLT out[2]; + survive_apply_bsd_calibration(ctx, LH_ID, fs->angles[i][LH_ID], out); + S_in[0][max_hits] = out[0]; + S_in[1][max_hits] = out[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]; diff --git a/src/poser_epnp.c b/src/poser_epnp.c index 2cbd9c1..c05450a 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -72,9 +72,9 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) for (size_t i = 0; i < so->sensor_ct; i++) { FLT *lengths = pdfs->lengths[i][lh]; - FLT *ang = pdfs->angles[i][lh]; - // FLT ang[2]; - // survive_apply_bsd_calibration(so->ctx, lh, _ang, ang); + FLT *_ang = pdfs->angles[i][lh]; + FLT ang[2]; + survive_apply_bsd_calibration(so->ctx, lh, _ang, ang); if (lengths[0] < 0 || lengths[1] < 0) continue; diff --git a/src/poser_sba.c b/src/poser_sba.c index 49854f2..bd7d520 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -73,8 +73,6 @@ static size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, } double *angles = pdfs->angles[sensor][lh]; - // double angles[2]; - // survive_apply_bsd_calibration(so->ctx, lh, _angles, angles); vmask[sensor * NUM_LIGHTHOUSES + lh] = 1; meas[measCount++] = angles[0]; diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 035fca7..4628207 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -13,6 +13,8 @@ #include #else #include //for alloca +#include + #endif @@ -1786,8 +1788,12 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) to->sensor[sensorCount].point.x = point[0]; to->sensor[sensorCount].point.y = point[1]; to->sensor[sensorCount].point.z = point[2]; - to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) - to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) + + FLT out[2]; + survive_apply_bsd_calibration(ctx, 0, fs->angles[i][0], out); + + to->sensor[sensorCount].theta = out[0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = out[1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) sensorCount++; } @@ -1822,8 +1828,12 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) to->sensor[sensorCount].point.x = point[0]; to->sensor[sensorCount].point.y = point[1]; to->sensor[sensorCount].point.z = point[2]; - to->sensor[sensorCount].theta = fs->angles[i][lh][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) - to->sensor[sensorCount].phi = fs->angles[i][lh][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + + FLT out[2]; + survive_apply_bsd_calibration(ctx, lh, fs->angles[i][lh], out); + + to->sensor[sensorCount].theta = out[0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = out[1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) sensorCount++; } } diff --git a/src/survive_reproject.c b/src/survive_reproject.c index 751abc0..0eaceb2 100644 --- a/src/survive_reproject.c +++ b/src/survive_reproject.c @@ -46,8 +46,8 @@ void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const surv } -void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, struct survive_calibration_config *config, - const FLT *in, FLT *out) { +void survive_apply_bsd_calibration_by_config(SurviveContext *ctx, int lh, struct survive_calibration_config *config, + const FLT *in, FLT *out) { const BaseStationCal *cal = &ctx->bsd[lh].fcal; out[0] = in[0] + config->phase_scale * cal->phase[0]; out[1] = in[1] + config->phase_scale * cal->phase[1]; @@ -95,7 +95,7 @@ survive_calibration_config survive_calibration_config_ctor() { } void survive_apply_bsd_calibration(SurviveContext *ctx, int lh, const FLT *in, FLT *out) { - survive_apply_bsd_calibration_by_flag(ctx, lh, &ctx->calibration_config, in, out); + survive_apply_bsd_calibration_by_config(ctx, lh, &ctx->calibration_config, in, out); } void survive_reproject_from_pose_with_config(const SurviveContext *ctx, struct survive_calibration_config *config, diff --git a/tools/showreproject/showreproject.cc b/tools/showreproject/showreproject.cc index 89d67c4..caa7a66 100644 --- a/tools/showreproject/showreproject.cc +++ b/tools/showreproject/showreproject.cc @@ -176,7 +176,7 @@ void drawbsds(SurviveContext *ctx) { FLT out[2]; auto config = survive_calibration_config_ctor(); config.use_flag = f; - survive_apply_bsd_calibration_by_flag(ctx, lh, &config, in, out); + survive_apply_bsd_calibration_by_config(ctx, lh, &config, in, out); double ex = out[0] - in[0]; double ey = out[1] - in[1]; if (f == SVCal_All) { -- cgit v1.2.3