From 89f37774395530ef7267d4115f1a6242e0e9ce49 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 11 Mar 2018 03:04:24 -0600 Subject: Fixed survive_reproject to match the new coordinate system --- include/libsurvive/survive_reproject.h | 12 ++++++++++-- src/survive_reproject.c | 5 ++--- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/include/libsurvive/survive_reproject.h b/include/libsurvive/survive_reproject.h index 08d1e96..961db2a 100644 --- a/include/libsurvive/survive_reproject.h +++ b/include/libsurvive/survive_reproject.h @@ -27,15 +27,23 @@ void survive_calibration_options_config_apply( 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); + +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); diff --git a/src/survive_reproject.c b/src/survive_reproject.c index 8ba4c77..7bfc7d7 100644 --- a/src/survive_reproject.c +++ b/src/survive_reproject.c @@ -86,9 +86,8 @@ void survive_reproject_from_pose_with_config( for (int i = 0; i < 3; i++) t_pt[i] = t_pt[i] - tvec[i]; - // If +Z is up, this is right - FLT x = t_pt[0] / t_pt[2]; - FLT y = t_pt[1] / t_pt[2]; + 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); -- cgit v1.2.3