From 93627f6b89d22e6e2f87df0bd4d809e168958b1d Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 16 Mar 2018 22:10:01 -0600 Subject: Removed unneeded prints; fixed memory issue in sba poser --- Makefile | 2 +- redist/json_helpers.c | 2 - redist/minimal_opencv.c | 2 - src/poser_epnp.c | 2 - src/poser_sba.c | 329 ++++++++++++++++++++++++++++++++++++++++++++++++ src/poser_sba.h | 16 +++ 6 files changed, 346 insertions(+), 7 deletions(-) create mode 100644 src/poser_sba.c create mode 100644 src/poser_sba.h diff --git a/Makefile b/Makefile index 1ce5778..3e5059a 100644 --- a/Makefile +++ b/Makefile @@ -2,7 +2,7 @@ all : lib data_recorder test calibrate calibrate_client simple_pose_test CC?=gcc -CFLAGS:=-Iinclude/libsurvive -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -llapacke -lcblas -lm +CFLAGS:=-Iinclude/libsurvive -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -llapacke -lcblas -lm -fsanitize=address -fsanitize=undefined #LDFLAGS:=-L/usr/local/lib -lpthread -lusb-1.0 -lz -lm -flto -g LDFLAGS:=-L/usr/local/lib -lpthread -lz -lm -flto -g diff --git a/redist/json_helpers.c b/redist/json_helpers.c index 8704a93..ebacd2c 100644 --- a/redist/json_helpers.c +++ b/redist/json_helpers.c @@ -188,7 +188,6 @@ void json_load_file(const char* path) { if (value_t->type == JSMN_ARRAY) { i += json_load_array(JSON_STRING, tokens+i+2,value_t->size, tag); //look at array children } else if (value_t->type == JSMN_OBJECT) { - printf("Begin Object\n"); if (json_begin_object != NULL) json_begin_object(tag); children = (int16_t)(value_t->size +1); //+1 to account for this loop where we are not yed parsing children // i += decode_jsmn_object(JSON_STRING, tokens+i+2,value_t->size); @@ -200,7 +199,6 @@ void json_load_file(const char* path) { if (children>=0) children--; if (children == 0) { children = -1; - printf("End Object\n"); if (json_end_object!=NULL) json_end_object(); } diff --git a/redist/minimal_opencv.c b/redist/minimal_opencv.c index 3f7bed7..e8a6214 100644 --- a/redist/minimal_opencv.c +++ b/redist/minimal_opencv.c @@ -175,10 +175,8 @@ double cvInvert(const CvMat *srcarr, CvMat *dstarr, int method) { lapack_int *ipiv = malloc(sizeof(lapack_int) * MIN(srcarr->rows, srcarr->cols)); inf = LAPACKE_dgetrf(LAPACK_ROW_MAJOR, rows, cols, a, lda, ipiv); assert(inf == 0); - print_mat(dstarr); inf = LAPACKE_dgetri(LAPACK_ROW_MAJOR, rows, a, lda, ipiv); - print_mat(dstarr); assert(inf >= 0); if (inf > 0) { printf("Warning: Singular matrix: \n"); diff --git a/src/poser_epnp.c b/src/poser_epnp.c index f2d0062..dfe86ff 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -37,8 +37,6 @@ static SurvivePose solve_correspondence(SurviveObject *so, epnp *pnp, bool camer cvTranspose(&R, &R); // Then 'tvec = -R * tvec' cvGEMM(&R, &Tmp, -1, 0, 0, &T, 0); - print_mat(&R); - print_mat(&T); } FLT tmp[4]; diff --git a/src/poser_sba.c b/src/poser_sba.c new file mode 100644 index 0000000..125c52f --- /dev/null +++ b/src/poser_sba.c @@ -0,0 +1,329 @@ +#ifndef USE_DOUBLE +#define FLT double +#define USE_DOUBLE +#endif + +#include + +#include "poser.h" +#include + +#include "assert.h" +#include "linmath.h" +#include "string.h" +#include "survive_config.h" +#include "survive_reproject.h" + +#include "math.h" + +typedef struct { + survive_calibration_config calibration_config; + PoserData *pdfs; + SurviveObject *so; +} sba_context; + +void metric_function(int j, int i, double *aj, double *xij, void *adata) { + sba_context *ctx = (sba_context *)(adata); + SurviveObject *so = ctx->so; + + survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, j, (SurvivePose *)aj, + &so->sensor_locations[i * 3], xij); +} + +size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, char *vmask, double *meas) { + size_t measCount = 0; + size_t size = so->sensor_ct * NUM_LIGHTHOUSES; // One set per lighthouse + for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) { + for (size_t lh = 0; lh < 2; lh++) { + FLT *l = pdfs->lengths[sensor][lh]; + if (l[0] < 0 || l[1] < 0) { + vmask[sensor * NUM_LIGHTHOUSES + lh] = 0; + continue; + } + + double *angles = pdfs->angles[sensor][lh]; + vmask[sensor * NUM_LIGHTHOUSES + lh] = 1; + + meas[measCount++] = angles[0]; + meas[measCount++] = angles[1]; + } + } + return measCount; +} + +size_t construct_input_from_scene(const SurviveObject *so, PoserDataLight *pdl, SurviveSensorActivations *scene, + char *vmask, double *meas) { + size_t rtn = 0; + + for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) { + for (size_t lh = 0; lh < 2; lh++) { + if (SurviveSensorActivations_isPairValid(scene, SurviveSensorActivations_default_tolerance, pdl->timecode, + sensor, lh)) { + double *a = scene->angles[sensor][lh]; + vmask[sensor * NUM_LIGHTHOUSES + lh] = 1; + meas[rtn++] = a[0]; + meas[rtn++] = a[1]; + } else { + vmask[sensor * NUM_LIGHTHOUSES + lh] = 0; + } + } + } + return rtn; +} + +void sba_set_cameras(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose, void *user) { + SurvivePose *poses = (SurvivePose *)(user); + poses[lighthouse] = *pose; +} + +typedef struct { + bool hasInfo; + SurvivePose poses; +} sba_set_position_t; + +void sba_set_position(SurviveObject *so, uint8_t lighthouse, SurvivePose *new_pose, void *_user) { + sba_set_position_t *user = _user; + assert(user->hasInfo == false); + user->hasInfo = 1; + user->poses = *new_pose; +} +void *GetDriver(const char *name); + +void str_metric_function(int j, int i, double *bi, double *xij, void *adata) { + SurvivePose obj = *(SurvivePose *)bi; + int sensor_idx = j >> 1; + int lh = j & 1; + + sba_context *ctx = (sba_context *)(adata); + SurviveObject *so = ctx->so; + + assert(lh < 2); + assert(sensor_idx < so->sensor_ct); + + quatnormalize(obj.Rot, obj.Rot); + FLT xyz[3]; + ApplyPoseToPoint(xyz, obj.Pos, &so->sensor_locations[sensor_idx * 3]); + + // std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl; + SurvivePose *camera = &so->ctx->bsd[lh].Pose; + survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, lh, camera, xyz, xij); +} + +static double run_sba_find_3d_structure(survive_calibration_config options, PoserDataLight *pdl, SurviveObject *so, + SurviveSensorActivations *scene, int max_iterations /* = 50*/, + double max_reproj_error /* = 0.005*/) { + double *covx = 0; + + char *vmask = malloc(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES); + double *meas = malloc(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES); + size_t meas_size = construct_input_from_scene(so, pdl, scene, vmask, meas); + + static int failure_count = 500; + if (so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0 || meas_size < 7) { + if (meas_size < 7 && failure_count++ == 500) { + SurviveContext *ctx = so->ctx; + SV_INFO("Can't solve for position with just %lu measurements", meas_size); + failure_count = 0; + } + free(vmask); + free(meas); + return -1; + } + failure_count = 0; + + SurvivePose soLocation = so->OutPose; + bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]); + + { + const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); + PoserCB driver = (PoserCB)GetDriver(subposer); + SurviveContext *ctx = so->ctx; + if (driver) { + PoserData hdr = pdl->hdr; + memset(&pdl->hdr, 0, sizeof(pdl->hdr)); // Clear callback functions + pdl->hdr.pt = hdr.pt; + pdl->hdr.rawposeproc = sba_set_position; + + sba_set_position_t locations = {}; + pdl->hdr.userdata = &locations; + driver(so, &pdl->hdr); + pdl->hdr = hdr; + + if (locations.hasInfo == false) { + free(vmask); + free(meas); + + return -1; + } else if (locations.hasInfo) { + soLocation = locations.poses; + } + } else { + SV_INFO("Not using a seed poser for SBA; results will likely be way off"); + } + } + + double opts[SBA_OPTSSZ] = {}; + double info[SBA_INFOSZ] = {}; + + sba_context ctx = {options, &pdl->hdr, so}; + + opts[0] = SBA_INIT_MU; + opts[1] = SBA_STOP_THRESH; + opts[2] = SBA_STOP_THRESH; + opts[3] = SBA_STOP_THRESH; + opts[3] = SBA_STOP_THRESH; // max_reproj_error * meas.size(); + opts[4] = 0.0; + + int status = sba_str_levmar(1, // Number of 3d points + 0, // Number of 3d points to fix in spot + NUM_LIGHTHOUSES * so->sensor_ct, vmask, + soLocation.Pos, // Reads as the full pose though + 7, // pnp -- SurvivePose + meas, // x* -- measurement data + 0, // cov data + 2, // mnp -- 2 points per image + str_metric_function, + 0, // jacobia of metric_func + &ctx, // user data + max_iterations, // Max iterations + 0, // verbosity + opts, // options + info); // info + + if (status > 0) { + quatnormalize(soLocation.Rot, soLocation.Rot); + PoserData_poser_raw_pose_func(&pdl->hdr, so, 1, &soLocation); + } + + { + SurviveContext *ctx = so->ctx; + // Docs say info[0] should be divided by meas; I don't buy it really... + static int cnt = 0; + if (cnt++ > 1000 || meas_size < 8) { + SV_INFO("%f original reproj error for %lu meas", (info[0] / meas_size * 2), meas_size); + SV_INFO("%f cur reproj error", (info[1] / meas_size * 2)); + cnt = 0; + } + } + + free(vmask); + free(meas); + + return info[1] / meas_size * 2; +} + +static double run_sba(survive_calibration_config options, PoserDataFullScene *pdfs, SurviveObject *so, + int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) { + double *covx = 0; + + char *vmask = malloc(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES); + double *meas = malloc(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES); + size_t meas_size = construct_input(so, pdfs, vmask, meas); + + SurvivePose camera_params[2] = {so->ctx->bsd[0].Pose, so->ctx->bsd[1].Pose}; + + if (true || so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0) { + const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); + PoserCB driver = (PoserCB)GetDriver(subposer); + SurviveContext *ctx = so->ctx; + if (driver) { + SV_INFO("Using %s seed poser for SBA", subposer); + PoserData hdr = pdfs->hdr; + memset(&pdfs->hdr, 0, sizeof(pdfs->hdr)); // Clear callback functions + pdfs->hdr.pt = hdr.pt; + pdfs->hdr.lighthouseposeproc = sba_set_cameras; + pdfs->hdr.userdata = camera_params; + driver(so, &pdfs->hdr); + pdfs->hdr = hdr; + } else { + SV_INFO("Not using a seed poser for SBA; results will likely be way off"); + for (int i = 0; i < 2; i++) { + so->ctx->bsd[i].Pose = (SurvivePose){}; + so->ctx->bsd[i].Pose.Rot[0] = 1.; + } + } + // opencv_solver_poser_cb(so, (PoserData *)pdfs); + // PoserCharlesSlow(so, (PoserData *)pdfs); + } + + double opts[SBA_OPTSSZ] = {}; + double info[SBA_INFOSZ] = {}; + + sba_context ctx = {options, &pdfs->hdr, so}; + + opts[0] = SBA_INIT_MU; + opts[1] = SBA_STOP_THRESH; + opts[2] = SBA_STOP_THRESH; + opts[3] = SBA_STOP_THRESH; + opts[3] = SBA_STOP_THRESH; // max_reproj_error * meas.size(); + opts[4] = 0.0; + + int status = sba_mot_levmar(so->sensor_ct, // number of 3d points + NUM_LIGHTHOUSES, // Number of cameras -- 2 lighthouses + 0, // Number of cameras to not modify + vmask, // boolean vis mask + (double *)&camera_params[0], // camera parameters + sizeof(SurvivePose) / sizeof(double), // The number of floats that are in a camera param + meas, // 2d points for 3d objs + covx, // covariance of measurement. Null sets to identity + 2, // 2 points per image + metric_function, + 0, // jacobia of metric_func + &ctx, // user data + max_iterations, // Max iterations + 0, // verbosity + opts, // options + info); // info + + if (status >= 0) { + PoserData_lighthouse_pose_func(&pdfs->hdr, so, 0, &camera_params[0]); + PoserData_lighthouse_pose_func(&pdfs->hdr, so, 1, &camera_params[1]); + } + // 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; + + free(vmask); + free(meas); + + { + SurviveContext *ctx = so->ctx; + // Docs say info[0] should be divided by meas; I don't buy it really... + SV_INFO("%f original reproj error for %lu meas", (info[0] / meas_size * 2), meas_size); + SV_INFO("%f cur reproj error", (info[1] / meas_size * 2)); + } + + return info[1] / meas_size * 2; +} + +int PoserSBA(SurviveObject *so, PoserData *pd) { + switch (pd->pt) { + case POSERDATA_LIGHT: { + SurviveSensorActivations *scene = &so->activations; + + PoserDataLight *lightData = (PoserDataLight *)pd; + + survive_calibration_config config = *survive_calibration_default_config(); + FLT error = run_sba_find_3d_structure(config, lightData, so, scene, 50, .5); + return 0; + } + case POSERDATA_FULL_SCENE: { + PoserDataFullScene *pdfs = (PoserDataFullScene *)(pd); + survive_calibration_config config = *survive_calibration_default_config(); + // std::cerr << "Running sba with " << config << std::endl; + double error = run_sba(config, pdfs, so, 50, .005); + // std::cerr << "Average reproj error: " << error << std::endl; + return 0; + } + default: { + const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); + PoserCB driver = (PoserCB)GetDriver(subposer); + if (driver) { + return driver(so, pd); + } + break; + } + } + return -1; +} + +REGISTER_LINKTIME(PoserSBA); diff --git a/src/poser_sba.h b/src/poser_sba.h new file mode 100644 index 0000000..8a49122 --- /dev/null +++ b/src/poser_sba.h @@ -0,0 +1,16 @@ +#pragma once + +#pragma once +#include +#define FLT double +#include "survive_reproject.h" +#include +#include + +struct SurviveObject; + +int sba_bruteforce_config_solver_cb(SurviveObject *so, PoserData *pd); +int sba_solver_poser_cb(SurviveObject *so, PoserData *pd); + +std::ostream &operator<<(std::ostream &o, const survive_calibration_options_config &self); +std::ostream &operator<<(std::ostream &o, const survive_calibration_config &self); \ No newline at end of file -- cgit v1.2.3