From 287fc6886057e1773d572b2058222b38fd11122f Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 25 Mar 2018 21:19:05 -0600 Subject: Fixed out of array stepping in charles-biguator --- src/survive_charlesbiguator.c | 35 +++++++++++++++++++++-------------- 1 file changed, 21 insertions(+), 14 deletions(-) (limited to 'src') diff --git a/src/survive_charlesbiguator.c b/src/survive_charlesbiguator.c index fbba888..83b3681 100644 --- a/src/survive_charlesbiguator.c +++ b/src/survive_charlesbiguator.c @@ -1,6 +1,7 @@ //<>< (C) 2016 C. N. Lohr, MOSTLY Under MIT/x11 License. // #include "survive_internal.h" +#include #include /* for sqrt */ #include #include @@ -15,12 +16,16 @@ static int32_t decode_acode(uint32_t length, int32_t main_divisor) { if (acode & 1) return -1; - return (acode >> 1) - 6; + int32_t rtn = (acode >> 1) - 6; + if (rtn > 7 || rtn < 0) { + return -1; + } + return rtn; } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ///////The charles disambiguator. Don't use this, mostly here for -///debugging./////////////////////////////////////////////////////// +/// debugging./////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// static void HandleOOTX(SurviveContext *ctx, SurviveObject *so) { @@ -169,21 +174,23 @@ void DisambiguatorCharles(SurviveObject *so, LightcapElement *le) { int32_t main_divisor = so->timebase_hz / 384000; // 125 @ 48 MHz. int acode = decode_acode(so->last_sync_length[0], main_divisor); - int whichlh; - if (acode < 0) - whichlh = 1; - else - whichlh = (acode >> 2); - int32_t dl = so->last_sync_time[whichlh]; - if (!so->did_handle_ootx) - HandleOOTX(ctx, so); + // If acode isn't right; don't even think of emitting an event + if (acode >= 0) { + int whichlh = (acode >> 2); + assert(whichlh <= 1); + int32_t dl = so->last_sync_time[whichlh]; - int32_t offset_from = le->timestamp - dl + le->length / 2; + if (!so->did_handle_ootx) + HandleOOTX(ctx, so); - // Make sure pulse is in valid window - if (offset_from < so->timecenter_ticks * 2 - so->pulse_in_clear_time && offset_from > so->pulse_in_clear_time) { - ctx->lightproc(so, le->sensor_id, acode, offset_from, le->timestamp, le->length, whichlh); + int32_t offset_from = le->timestamp - dl + le->length / 2; + + // Make sure pulse is in valid window + if (offset_from < so->timecenter_ticks * 2 - so->pulse_in_clear_time && + offset_from > so->pulse_in_clear_time) { + ctx->lightproc(so, le->sensor_id, acode, offset_from, le->timestamp, le->length, whichlh); + } } } else { // printf( "FAIL %d %d - %d = %d\n", le->length, so->last_photo_time, le->timestamp, so->last_photo_time - -- cgit v1.3.1 From 801e17d2c52c21adad5eff63265c1aaea2255b1b Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 25 Mar 2018 21:22:01 -0600 Subject: Added additional parameters to EPNP and SBA, made it so that degenerate poses didn't occur as easily in EPNP --- src/poser_epnp.c | 16 ++++++++++++++-- src/poser_sba.c | 25 ++++++++++++++++--------- 2 files changed, 30 insertions(+), 11 deletions(-) (limited to 'src') diff --git a/src/poser_epnp.c b/src/poser_epnp.c index 401ea2a..ea1e735 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -27,6 +27,12 @@ static SurvivePose solve_correspondence(SurviveObject *so, epnp *pnp, bool camer CvMat R = cvMat(3, 3, CV_64F, r); CvMat T = cvMat(3, 1, CV_64F, rtn.Pos); + + // Super degenerate inputs will project us basically right in the camera. Detect and reject + if (magnitude3d(rtn.Pos) < 0.25) { + return rtn; + } + // Requested output is camera -> world, so invert if (cameraToWorld) { FLT tmp[3]; @@ -81,7 +87,10 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) } SurvivePose lighthouse2object = solve_correspondence(so, &pnp, true); - PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &additionalTx, &lighthouse2object, 0); + + if (quatmagnitude(lighthouse2object.Rot) != 0.0) { + PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &additionalTx, &lighthouse2object, 0); + } epnp_dtor(&pnp); } @@ -123,8 +132,11 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) { epnp_set_maximum_number_of_correspondences(&pnp, so->sensor_ct); add_correspondences(so, &pnp, scene, lightData); + static int required_meas = -1; + if (required_meas == -1) + required_meas = survive_configi(so->ctx, "epnp-required-meas", SC_GET, 4); - if (pnp.number_of_correspondences > 4) { + if (pnp.number_of_correspondences > required_meas) { SurvivePose pose = solve_correspondence(so, &pnp, false); diff --git a/src/poser_sba.c b/src/poser_sba.c index c01cc61..226f387 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -37,6 +37,9 @@ typedef struct SBAData { int failures_to_reset_cntr; int successes_to_reset; int successes_to_reset_cntr; + + int required_meas; + } SBAData; void metric_function(int j, int i, double *aj, double *xij, void *adata) { @@ -175,7 +178,7 @@ void str_metric_function(int j, int i, double *bi, double *xij, void *adata) { SurvivePose *camera = &so->ctx->bsd[lh].Pose; survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, lh, camera, xyz, xij); } - +#if 0 static double run_sba_find_3d_structure_single_sweep(survive_calibration_config options, PoserDataLight *pdl, SurviveObject *so, SurviveSensorActivations *scene, int acode, int lh, int max_iterations /* = 50*/, @@ -187,7 +190,7 @@ static double run_sba_find_3d_structure_single_sweep(survive_calibration_config size_t meas_size = construct_input_from_scene_single_sweep(so, pdl, scene, vmask, meas, acode, lh); static int failure_count = 500; - if (so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0 || meas_size < 8) { + if (so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0 || meas_size < d->required_meas) { if (so->ctx->bsd[0].PositionSet && so->ctx->bsd[1].PositionSet && failure_count++ == 500) { SurviveContext *ctx = so->ctx; SV_INFO("Can't solve for position with just %u measurements", (unsigned int)meas_size); @@ -261,7 +264,7 @@ static double run_sba_find_3d_structure_single_sweep(survive_calibration_config 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) { + if (cnt++ > 1000 || meas_size < d->required_meas) { SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (unsigned int)meas_size); SV_INFO("%f cur reproj error", (info[1] / meas_size * 2)); cnt = 0; @@ -270,7 +273,7 @@ static double run_sba_find_3d_structure_single_sweep(survive_calibration_config return info[1] / meas_size * 2; } - +#endif static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config options, PoserDataLight *pdl, SurviveObject *so, SurviveSensorActivations *scene, int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) { @@ -281,7 +284,7 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o 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 (so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0 || meas_size < d->required_meas) { if (so->ctx->bsd[0].PositionSet && so->ctx->bsd[1].PositionSet && failure_count++ == 500) { SurviveContext *ctx = so->ctx; SV_INFO("Can't solve for position with just %u measurements", (unsigned int)meas_size); @@ -361,7 +364,7 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o 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) { + if (cnt++ > 1000 || meas_size < d->required_meas) { SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (int)meas_size); SV_INFO("%f cur reproj error", (info[1] / meas_size * 2)); cnt = 0; @@ -456,16 +459,20 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd } int PoserSBA(SurviveObject *so, PoserData *pd) { + SurviveContext *ctx = so->ctx; if (so->PoserData == 0) { so->PoserData = calloc(1, sizeof(SBAData)); SBAData *d = so->PoserData; d->failures_to_reset_cntr = 0; - d->failures_to_reset = 5; + d->failures_to_reset = survive_configi(ctx, "sba-failures-to-reset", SC_GET, 1); d->successes_to_reset_cntr = 0; - d->successes_to_reset = 20; + d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, 1); + + d->required_meas = survive_configi(ctx, "sba-required-meas", SC_GET, 8); + + SV_INFO("Initializing SBA with %d required measurements", d->required_meas); } SBAData *d = so->PoserData; - SurviveContext *ctx = so->ctx; switch (pd->pt) { case POSERDATA_LIGHT: { // No poses if calibration is ongoing -- cgit v1.3.1 From 31c9dcb9f783e4920f675010833739f9a3783eea Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 25 Mar 2018 21:24:19 -0600 Subject: Made degenerate poses call SV_ERROR --- src/poser.c | 6 ++++++ 1 file changed, 6 insertions(+) (limited to 'src') diff --git a/src/poser.c b/src/poser.c index 2cfe28d..7dcdc33 100644 --- a/src/poser.c +++ b/src/poser.c @@ -1,6 +1,7 @@ #include "math.h" #include #include +#include #include void PoserData_poser_raw_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { @@ -18,6 +19,11 @@ void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, ui } else { const FLT up[3] = {0, 0, 1}; + if (quatmagnitude(lighthouse_pose->Rot) == 0) { + SurviveContext *ctx = so->ctx; + SV_ERROR("Pose func called with invalid pose."); + } + // Assume that the space solved for is valid but completely arbitrary. We are going to do a few things: // a) Using the gyro data, normalize it so that gravity is pushing straight down along Z // c) Assume the object is at origin -- cgit v1.3.1 From d9ecb4d321bfa04b5d67fb501be0cd9c46140775 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 25 Mar 2018 21:29:32 -0600 Subject: Added tool to calculate various things about length of received pulses --- include/libsurvive/survive.h | 1 + src/survive_sensor_activations.c | 4 +- tools/lightlengthparams/Makefile | 15 ++++ tools/lightlengthparams/lightlengthparams.c | 112 ++++++++++++++++++++++++++++ 4 files changed, 131 insertions(+), 1 deletion(-) create mode 100644 tools/lightlengthparams/Makefile create mode 100644 tools/lightlengthparams/lightlengthparams.c (limited to 'src') diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index b1c32cd..8a1474b 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -16,6 +16,7 @@ extern "C" { typedef struct { FLT angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; // 2 Axes (Angles in LH space) uint32_t timecode[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; // Timecode per axis in ticks + uint32_t lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; // Timecode per axis in ticks FLT accel[3]; FLT gyro[3]; diff --git a/src/survive_sensor_activations.c b/src/survive_sensor_activations.c index 4d1801c..e42b50e 100644 --- a/src/survive_sensor_activations.c +++ b/src/survive_sensor_activations.c @@ -21,9 +21,11 @@ void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDa int axis = (lightData->acode & 1); uint32_t *data_timecode = &self->timecode[lightData->sensor_id][lightData->lh][axis]; FLT *angle = &self->angles[lightData->sensor_id][lightData->lh][axis]; + uint32_t *length = &self->lengths[lightData->sensor_id][lightData->lh][axis]; *angle = lightData->angle; *data_timecode = lightData->timecode; + *length = lightData->length * 48000000; } -uint32_t SurviveSensorActivations_default_tolerance = (uint32_t)(48000000 /*mhz*/ * (16.7 * 2 /*ms*/) / 1000); \ No newline at end of file +uint32_t SurviveSensorActivations_default_tolerance = (uint32_t)(48000000 /*mhz*/ * (16.7 * 2 /*ms*/) / 1000); diff --git a/tools/lightlengthparams/Makefile b/tools/lightlengthparams/Makefile new file mode 100644 index 0000000..b743f4f --- /dev/null +++ b/tools/lightlengthparams/Makefile @@ -0,0 +1,15 @@ +all : lightlengthparams + +SRT:=../.. + +LIBSURVIVE:=$(SRT)/lib/libsurvive.so + +CFLAGS:=-I$(SRT)/redist -I$(SRT)/include -O0 -g -DFLT=double -DUSE_DOUBLE +LDFLAGS:=-lm -lpthread -llapacke -lcblas + +lightlengthparams : lightlengthparams.c $(LIBSURVIVE) + gcc $(CFLAGS) -o $@ $^ $(LDFLAGS) + +clean : + rm -rf lightlengthparams + diff --git a/tools/lightlengthparams/lightlengthparams.c b/tools/lightlengthparams/lightlengthparams.c new file mode 100644 index 0000000..3b28b3f --- /dev/null +++ b/tools/lightlengthparams/lightlengthparams.c @@ -0,0 +1,112 @@ +#include +#include +#include +#include +#include + +uint32_t current_timecode; + +void light_process(SurviveObject *so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, + uint32_t lighthouse) { + current_timecode = timecode; + survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode, length, lighthouse); +} + +void raw_pose_process(SurviveObject *so, uint8_t _lh, SurvivePose *pose2w) { + survive_default_raw_pose_process(so, _lh, pose2w); + + /*printf("Pose: "); + for(int i = 0;i < 7;i++) { + printf("%f, ", pose2w->Pos[i]); + }; + printf("\n"); + */ + for (int lh = 0; lh < 2; lh++) { + SurvivePose pose2lh = {}; + SurvivePose w2lh = {}; + InvertPose(&w2lh, &so->ctx->bsd[lh].Pose); + ApplyPoseToPose(&pose2lh, &w2lh, pose2w); + + SurviveSensorActivations *scene = &so->activations; + for (size_t sensor_idx = 0; sensor_idx < so->sensor_ct; sensor_idx++) { + if (SurviveSensorActivations_isPairValid(scene, SurviveSensorActivations_default_tolerance / 2, + current_timecode, sensor_idx, lh)) { + uint32_t *lengths = scene->lengths[sensor_idx][lh]; + + const FLT *sensor_location = so->sensor_locations + 3 * sensor_idx; + const FLT *sensor_normals = so->sensor_normals + 3 * sensor_idx; + + LinmathPoint3d sensor = {}, sensorN = {}; + + scale3d(sensorN, sensor_normals, .01); + add3d(sensorN, sensor_location, sensor_normals); + + /* + printf("\n"); + printf("%f, %f, %f\n", sensor_location[0], sensor_location[1], sensor_location[2]); + printf("%f, %f, %f\n", sensorN[0], sensorN[1], sensorN[2]); + + FLT m[4][4]; + PoseToMatrix((FLT*)m, &pose2lh); + + for(int i = 0;i < 7;i++) { + printf("%f, ", pose2lh.Pos[i]); + }; + printf("\n"); + for(int i = 0;i < 4;i++) { + for(int j = 0;j < 4;j++) { + printf("%f, ", m[i][j]); + } + printf("\n"); + } + printf("\n"); + */ + ApplyPoseToPoint(sensor, &pose2lh, sensor_location); + ApplyPoseToPoint(sensorN, &pose2lh, sensorN); + + // printf("%f, %f, %f\n", sensor[0], sensor[1], sensor[2]); + // printf("%f, %f, %f\n", sensorN[0], sensorN[1], sensorN[2]); + FLT dist = magnitude3d(sensor); + LinmathVec3d zout = {0, 0, -dist}; + LinmathQuat r; + quatfrom2vectors(r, sensor, zout); + quatrotatevector(sensorN, r, sensorN); + sub3d(sensorN, sensorN, zout); + + FLT dot = dot3d(sensor, sensorN); + if (dist > 20 || dist < 0.25) + continue; + + FLT angs[2] = {}; + for (int axis = 0; axis < 2; axis++) { + angs[axis] = cos(atan2(fabs(sensorN[axis ? 0 : 1]), sensorN[2])); + } + FLT area = angs[0] * angs[1]; + + printf("%u\t%u\t%lu\t%f\t%f\t%f\t%f\t%u\t%u\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n", current_timecode, lh, + sensor_idx, dist, angs[0], angs[1], area, lengths[0], lengths[1], dot, sensor[0], sensor[1], + sensor[2], sensorN[0], sensorN[1], sensorN[2]); + + // assert(angs[0] >= 0); + } + } + } +} + +int main(int argc, char **argv) { + SurviveContext *ctx = survive_init(argc, argv); + if (ctx == 0) // implies -help or similiar + return 0; + + printf("timecode\tlh\tsensor_idx\tdistance\tnorm_x\tnorm_y\tarea\tlength_x\tlength_y\tdot\tx\ty\tz\tnx\tny\tnz\n"); + + survive_startup(ctx); + + survive_install_raw_pose_fn(ctx, raw_pose_process); + survive_install_light_fn(ctx, light_process); + while (survive_poll(ctx) == 0) { + } + + survive_close(ctx); + return 0; +} -- cgit v1.3.1 From 18e717642be3af3b9f72b630dcad68ca17c32dc9 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 26 Mar 2018 13:50:41 -0600 Subject: Made SBA calibrate with 1lh --- src/poser.c | 3 ++- src/poser_sba.c | 9 +++++++-- 2 files changed, 9 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/poser.c b/src/poser.c index 7dcdc33..ea5812e 100644 --- a/src/poser.c +++ b/src/poser.c @@ -21,7 +21,8 @@ void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, ui if (quatmagnitude(lighthouse_pose->Rot) == 0) { SurviveContext *ctx = so->ctx; - SV_ERROR("Pose func called with invalid pose."); + SV_INFO("Pose func called with invalid pose."); + return; } // Assume that the space solved for is valid but completely arbitrary. We are going to do a few things: diff --git a/src/poser_sba.c b/src/poser_sba.c index 226f387..82fbd56 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -439,8 +439,13 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd if (status >= 0) { 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); + for (int i = 0; i < NUM_LIGHTHOUSES; i++) { + if (quatmagnitude(sbactx.camera_params[i].Rot) != 0) { + PoserData_lighthouse_pose_func(&pdfs->hdr, so, i, &additionalTx, &sbactx.camera_params[i], + &sbactx.obj_pose); + } + } + } else { SurviveContext *ctx = so->ctx; SV_INFO("SBA was unable to run %d", status); -- cgit v1.3.1