From d2468fcc0bd63e163a7a0564fa37b7880e2209df Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 21 Mar 2018 09:23:36 -0600 Subject: Fixed nullptr issue in poser_sba --- redist/linmath.c | 3 +++ redist/linmath.h | 3 +++ src/poser_sba.c | 11 +++++++++-- 3 files changed, 15 insertions(+), 2 deletions(-) diff --git a/redist/linmath.c b/redist/linmath.c index 16f771e..d005074 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -653,3 +653,6 @@ void InvertPose(LinmathPose *poseout, const LinmathPose *pose) { quatrotatevector(poseout->Pos, poseout->Rot, pose->Pos); scale3d(poseout->Pos, poseout->Pos, -1); } + +LinmathQuat LinmathQuat_Identity = {1.0}; +LinmathPose LinmathPose_Identity = {.Rot = {1.0}}; diff --git a/redist/linmath.h b/redist/linmath.h index 782a13d..cacb1c6 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -56,6 +56,9 @@ typedef struct LinmathPose { LinmathQuat Rot; } LinmathPose; +extern LinmathQuat LinmathQuat_Identity; +extern LinmathPose LinmathPose_Identity; + //NOTE: Inputs may never be output with cross product. void cross3d( FLT * out, const FLT * a, const FLT * b ); diff --git a/src/poser_sba.c b/src/poser_sba.c index 870f541..1e62035 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -104,7 +104,10 @@ size_t construct_input_from_scene(const SurviveObject *so, PoserDataLight *pdl, void sba_set_cameras(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose, SurvivePose *obj_pose, void *user) { sba_context *ctx = (sba_context *)user; ctx->camera_params[lighthouse] = *pose; - ctx->obj_pose = *obj_pose; + if (obj_pose) + ctx->obj_pose = *obj_pose; + else + ctx->obj_pose = LinmathPose_Identity; } typedef struct { @@ -422,6 +425,9 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd SurvivePose additionalTx = {}; PoserData_lighthouse_pose_func(&pdfs->hdr, so, 0, &additionalTx, &sbactx.camera_params[0], &sbactx.obj_pose); PoserData_lighthouse_pose_func(&pdfs->hdr, so, 1, &additionalTx, &sbactx.camera_params[1], &sbactx.obj_pose); + } else { + SurviveContext *ctx = so->ctx; + SV_INFO("SBA was unable to run %d", status); } // Docs say info[0] should be divided by meas; I don't buy it really... // std::cerr << info[0] / meas.size() * 2 << " original reproj error" << std::endl; @@ -455,9 +461,10 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { return 0; } case POSERDATA_FULL_SCENE: { + SurviveContext *ctx = so->ctx; PoserDataFullScene *pdfs = (PoserDataFullScene *)(pd); survive_calibration_config config = *survive_calibration_default_config(); - // std::cerr << "Running sba with " << config << std::endl; + SV_INFO("Running sba with %lu", survive_calibration_config_index(&config)); double error = run_sba(config, pdfs, so, 50, .005); // std::cerr << "Average reproj error: " << error << std::endl; return 0; -- cgit v1.2.3