diff options
author | CNLohr <charles@cnlohr.com> | 2018-03-11 21:32:49 -0400 |
---|---|---|
committer | GitHub <noreply@github.com> | 2018-03-11 21:32:49 -0400 |
commit | 9c7823c17219c659cf12eab9cc8bb2b3f68bbc5e (patch) | |
tree | 550d3faabe5a22ccd099d55290682bd0b01bc1d1 | |
parent | 6ba96f31c51cbfc8b57cfe87c129eba125bf793d (diff) | |
parent | 89f37774395530ef7267d4115f1a6242e0e9ce49 (diff) | |
download | libsurvive-9c7823c17219c659cf12eab9cc8bb2b3f68bbc5e.tar.gz libsurvive-9c7823c17219c659cf12eab9cc8bb2b3f68bbc5e.tar.bz2 |
Merge pull request #110 from cnlohr/reprojection
This adds reprojection functionality to the library
-rw-r--r-- | Makefile | 2 | ||||
-rw-r--r-- | include/libsurvive/survive_reproject.h | 53 | ||||
-rw-r--r-- | src/poser_charlesslow.c | 6 | ||||
-rwxr-xr-x | src/survive_cal.c | 45 | ||||
-rw-r--r-- | src/survive_reproject.c | 144 |
5 files changed, 249 insertions, 1 deletions
@@ -37,7 +37,7 @@ REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/os_generic. ifeq ($(UNAME), Darwin) REDISTS:=$(REDISTS) redist/hid-osx.c endif -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_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.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_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o #If you want to use HIDAPI on Linux. diff --git a/include/libsurvive/survive_reproject.h b/include/libsurvive/survive_reproject.h new file mode 100644 index 0000000..961db2a --- /dev/null +++ b/include/libsurvive/survive_reproject.h @@ -0,0 +1,53 @@ +#pragma once +#include "survive.h" +#include <stdbool.h> +#include <stdlib.h> + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct { + bool enable[2]; + bool invert[2]; + bool swap; +} survive_calibration_options_config; + +typedef struct { + + survive_calibration_options_config phase, tilt, curve, gibMag, gibPhase; + + bool gibUseSin; + +} survive_calibration_config; + +void survive_calibration_options_config_apply( + const survive_calibration_options_config *option, const FLT *input, + FLT *output); +const survive_calibration_config *survive_calibration_default_config(); + +size_t survive_calibration_config_max_idx(); + +survive_calibration_config survive_calibration_config_create_from_idx(size_t v); + +size_t survive_calibration_config_index(const survive_calibration_config *config); + +void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, + FLT *out); +void survive_reproject_from_pose(const SurviveContext *ctx, 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 +// 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. +void survive_reproject_from_pose_with_config( + const SurviveContext *ctx, const survive_calibration_config *config, + int lighthouse, const SurvivePose *pose, const FLT *point3d, FLT *out); + +#ifdef __cplusplus +} +#endif diff --git a/src/poser_charlesslow.c b/src/poser_charlesslow.c index 080ad6a..96442b3 100644 --- a/src/poser_charlesslow.c +++ b/src/poser_charlesslow.c @@ -169,6 +169,7 @@ int PoserCharlesSlow( SurviveObject * so, PoserData * pd ) copy3d( ctx->bsd[lh].Pose.Pos, LighthousePos ); quatcopy( ctx->bsd[lh].Pose.Rot, LighthouseQuat ); #define ALT_COORDS + #ifdef ALT_COORDS so->FromLHPose[lh].Pos[0] = LighthousePos[0]; so->FromLHPose[lh].Pos[1] = LighthousePos[1]; @@ -188,6 +189,11 @@ int PoserCharlesSlow( SurviveObject * so, PoserData * pd ) so->FromLHPose[lh].Rot[2] = LighthouseQuat[2]; so->FromLHPose[lh].Rot[3] = LighthouseQuat[3]; #endif + + const FLT rt[4] = {0, 0, 1, 0}; + FLT tmp[4]; + quatrotateabout(tmp, so->ctx->bsd[lh].Pose.Rot, rt); + memcpy(so->ctx->bsd[lh].Pose.Rot, tmp, sizeof(FLT) * 4); } return 0; diff --git a/src/survive_cal.c b/src/survive_cal.c index 2ed0df0..696cce4 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -10,6 +10,8 @@ #include "survive_cal.h" #include "survive_internal.h" +#include "survive_reproject.h" + #include <math.h> #include <string.h> #include <sys/stat.h> @@ -634,6 +636,8 @@ static void handle_calibration( struct SurviveCalData *cd ) return; } + int compute_reprojection_error = config_read_uint32( + ctx->global_config_values, "ComputeReprojectError", 0); int lh; for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ ) @@ -645,7 +649,48 @@ static void handle_calibration( struct SurviveCalData *cd ) fprintf( fobjp, "%f %f %f\n", objfromlh->Pos[0], objfromlh->Pos[1], objfromlh->Pos[2] ); fprintf( fobjp, "%f %f %f %f\n", objfromlh->Rot[0], objfromlh->Rot[1], objfromlh->Rot[2], objfromlh->Rot[3] ); + + if (ctx->bsd[lh].PositionSet) { + config_set_lighthouse(ctx->lh_config, &ctx->bsd[0], 0); + config_set_lighthouse(ctx->lh_config, &ctx->bsd[1], 1); + + if (compute_reprojection_error) { + FLT reproj_err = 0; + size_t cnt = 0; + SurviveObject *so = cd->poseobjects[obj]; + for (size_t idx = 0; idx < so->nr_locations; idx++) { + FLT *lengths = fsd.lengths[idx][lh]; + FLT *pt = fsd.angles[idx][lh]; + if (lengths[0] < 0 || lengths[1] < 0) + continue; + + cnt++; + FLT reproj_pt[2]; + survive_reproject(ctx, lh, so->sensor_locations, + reproj_pt); + + FLT err = 0; + for (int dim = 0; dim < 2; dim++) { + err += (reproj_pt[dim] - pt[dim]) * + (reproj_pt[dim] - pt[dim]); + } + reproj_err += sqrt(err); + } + + // This represents the average distance we were off in our + // reprojection. + // Different libraries have slightly different variations on + // this theme, + // but this one has an intuitive meaning + reproj_err = (reproj_err / cnt); + + SV_INFO("Reproject error was %.13g for lighthouse %d", + reproj_err, lh); + } + } } + + config_save(ctx, "config.json"); } fclose( fobjp ); diff --git a/src/survive_reproject.c b/src/survive_reproject.c new file mode 100644 index 0000000..7bfc7d7 --- /dev/null +++ b/src/survive_reproject.c @@ -0,0 +1,144 @@ +#include "survive_reproject.h" +#include <../redist/linmath.h> +#include <math.h> +#include <string.h> + +static void survive_calibration_options_config_normalize( + survive_calibration_options_config *option) { + if (!option->enable[0]) + option->invert[0] = false; + if (!option->enable[1]) + option->invert[1] = false; + if (!option->enable[0] && !option->enable[1]) + option->swap = false; +} + +void survive_calibration_options_config_apply( + const survive_calibration_options_config *option, const FLT *input, + FLT *output) { + FLT tmp[2]; // In case they try to do in place + for (int i = 0; i < 2; i++) { + tmp[i] = option->enable[i] * (option->invert[i] ? -1 : 1) * + input[i ^ option->swap]; + } + for (int i = 0; i < 2; i++) { + output[i] = tmp[i]; + } +} + +survive_calibration_config +survive_calibration_config_create_from_idx(size_t v) { + survive_calibration_config config; + memset(&config, 0, sizeof(config)); + + bool *_this = (bool *)&config; + + for (size_t i = 0; i < sizeof(config); i++) { + _this[i] = (bool)(v & 1); + v = v >> 1; + } + + survive_calibration_options_config_normalize(&config.phase); + survive_calibration_options_config_normalize(&config.tilt); + survive_calibration_options_config_normalize(&config.curve); + survive_calibration_options_config_normalize(&config.gibMag); + + config.gibPhase.enable[0] = config.gibMag.enable[0]; + config.gibPhase.enable[1] = config.gibMag.enable[1]; + + survive_calibration_options_config_normalize(&config.gibPhase); + + if (!config.gibPhase.enable[0] && !config.gibPhase.enable[1]) + config.gibUseSin = false; + + return config; +} + +size_t +survive_calibration_config_index(const survive_calibration_config *config) { + bool *_this = (bool *)config; + size_t v = 0; + for (size_t i = 0; i < sizeof(*config); i++) { + v = (v | _this[sizeof(*config) - i - 1]); + v = v << 1; + } + v = v >> 1; + return v; +} + +static FLT gibf(bool useSin, FLT v) { + if (useSin) + return sin(v); + return cos(v); +} + +void survive_reproject_from_pose_with_config( + const SurviveContext *ctx, const survive_calibration_config *config, + int lighthouse, const SurvivePose *pose, const FLT *pt, FLT *out) { + FLT invq[4]; + quatgetreciprocal(invq, pose->Rot); + + FLT tvec[3]; + quatrotatevector(tvec, invq, pose->Pos); + + FLT t_pt[3]; + quatrotatevector(t_pt, invq, pt); + for (int i = 0; i < 3; i++) + t_pt[i] = t_pt[i] - tvec[i]; + + FLT x = -t_pt[0] / -t_pt[2]; + FLT y = t_pt[1] / -t_pt[2]; + + double ang_x = atan(x); + double ang_y = atan(y); + + const BaseStationData *bsd = &ctx->bsd[lighthouse]; + double phase[2]; + survive_calibration_options_config_apply(&config->phase, bsd->fcalphase, + phase); + double tilt[2]; + survive_calibration_options_config_apply(&config->tilt, bsd->fcaltilt, + tilt); + double curve[2]; + survive_calibration_options_config_apply(&config->curve, bsd->fcalcurve, + curve); + double gibPhase[2]; + survive_calibration_options_config_apply(&config->gibPhase, bsd->fcalgibpha, + gibPhase); + double gibMag[2]; + survive_calibration_options_config_apply(&config->gibMag, bsd->fcalgibmag, + gibMag); + + out[0] = ang_x + phase[0] + tan(tilt[0]) * y + curve[0] * y * y + + gibf(config->gibUseSin, gibPhase[0] + ang_x) * gibMag[0]; + out[1] = ang_y + phase[1] + tan(tilt[1]) * x + curve[1] * x * x + + gibf(config->gibUseSin, gibPhase[1] + ang_y) * gibMag[1]; +} + +void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, + const SurvivePose *pose, FLT *pt, FLT *out) { + survive_reproject_from_pose_with_config( + ctx, survive_calibration_default_config(), lighthouse, pose, pt, out); +} + +void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, + FLT *out) { + survive_reproject_from_pose(ctx, lighthouse, &ctx->bsd[lighthouse].Pose, + point3d, out); +} + +const survive_calibration_config *survive_calibration_default_config() { + static survive_calibration_config *def = 0; + if (def == 0) { + def = malloc(sizeof(survive_calibration_config)); + memset(def, 0, sizeof(survive_calibration_config)); + *def = survive_calibration_config_create_from_idx(0); + } + return def; +} + +size_t survive_calibration_config_max_idx() { + survive_calibration_config cfg; + memset(&cfg, 0x1, sizeof(survive_calibration_config)); + return survive_calibration_config_index(&cfg); +} |