From ee3160032cd6a609a490be011019b323fe1c0c02 Mon Sep 17 00:00:00 2001 From: dpeter99 Date: Sun, 1 Apr 2018 17:36:08 +0200 Subject: Added new accesors (WIP) --- src/survive.c | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/survive.c b/src/survive.c index 2a7aad1..e4015de 100644 --- a/src/survive.c +++ b/src/survive.c @@ -559,9 +559,19 @@ int survive_simple_inflate(struct SurviveContext *ctx, const char *input, int in return len; } + + +#endif + + const char *survive_object_codename(SurviveObject *so) { return so->codename; } + +const char *survive_object_drivername(SurviveObject *so) { return so->drivername; } +const int8_t *survive_object_charge(SurviveObject *so) { return so->charge; } +const bool *survive_object_charging(SurviveObject *so) { return so->charging; } + +const SurvivePose *survive_object_pose(SurviveObject *so) { return &so->OutPose; } + int8_t survive_object_sensor_ct(SurviveObject *so) { return so->sensor_ct; } const FLT *survive_object_sensor_locations(SurviveObject *so) { return so->sensor_locations; } -const FLT *survive_object_sensor_normals(SurviveObject *so) { return so->sensor_normals; } - -#endif +const FLT *survive_object_sensor_normals(SurviveObject *so) { return so->sensor_normals; } \ No newline at end of file -- cgit v1.2.3 From 7770495e7f14fe5094b31df5f5976c34fd68a0da Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 1 Apr 2018 11:48:56 -0600 Subject: Fixed marshalling --- src/survive.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/survive.c b/src/survive.c index e4015de..f74de85 100644 --- a/src/survive.c +++ b/src/survive.c @@ -567,8 +567,8 @@ int survive_simple_inflate(struct SurviveContext *ctx, const char *input, int in const char *survive_object_codename(SurviveObject *so) { return so->codename; } const char *survive_object_drivername(SurviveObject *so) { return so->drivername; } -const int8_t *survive_object_charge(SurviveObject *so) { return so->charge; } -const bool *survive_object_charging(SurviveObject *so) { return so->charging; } +const int8_t survive_object_charge(SurviveObject *so) { return so->charge; } +const bool survive_object_charging(SurviveObject *so) { return so->charging; } const SurvivePose *survive_object_pose(SurviveObject *so) { return &so->OutPose; } -- cgit v1.2.3 From d77155ee57f715288d73f7c77a14dc05ebc601da Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 2 Apr 2018 00:14:26 -0600 Subject: Fixed windows init issue --- src/poser_charlesslow.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/poser_charlesslow.c b/src/poser_charlesslow.c index b44225e..f725334 100644 --- a/src/poser_charlesslow.c +++ b/src/poser_charlesslow.c @@ -256,7 +256,7 @@ static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int pr int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2; if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue; - FLT out[2] = {}; + FLT out[2] = { 0 }; survive_apply_bsd_calibration(hmd->ctx, lh, fs->angles[p][lh], out); //Find out where our ray shoots forth from. @@ -343,7 +343,7 @@ static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int pr if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue; //Find out where our ray shoots forth from. - FLT out[2] = {}; + FLT out[2] = { 0 }; survive_apply_bsd_calibration(hmd->ctx, lh, fs->angles[p][lh], out); // Find out where our ray shoots forth from. -- cgit v1.2.3 From 8964003b98025168bacf524065f0fb854ed040f7 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Tue, 3 Apr 2018 01:05:48 -0400 Subject: Throw in my Wip poser. --- src/poser_charlesrefine.c | 302 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 302 insertions(+) create mode 100644 src/poser_charlesrefine.c (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c new file mode 100644 index 0000000..4d44722 --- /dev/null +++ b/src/poser_charlesrefine.c @@ -0,0 +1,302 @@ +//EXPERIMENTAL DRIVER - DO NOT USE + +#include +#include +#include + +#include "epnp/epnp.h" +#include "linmath.h" +#include +#include +#include + +#define MAX_PT_PER_SWEEP 32 + + +typedef struct +{ + int sweepaxis; + int sweeplh; + FLT normal_at_errors[MAX_PT_PER_SWEEP][3]; + FLT quantity_errors[MAX_PT_PER_SWEEP] + uint8_t sensor_ids[MAX_PT_PER_SWEEP]; + int ptsweep; +} CharlesPoserData; + + + +int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { + CharlesPoserData * dd = so->PoserData; + if( !dd ) so->PoserData = dd = calloc( sizeof(CharlesPoserData), 1 ); + + SurviveSensorActivations *scene = &so->activations; + switch (pd->pt) { + case POSERDATA_IMU: { + // Really should use this... + PoserDataIMU *imuData = (PoserDataIMU *)pd; + return 0; + } + case POSERDATA_LIGHT: { + int i; + PoserDataLight *ld = (PoserDataLight *)pd; + int lhid = ld->lh; + int senid = ld->sensor_id; + BaseStationData * bsd = &so->ctx->bsd[ld->lh]; + if( !bsd->PositionSet ) break; + SurvivePose * lhp = &bsd->Pose; + FLT angle = ld->angle; + int sensor_id = ld->sensor_id; + int axis = dd->sweepaxis; + const SurvivePose * object_pose = &so->OutPose; + dd->sweeplh = lhid; + + //FOR NOW, drop LH1. + if( lhid == 1 ) break; + + +// const FLT * sensor_normal = &so->sensor_normals[senid*3]; +// FLT sensor_normal_worldspace[3]; +// ApplyPoseToPoint(sensor_normal_worldspace, object_pose, sensor_inpos); + + const FLT * sensor_inpos = &so->sensor_locations[senid*3]; + FLT sensor_position_worldspace[3]; + ApplyPoseToPoint(sensor_position_worldspace, object_pose, sensor_inpos); + //printf( "%f %f %f == > %f %f %f\n", sensor_inpos[0], sensor_inpos[1], sensor_inpos[2], sensor_position_worldspace[0], sensor_position_worldspace[1], sensor_position_worldspace[2] ); + // = sensor position, relative to lighthouse center. + FLT sensorpos_rel_lh[3]; + sub3d( sensorpos_rel_lh, sensor_position_worldspace, lhp->Pos ); + + //Next, define a normal in global space of the plane created by the sweep hit. + //Careful that this must be normalized. + FLT sweep_normal[3]; + + //If 1, the "y" axis. //XXX Check me. + if( axis ) //XXX Just FYI this should include account for skew + { + sweep_normal[0] = 0; + sweep_normal[1] = cos(angle ); + sweep_normal[2] = sin( angle ); + //printf( "+" ); + } + else + { + sweep_normal[0] = cos( angle ); + sweep_normal[1] = 0; + sweep_normal[2] = -sin( angle ); + //printf( "-" ); + } + + //Need to apply the lighthouse's transformation to the sweep's normal. + quatrotatevector( sweep_normal, lhp->Rot, sweep_normal); + + //Compute point-line distance between sensorpos_rel_lh and the plane defined by sweep_normal. + //Do this by projecting sensorpos_rel_lh (w) onto sweep_normal (v). + //You can do this by |v dot w| / |v| ... But we know |v| is 1. So... + FLT dist = dot3d( sensorpos_rel_lh, sweep_normal ); + + if( (i = dd->ptsweep) < MAX_PT_PER_SWEEP ) + { + dd->normal_at_errors[i] = sweep_normal; + dd->quantity_errors[i] = dist; + dd->sensor_ids[i] = sensor_id; + dd->ptsweep++: + } + +#if 0 + printf( "D %d %d: %f [%f %f %f]\n", lhid, axis, dist, sweep_normal[0], sweep_normal[1], sweep_normal[2] ); + + + //Naieve approach... Push it in the right direction + SurvivePose object_pose_out; + quatcopy( object_pose_out.Rot, object_pose->Rot ); + scale3d(sweep_normal, sweep_normal, -0.1*dist); + add3d(object_pose_out.Pos, sweep_normal, object_pose->Pos); + + if( so->PoseConfidence < .01 ) + { + dd->average_nudge[0] = 0; + dd->average_nudge[1] = 0; + dd->average_nudge[2] = 0; + + memcpy( &object_pose_out, &LinmathPose_Identity, sizeof( LinmathPose_Identity ) ); + object_pose_out.Pos[1] = 2.5; + object_pose_out.Pos[2] = 1.8; + so->PoseConfidence = 1.0; + } +// printf( "%f %f %f %f\n", object_pose->Rot[0], object_pose->Rot[1], object_pose->Rot[2], object_pose->Rot[3] ); + + PoserData_poser_raw_pose_func(pd, so, lhid, &object_pose_out); +#endif + +#if 0 +// = { + axis?0.0:sin(angle), + axis?sin(angle):0.0, + cos(angle) }; + + = sensor_locations; + LinmathPoint3d + int8_t sensor_ct; // sensor count + FLT *sensor_locations; // size is sensor_ct*3. Contains x,y,z values for each sensor + FLT *sensor_normals; // size is nrlocations*3. cointains normal vector for each sensor + + + // This is the quat equivalent of 'pout = pose * pin' if pose were a 4x4 matrix in homogenous space +void ApplyPoseToPoint(LinmathPoint3d pout, const LinmathPose *pose, const LinmathPoint3d pin); + + + + SurvivePose obj2world; +// ApplyPoseToPose(&obj2world, &lh2world, &objpose); + memcpy( &obj2world, &LinmathPose_Identity, sizeof( obj2world ) ); + obj2world.Pos[1] = 1; + PoserData_poser_raw_pose_func(pd, so, lhid, &obj2world); +// PoserData_poser_raw_pose_func(pd, so, ld->lh, &posers[lightData->lh]); + + + // Pose Information, also "poser" field. + ///from SurviveObject + // FLT PoseConfidence; // 0..1 + // SurvivePose OutPose; // Final pose? (some day, one can dream!) + // SurvivePose FromLHPose[NUM_LIGHTHOUSES]; // Filled out by poser, contains computed position from each lighthouse. + // void *PoserData; // Initialized to zero, configured by poser, can be anything the poser wants. + // PoserCB PoserFn; + + + // printf( "%d %d %f\n", ld->sensor_id, ld->lh, ld->angle ); + +/* + SurvivePose *object_pose, void *user); + +typedef struct +{ + PoserType pt; + poser_raw_pose_func rawposeproc; + poser_lighthouse_pose_func lighthouseposeproc; + void *userdata; +} PoserData; + + PoserData hdr; + int sensor_id; + int acode; //OOTX Code associated with this sweep. bit 1 indicates vertical(1) or horizontal(0) sweep + int lh; //Lighthouse making this sweep + uint32_t timecode; //In object-local ticks. + FLT length; //In seconds + FLT angle; //In radians from center of lighthouse. +} PoserDataLight; + + + +*/ +#if 0 + SurvivePose posers[2]; + int meas[2] = {0, 0}; + for (int lh = 0; lh < so->ctx->activeLighthouses; lh++) { + if (so->ctx->bsd[lh].PositionSet) { + epnp pnp = {.fu = 1, .fv = 1}; + epnp_set_maximum_number_of_correspondences(&pnp, so->sensor_ct); + + add_correspondences(so, &pnp, scene, lightData->timecode, lh); + 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 > required_meas) { + + SurvivePose objInLh = solve_correspondence(so, &pnp, false); + if (quatmagnitude(objInLh.Rot) != 0) { + SurvivePose *lh2world = &so->ctx->bsd[lh].Pose; + + SurvivePose txPose = {.Rot = {1}}; + ApplyPoseToPose(&txPose, lh2world, &objInLh); + posers[lh] = txPose; + meas[lh] = pnp.number_of_correspondences; + } + } + + epnp_dtor(&pnp); + } + } + + if (meas[0] > 0 && meas[1] > 0) { + SurvivePose interpolate = {0}; + bool winnerTakesAll = true; // Not convinced slerp does the right thing, will change this when i am + + if (winnerTakesAll) { + int winner = meas[0] > meas[1] ? 0 : 1; + PoserData_poser_raw_pose_func(pd, so, winner, &posers[winner]); + } else { + double a, b; + a = meas[0] * meas[0]; + b = meas[1] * meas[1]; + + double t = a + b; + for (size_t i = 0; i < 3; i++) { + interpolate.Pos[i] = (posers[0].Pos[i] * a + posers[1].Pos[i] * b) / (t); + } + quatslerp(interpolate.Rot, posers[0].Rot, posers[1].Rot, b / (t)); + PoserData_poser_raw_pose_func(pd, so, lightData->lh, &interpolate); + } + } else { + if (meas[lightData->lh]) + PoserData_poser_raw_pose_func(pd, so, lightData->lh, &posers[lightData->lh]); + } +#endif + +#endif + return 0; + } + + case POSERDATA_SYNC: { + PoserDataLight *l = (PoserDataLight *)pd; + int lhid = l->lh; + + //you can get sweepaxis and sweeplh. + + if( dd->ptsweep ) + { + int i; + int lhid = dd->lhid; + int pts = dd->ptsweep; + const SurvivePose * object_pose = &so->OutPose; + + FLT avg_err[3] = { 0, 0, 0 }; + FLT avgtot = 0.0; + for( i = 0; i < pts; i++ ) + { + FLT * nrm = dd->normal_at_errors[pts]; + FLT qty = quantity_errors[pts]; + avgtot += qty; + avg_err[0] = avg_err[0] + nrm[0] * qty; + avg_err[1] = avg_err[1] + nrm[1] * qty; + avg_err[2] = avg_err[2] + nrm[2] * qty; + } + scale3d(avg_err, avg_err, 1./pts); + //We have "Average error" now. This is a world space value. + //This can correct for lateral error, but not distance from camera. + + //Next we need to find out what the weighting is to determine "zoom" + //How do we do this? ??? Too tired to math. + FLT weight = 0.0; + for( i = 0; i < pts; i++ ) + { + //??!?!? Sturfff + } + + dd->ptsweep = 0; + + //Update PoserData_poser_raw_pose_func(pd, so, lhid, &object_pose_out); + } + + dd->nextaxis = l->acode & 1; + printf( "SYNC %d %p\n", l->acode, dd ); + break; + } + case POSERDATA_FULL_SCENE: { + //return opencv_solver_fullscene(so, (PoserDataFullScene *)(pd)); + } + } + return -1; +} + +REGISTER_LINKTIME(PoserCharlesRefine); -- cgit v1.2.3 From 18b20af7195b94889924156de2b4aa704b2c7391 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 3 Apr 2018 15:11:12 -0600 Subject: Refactor pose function to get timecode and not lh --- src/poser.c | 26 ++++++++++++++++++++++---- src/poser_daveortho.c | 2 +- src/poser_epnp.c | 6 +++--- src/poser_sba.c | 6 +++--- src/poser_turveytori.c | 4 ++-- src/survive.c | 8 ++++---- src/survive_process.c | 6 +++--- 7 files changed, 38 insertions(+), 20 deletions(-) (limited to 'src') diff --git a/src/poser.c b/src/poser.c index 9a0de24..20334f7 100644 --- a/src/poser.c +++ b/src/poser.c @@ -7,11 +7,29 @@ #define _USE_MATH_DEFINES // for C #include -void PoserData_poser_raw_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { - if (poser_data->rawposeproc) { - poser_data->rawposeproc(so, lighthouse, pose, poser_data->userdata); +static uint32_t PoserData_timecode(PoserData *poser_data) { + switch (poser_data->pt) { + case POSERDATA_LIGHT: { + PoserDataLight *lightData = (PoserDataLight *)poser_data; + return lightData->timecode; + } + case POSERDATA_FULL_SCENE: { + PoserDataFullScene* pdfs = (PoserDataFullScene *)(poser_data); + return -1; + } + case POSERDATA_IMU: { + PoserDataIMU *imuData = (PoserDataIMU *)poser_data; + return imuData->timecode; + } + } + return -1; +} + +void PoserData_poser_pose_func(PoserData *poser_data, SurviveObject *so, SurvivePose *pose) { + if (poser_data->poseproc) { + poser_data->poseproc(so, PoserData_timecode(poser_data), pose, poser_data->userdata); } else { - so->ctx->rawposeproc(so, lighthouse, pose); + so->ctx->poseproc(so, PoserData_timecode(poser_data), pose); } } diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index 9cdab45..330e7e8 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -107,7 +107,7 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) SurvivePose obj2world; ApplyPoseToPose(&obj2world, &lh2world, &objpose); - PoserData_poser_raw_pose_func(pd, so, lhid, &obj2world); + PoserData_poser_pose_func(pd, so, &obj2world); if (0) { fprintf(stderr,"INQUAT: %f %f %f %f = %f [%f %f %f]\n", objpose.Rot[0], objpose.Rot[1], objpose.Rot[2], diff --git a/src/poser_epnp.c b/src/poser_epnp.c index c05450a..eaa1659 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -164,7 +164,7 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) { if (winnerTakesAll) { int winner = meas[0] > meas[1] ? 0 : 1; - PoserData_poser_raw_pose_func(pd, so, winner, &posers[winner]); + PoserData_poser_pose_func(pd, so, &posers[winner]); } else { double a, b; a = meas[0] * meas[0]; @@ -175,11 +175,11 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) { interpolate.Pos[i] = (posers[0].Pos[i] * a + posers[1].Pos[i] * b) / (t); } quatslerp(interpolate.Rot, posers[0].Rot, posers[1].Rot, b / (t)); - PoserData_poser_raw_pose_func(pd, so, lightData->lh, &interpolate); + PoserData_poser_pose_func(pd, so, &interpolate); } } else { if (meas[lightData->lh]) - PoserData_poser_raw_pose_func(pd, so, lightData->lh, &posers[lightData->lh]); + PoserData_poser_pose_func(pd, so, &posers[lightData->lh]); } return 0; } diff --git a/src/poser_sba.c b/src/poser_sba.c index bd7d520..e74bb20 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -129,7 +129,7 @@ typedef struct { SurvivePose poses; } sba_set_position_t; -static void sba_set_position(SurviveObject *so, uint8_t lighthouse, SurvivePose *new_pose, void *_user) { +static void sba_set_position(SurviveObject *so, uint32_t timecode, SurvivePose *new_pose, void *_user) { sba_set_position_t *user = _user; assert(user->hasInfo == false); user->hasInfo = 1; @@ -220,7 +220,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive 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; + pdl->hdr.poseproc = sba_set_position; sba_set_position_t locations = {0}; pdl->hdr.userdata = &locations; @@ -278,7 +278,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive if (status > 0 && (info[1] / meas_size * 2) < d->max_error) { d->failures_to_reset_cntr = d->failures_to_reset; quatnormalize(soLocation.Rot, soLocation.Rot); - PoserData_poser_raw_pose_func(&pdl->hdr, so, 1, &soLocation); + PoserData_poser_pose_func(&pdl->hdr, so, &soLocation); } { diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 4628207..243051e 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1631,9 +1631,9 @@ static void QuickPose(SurviveObject *so, PoserData *pd, SurvivePose *additionalT SolveForLighthouse(&pose.Pos[0], &pose.Rot[0], to, so, pd, 0, additionalTx, lh, 0); //printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); - if (so->ctx->rawposeproc) + if (so->ctx->poseproc) { - so->ctx->rawposeproc(so, lh, &pose); + so->ctx->poseproc(so, lh, &pose); } if (ttDebug) printf("!\n"); diff --git a/src/survive.c b/src/survive.c index f74de85..f6f98fc 100644 --- a/src/survive.c +++ b/src/survive.c @@ -217,7 +217,7 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) { ctx->angleproc = survive_default_angle_process; ctx->lighthouseposeproc = survive_default_lighthouse_pose_process; ctx->configfunction = survive_default_htc_config_process; - ctx->rawposeproc = survive_default_raw_pose_process; + ctx->poseproc = survive_default_raw_pose_process; ctx->calibration_config = survive_calibration_config_ctor(); ctx->calibration_config.use_flag = (enum SurviveCalFlag)survive_configi(ctx, "bsd-cal", SC_GET, SVCal_All); @@ -381,11 +381,11 @@ void survive_install_button_fn(SurviveContext *ctx, button_process_func fbp) { ctx->buttonproc = survive_default_button_process; } -void survive_install_raw_pose_fn(SurviveContext *ctx, raw_pose_func fbp) { +void survive_install_pose_fn(SurviveContext *ctx, pose_func fbp) { if (fbp) - ctx->rawposeproc = fbp; + ctx->poseproc = fbp; else - ctx->rawposeproc = survive_default_raw_pose_process; + ctx->poseproc = survive_default_raw_pose_process; } void survive_install_lighthouse_pose_fn(SurviveContext *ctx, lighthouse_pose_func fbp) { diff --git a/src/survive_process.c b/src/survive_process.c index 62459f2..e013327 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -139,12 +139,12 @@ void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8 //} } -void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { +void survive_default_raw_pose_process(SurviveObject *so, uint32_t timecode, SurvivePose *pose) { // print the pose; //printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); so->OutPose = *pose; - so->FromLHPose[lighthouse] = *pose; - survive_recording_raw_pose_process(so, lighthouse, pose); + so->OutPose_timecode = timecode; + survive_recording_raw_pose_process(so, timecode, pose); } void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose, -- cgit v1.2.3 From 52c11484ca8ada2e163cbbae72d0d417f0342a38 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Tue, 3 Apr 2018 21:56:50 -0400 Subject: The reuslts are incredibly stable. Need to test on the opi. --- src/poser_charlesrefine.c | 366 +++++++++++++++++++++++++--------------------- 1 file changed, 201 insertions(+), 165 deletions(-) (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 4d44722..e6f6a57 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -9,6 +9,7 @@ #include #include #include +#include #define MAX_PT_PER_SWEEP 32 @@ -17,8 +18,10 @@ typedef struct { int sweepaxis; int sweeplh; - FLT normal_at_errors[MAX_PT_PER_SWEEP][3]; - FLT quantity_errors[MAX_PT_PER_SWEEP] + FLT normal_at_errors[MAX_PT_PER_SWEEP][3]; //Value is actually normalized, not just normal to sweep plane. + FLT quantity_errors[MAX_PT_PER_SWEEP]; + FLT angles_at_pts[MAX_PT_PER_SWEEP]; + SurvivePose object_pose_at_hit[MAX_PT_PER_SWEEP]; uint8_t sensor_ids[MAX_PT_PER_SWEEP]; int ptsweep; } CharlesPoserData; @@ -51,7 +54,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { dd->sweeplh = lhid; //FOR NOW, drop LH1. - if( lhid == 1 ) break; + //if( lhid == 1 ) break; // const FLT * sensor_normal = &so->sensor_normals[senid*3]; @@ -60,7 +63,11 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { const FLT * sensor_inpos = &so->sensor_locations[senid*3]; FLT sensor_position_worldspace[3]; + //XXX Once I saw this get pretty wild (When in playback) + //I had to invert the values of sensor_inpos. Not sure why. ApplyPoseToPoint(sensor_position_worldspace, object_pose, sensor_inpos); + + //printf( "%f %f %f == > %f %f %f\n", sensor_inpos[0], sensor_inpos[1], sensor_inpos[2], sensor_position_worldspace[0], sensor_position_worldspace[1], sensor_position_worldspace[2] ); // = sensor position, relative to lighthouse center. FLT sensorpos_rel_lh[3]; @@ -96,200 +103,229 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { if( (i = dd->ptsweep) < MAX_PT_PER_SWEEP ) { - dd->normal_at_errors[i] = sweep_normal; + memcpy( dd->normal_at_errors[i], sweep_normal, sizeof(FLT)*3 ); dd->quantity_errors[i] = dist; + dd->angles_at_pts[i] = angle; dd->sensor_ids[i] = sensor_id; - dd->ptsweep++: + memcpy( &dd->object_pose_at_hit[i], object_pose, sizeof(SurvivePose) ); + dd->ptsweep++; } -#if 0 - printf( "D %d %d: %f [%f %f %f]\n", lhid, axis, dist, sweep_normal[0], sweep_normal[1], sweep_normal[2] ); + return 0; + } + case POSERDATA_SYNC: { + PoserDataLight *l = (PoserDataLight *)pd; + int lhid = l->lh; - //Naieve approach... Push it in the right direction - SurvivePose object_pose_out; - quatcopy( object_pose_out.Rot, object_pose->Rot ); - scale3d(sweep_normal, sweep_normal, -0.1*dist); - add3d(object_pose_out.Pos, sweep_normal, object_pose->Pos); - if( so->PoseConfidence < .01 ) + //you can get sweepaxis and sweeplh. + if( dd->ptsweep ) { - dd->average_nudge[0] = 0; - dd->average_nudge[1] = 0; - dd->average_nudge[2] = 0; - - memcpy( &object_pose_out, &LinmathPose_Identity, sizeof( LinmathPose_Identity ) ); - object_pose_out.Pos[1] = 2.5; - object_pose_out.Pos[2] = 1.8; - so->PoseConfidence = 1.0; - } -// printf( "%f %f %f %f\n", object_pose->Rot[0], object_pose->Rot[1], object_pose->Rot[2], object_pose->Rot[3] ); - - PoserData_poser_raw_pose_func(pd, so, lhid, &object_pose_out); -#endif - -#if 0 -// = { - axis?0.0:sin(angle), - axis?sin(angle):0.0, - cos(angle) }; - - = sensor_locations; - LinmathPoint3d - int8_t sensor_ct; // sensor count - FLT *sensor_locations; // size is sensor_ct*3. Contains x,y,z values for each sensor - FLT *sensor_normals; // size is nrlocations*3. cointains normal vector for each sensor - - - // This is the quat equivalent of 'pout = pose * pin' if pose were a 4x4 matrix in homogenous space -void ApplyPoseToPoint(LinmathPoint3d pout, const LinmathPose *pose, const LinmathPoint3d pin); + int i; + int lhid = dd->sweeplh; + int axis = dd->sweepaxis; + int pts = dd->ptsweep; + const SurvivePose * object_pose = &so->OutPose; //XXX TODO Should pull pose from approximate time when LHs were scanning it. + BaseStationData * bsd = &so->ctx->bsd[lhid]; + SurvivePose * lh_pose = &bsd->Pose; + int validpoints = 0; + int ptvalid[MAX_PT_PER_SWEEP]; + FLT avgerr = 0.0; + FLT vec_correct[3] = { 0., 0. , 0. }; + FLT avgang = 0.0; - SurvivePose obj2world; -// ApplyPoseToPose(&obj2world, &lh2world, &objpose); - memcpy( &obj2world, &LinmathPose_Identity, sizeof( obj2world ) ); - obj2world.Pos[1] = 1; - PoserData_poser_raw_pose_func(pd, so, lhid, &obj2world); -// PoserData_poser_raw_pose_func(pd, so, ld->lh, &posers[lightData->lh]); +//Tunable parameters: +#define MIN_HIT_QUALITY 0.5 //Determines which hits to cull. +#define HIT_QUALITY_BASELINE 0.0001 //Determines which hits to cull. Actually SQRT(baseline) if 0.0001, it is really 1cm +#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.2 //Explodes if you exceed 1.0 +#define CORRECT_TELESCOPTION_COEFFICIENT 1.0 //Converges even as high as 10.0 and doesn't explode. +#define CORRECT_ROTATION_COEFFICIENT 5.0 //This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. +#define ROTATIONAL_CORRECTION_MAXFORCE 0.10 - // Pose Information, also "poser" field. - ///from SurviveObject - // FLT PoseConfidence; // 0..1 - // SurvivePose OutPose; // Final pose? (some day, one can dream!) - // SurvivePose FromLHPose[NUM_LIGHTHOUSES]; // Filled out by poser, contains computed position from each lighthouse. - // void *PoserData; // Initialized to zero, configured by poser, can be anything the poser wants. - // PoserCB PoserFn; + //Step 1: Determine standard of deviation, and average in order to + // drop points that are likely in error. + { + //Calculate average + FLT avgerr_orig = 0.0; + FLT stddevsq = 0.0; + for( i = 0; i < pts; i++ ) + avgerr_orig += dd->quantity_errors[i]; + avgerr_orig/=pts; + + //Calculate standard of deviation. + for( i = 0; i < pts; i++ ) + { + FLT diff = dd->quantity_errors[i]-avgerr_orig; + stddevsq += diff*diff; + } + stddevsq/=pts; + + for( i = 0; i < pts; i++ ) + { + FLT err = dd->quantity_errors[i]; + FLT diff = err-avgerr_orig; + diff *= diff; + int isptvalid = (diff * MIN_HIT_QUALITY <= stddevsq + HIT_QUALITY_BASELINE)?1:0; + ptvalid[i] = isptvalid; + if( isptvalid ) + { + avgang += dd->angles_at_pts[i]; + avgerr += err; + validpoints ++; + } + } + avgang /= validpoints; + avgerr /= validpoints; + } + //Step 2: Determine average lateral error. + //We can actually always perform this operation. Even with only one point. + { + FLT avg_err[3] = { 0, 0, 0 }; //Positional error. + for( i = 0; i < pts; i++ ) + { + if( !ptvalid[i] ) continue; + FLT * nrm = dd->normal_at_errors[i]; + FLT err = dd->quantity_errors[i]; + avg_err[0] = avg_err[0] + nrm[0] * err; + avg_err[1] = avg_err[1] + nrm[1] * err; + avg_err[2] = avg_err[2] + nrm[2] * err; + } - // printf( "%d %d %f\n", ld->sensor_id, ld->lh, ld->angle ); + //NOTE: The "avg_err" is not geometrically centered. This is actually + //probably okay, since if you have sevearl data points to one side, you + //can probably trust that more. + scale3d(avg_err, avg_err, 1./validpoints); -/* - SurvivePose *object_pose, void *user); + //We have "Average error" now. A vector in worldspace. + //This can correct for lateral error, but not distance from camera. -typedef struct -{ - PoserType pt; - poser_raw_pose_func rawposeproc; - poser_lighthouse_pose_func lighthouseposeproc; - void *userdata; -} PoserData; - - PoserData hdr; - int sensor_id; - int acode; //OOTX Code associated with this sweep. bit 1 indicates vertical(1) or horizontal(0) sweep - int lh; //Lighthouse making this sweep - uint32_t timecode; //In object-local ticks. - FLT length; //In seconds - FLT angle; //In radians from center of lighthouse. -} PoserDataLight; - - - -*/ -#if 0 - SurvivePose posers[2]; - int meas[2] = {0, 0}; - for (int lh = 0; lh < so->ctx->activeLighthouses; lh++) { - if (so->ctx->bsd[lh].PositionSet) { - epnp pnp = {.fu = 1, .fv = 1}; - epnp_set_maximum_number_of_correspondences(&pnp, so->sensor_ct); - - add_correspondences(so, &pnp, scene, lightData->timecode, lh); - 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 > required_meas) { - - SurvivePose objInLh = solve_correspondence(so, &pnp, false); - if (quatmagnitude(objInLh.Rot) != 0) { - SurvivePose *lh2world = &so->ctx->bsd[lh].Pose; - - SurvivePose txPose = {.Rot = {1}}; - ApplyPoseToPose(&txPose, lh2world, &objInLh); - posers[lh] = txPose; - meas[lh] = pnp.number_of_correspondences; - } - } - - epnp_dtor(&pnp); + //XXX TODO: Should we check to see if we only have one or + //two points to make sure the error on this isn't unusually high? + //If calculated error is unexpectedly high, then we should probably + //Not apply the transform. + scale3d( avg_err, avg_err, -CORRECT_LATERAL_POSITION_COEFFICIENT ); + add3d( vec_correct, vec_correct, avg_err ); } - } - if (meas[0] > 0 && meas[1] > 0) { - SurvivePose interpolate = {0}; - bool winnerTakesAll = true; // Not convinced slerp does the right thing, will change this when i am - - if (winnerTakesAll) { - int winner = meas[0] > meas[1] ? 0 : 1; - PoserData_poser_raw_pose_func(pd, so, winner, &posers[winner]); - } else { - double a, b; - a = meas[0] * meas[0]; - b = meas[1] * meas[1]; - - double t = a + b; - for (size_t i = 0; i < 3; i++) { - interpolate.Pos[i] = (posers[0].Pos[i] * a + posers[1].Pos[i] * b) / (t); + //Step 3: Control telecoption from lighthouse. + // we need to find out what the weighting is to determine "zoom" + if( validpoints > 1 ) //Can't correct "zoom" with only one point. + { + FLT zoom = 0.0; + FLT rmsang = 0.0; + for( i = 0; i < pts; i++ ) + { + if( !ptvalid[i] ) continue; + FLT delang = dd->angles_at_pts[i] - avgang; + FLT delerr = dd->quantity_errors[i] - avgerr; + if( axis ) delang *= -1; //Flip sign on alternate axis because it's measured backwards. + zoom += delerr * delang; + rmsang += delang * delang; } - quatslerp(interpolate.Rot, posers[0].Rot, posers[1].Rot, b / (t)); - PoserData_poser_raw_pose_func(pd, so, lightData->lh, &interpolate); - } - } else { - if (meas[lightData->lh]) - PoserData_poser_raw_pose_func(pd, so, lightData->lh, &posers[lightData->lh]); - } -#endif -#endif - return 0; - } + //Control into or outof lighthouse. + //XXX Check to see if we need to sqrt( the rmsang), need to check convergance behavior close to lighthouse. + //This is a questionable step. + zoom /= sqrt(rmsang); - case POSERDATA_SYNC: { - PoserDataLight *l = (PoserDataLight *)pd; - int lhid = l->lh; + zoom *= CORRECT_TELESCOPTION_COEFFICIENT; - //you can get sweepaxis and sweeplh. + FLT veccamalong[3]; + sub3d( veccamalong, lh_pose->Pos, object_pose->Pos ); + normalize3d( veccamalong, veccamalong ); + scale3d( veccamalong, veccamalong, zoom ); + add3d( vec_correct, veccamalong, vec_correct ); + } - if( dd->ptsweep ) - { - int i; - int lhid = dd->lhid; - int pts = dd->ptsweep; - const SurvivePose * object_pose = &so->OutPose; - FLT avg_err[3] = { 0, 0, 0 }; - FLT avgtot = 0.0; - for( i = 0; i < pts; i++ ) - { - FLT * nrm = dd->normal_at_errors[pts]; - FLT qty = quantity_errors[pts]; - avgtot += qty; - avg_err[0] = avg_err[0] + nrm[0] * qty; - avg_err[1] = avg_err[1] + nrm[1] * qty; - avg_err[2] = avg_err[2] + nrm[2] * qty; + SurvivePose object_pose_out; + add3d(object_pose_out.Pos, vec_correct, object_pose->Pos); + + quatcopy( object_pose_out.Rot, object_pose->Rot ); + + //Stage 4: "Tug" on the rotation of the object, from all of the sensor's pov. + //If we were able to determine likliehood of a hit in the sweep instead of afterward + //we would actually be able to perform this on a per-hit basis. + if( 1 ) { + LinmathQuat correction; + quatcopy( correction, LinmathQuat_Identity ); + for( i = 0; i < pts; i++ ) + { + if( !ptvalid[i] ) continue; + FLT dist = dd->quantity_errors[i]-avgerr; + FLT angle = dd->angles_at_pts[i]; + int sensor_id = dd->sensor_ids[i]; + FLT * normal = dd->normal_at_errors[i]; + const SurvivePose * object_pose_at_hit = &dd->object_pose_at_hit[i]; + const FLT * sensor_inpos = &so->sensor_locations[sensor_id*3]; + + LinmathQuat world_to_object_space; + quatgetreciprocal(world_to_object_space, object_pose_at_hit->Rot); + FLT correction_in_object_space[3]; //The amount across the surface of the object the rotation should happen. + + quatrotatevector(correction_in_object_space, world_to_object_space, normal ); + dist *= CORRECT_ROTATION_COEFFICIENT; + if( dist > ROTATIONAL_CORRECTION_MAXFORCE ) dist = ROTATIONAL_CORRECTION_MAXFORCE; + if( dist <-ROTATIONAL_CORRECTION_MAXFORCE ) dist =-ROTATIONAL_CORRECTION_MAXFORCE; + + //Now, we have a "tug" vector in object-local space. Need to apply the torque. + FLT vector_from_center_of_object[3]; + normalize3d( vector_from_center_of_object, sensor_inpos ); + //scale3d(vector_from_center_of_object, sensor_inpos, 10.0 ); + // vector_from_center_of_object[2]*=-1; + // vector_from_center_of_object[1]*=-1; +// vector_from_center_of_object[0]*=-1; + //vector_from_center_of_object + scale3d(vector_from_center_of_object,vector_from_center_of_object, 1); + + + FLT new_vector_in_object_space[3]; + //printf( "%f %f %f %f\n", object_pose_at_hit->Rot[0], object_pose_at_hit->Rot[1], object_pose_at_hit->Rot[2], object_pose_at_hit->Rot[3] ); + //printf( "%f %f %f // %f %f %f // %f\n", vector_from_center_of_object[0], vector_from_center_of_object[1], vector_from_center_of_object[2], correction_in_object_space[0], correction_in_object_space[1], correction_in_object_space[2], dist ); + scale3d( correction_in_object_space, correction_in_object_space, -dist ); + add3d( new_vector_in_object_space, vector_from_center_of_object, correction_in_object_space ); + + normalize3d( new_vector_in_object_space, new_vector_in_object_space ); + + LinmathQuat corrective_quaternion; + quatfrom2vectors(corrective_quaternion, vector_from_center_of_object, new_vector_in_object_space ); + quatrotateabout( correction, correction, corrective_quaternion ); + //printf( "%f -> %f %f %f => %f %f %f [%f %f %f %f]\n", dist, vector_from_center_of_object[0], vector_from_center_of_object[1], vector_from_center_of_object[2], + //correction_in_object_space[0], correction_in_object_space[1], correction_in_object_space[2], + //corrective_quaternion[0],corrective_quaternion[1],corrective_quaternion[1],corrective_quaternion[3]); + } + printf( "Applying: %f %f %f %f\n", correction[0], correction[1], correction[2], correction[3] ); + //Apply our corrective quaternion to the output. + quatrotateabout( object_pose_out.Rot, object_pose_out.Rot, correction ); + quatnormalize( object_pose_out.Rot, object_pose_out.Rot ); } - scale3d(avg_err, avg_err, 1./pts); - //We have "Average error" now. This is a world space value. - //This can correct for lateral error, but not distance from camera. - - //Next we need to find out what the weighting is to determine "zoom" - //How do we do this? ??? Too tired to math. - FLT weight = 0.0; - for( i = 0; i < pts; i++ ) + + //Janky need to do this somewhere else... This initializes the pose estimator. + if( so->PoseConfidence < .01 ) { - //??!?!? Sturfff + memcpy( &object_pose_out, &LinmathPose_Identity, sizeof( LinmathPose_Identity ) ); + object_pose_out.Pos[0] = -0.14372776; + object_pose_out.Pos[1] = 0.06856518; + object_pose_out.Pos[2] = 0.01960009; + object_pose_out.Rot[0] = 1.0; + object_pose_out.Rot[1] = -0.0; + object_pose_out.Rot[2] = 0.0; + object_pose_out.Rot[3] = 0.0; + so->PoseConfidence = 1.0; } - dd->ptsweep = 0; + PoserData_poser_raw_pose_func(pd, so, lhid, &object_pose_out); - //Update PoserData_poser_raw_pose_func(pd, so, lhid, &object_pose_out); + dd->ptsweep = 0; } - dd->nextaxis = l->acode & 1; - printf( "SYNC %d %p\n", l->acode, dd ); + dd->sweepaxis = l->acode & 1; + //printf( "SYNC %d %p\n", l->acode, dd ); break; } case POSERDATA_FULL_SCENE: { -- cgit v1.2.3 From 872829b375ec0bc5bdbf9b223cf49ac1b4055de2 Mon Sep 17 00:00:00 2001 From: Charles Lohr Date: Wed, 4 Apr 2018 03:10:21 +0000 Subject: Ok, it works! --- src/poser_charlesrefine.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index e6f6a57..61bd575 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -141,9 +141,9 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { #define MIN_HIT_QUALITY 0.5 //Determines which hits to cull. #define HIT_QUALITY_BASELINE 0.0001 //Determines which hits to cull. Actually SQRT(baseline) if 0.0001, it is really 1cm -#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.2 //Explodes if you exceed 1.0 -#define CORRECT_TELESCOPTION_COEFFICIENT 1.0 //Converges even as high as 10.0 and doesn't explode. -#define CORRECT_ROTATION_COEFFICIENT 5.0 //This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. +#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.1 //Explodes if you exceed 1.0 +#define CORRECT_TELESCOPTION_COEFFICIENT 0.5 //Converges even as high as 10.0 and doesn't explode. +#define CORRECT_ROTATION_COEFFICIENT 1.0 //This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. #define ROTATIONAL_CORRECTION_MAXFORCE 0.10 //Step 1: Determine standard of deviation, and average in order to @@ -299,7 +299,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { //correction_in_object_space[0], correction_in_object_space[1], correction_in_object_space[2], //corrective_quaternion[0],corrective_quaternion[1],corrective_quaternion[1],corrective_quaternion[3]); } - printf( "Applying: %f %f %f %f\n", correction[0], correction[1], correction[2], correction[3] ); + //printf( "Applying: %f %f %f %f\n", correction[0], correction[1], correction[2], correction[3] ); //Apply our corrective quaternion to the output. quatrotateabout( object_pose_out.Rot, object_pose_out.Rot, correction ); quatnormalize( object_pose_out.Rot, object_pose_out.Rot ); -- cgit v1.2.3 From 34c6afbd52b7a6109d563ce17d082072b16ee032 Mon Sep 17 00:00:00 2001 From: Charles Lohr Date: Wed, 4 Apr 2018 03:34:25 +0000 Subject: Add gyro support (Still need madgwick's) --- src/poser_charlesrefine.c | 15 +++++++++++++++ 1 file changed, 15 insertions(+) (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 61bd575..9aa44d9 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -37,6 +37,21 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { case POSERDATA_IMU: { // Really should use this... PoserDataIMU *imuData = (PoserDataIMU *)pd; + + + //TODO: Actually do Madgwick's algorithm + LinmathQuat applymotion; + const SurvivePose * object_pose = &so->OutPose; + imuData->gyro[0] *= -0.001; + imuData->gyro[1] *= -0.001; + imuData->gyro[2] *= 0.001; + quatfromeuler( applymotion, imuData->gyro ); + //printf( "%f %f %f\n", imuData->gyro [0], imuData->gyro [1], imuData->gyro [2] ); + SurvivePose object_pose_out; + quatrotateabout(object_pose_out.Rot, object_pose->Rot, applymotion ); + copy3d( object_pose_out.Pos, object_pose->Pos ); + PoserData_poser_raw_pose_func(pd, so, 0, &object_pose_out); + return 0; } case POSERDATA_LIGHT: { -- cgit v1.2.3 From 22301a092b4009c4f2ca2bc8be4511d75e3082b6 Mon Sep 17 00:00:00 2001 From: Charles Lohr Date: Wed, 4 Apr 2018 04:06:31 +0000 Subject: Tweak to paraemters to fix performance of gyro --- src/poser_charlesrefine.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 9aa44d9..c1cc6de 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -42,9 +42,9 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { //TODO: Actually do Madgwick's algorithm LinmathQuat applymotion; const SurvivePose * object_pose = &so->OutPose; - imuData->gyro[0] *= -0.001; - imuData->gyro[1] *= -0.001; - imuData->gyro[2] *= 0.001; + imuData->gyro[0] *= -0.0005; + imuData->gyro[1] *= -0.0005; + imuData->gyro[2] *= 0.0005; quatfromeuler( applymotion, imuData->gyro ); //printf( "%f %f %f\n", imuData->gyro [0], imuData->gyro [1], imuData->gyro [2] ); SurvivePose object_pose_out; @@ -156,8 +156,8 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { #define MIN_HIT_QUALITY 0.5 //Determines which hits to cull. #define HIT_QUALITY_BASELINE 0.0001 //Determines which hits to cull. Actually SQRT(baseline) if 0.0001, it is really 1cm -#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.1 //Explodes if you exceed 1.0 -#define CORRECT_TELESCOPTION_COEFFICIENT 0.5 //Converges even as high as 10.0 and doesn't explode. +#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.8 //Explodes if you exceed 1.0 +#define CORRECT_TELESCOPTION_COEFFICIENT 8.0 //Converges even as high as 10.0 and doesn't explode. #define CORRECT_ROTATION_COEFFICIENT 1.0 //This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. #define ROTATIONAL_CORRECTION_MAXFORCE 0.10 -- cgit v1.2.3 From acba33deca862be657f57876de7de950dd1d9af4 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Tue, 3 Apr 2018 01:05:48 -0400 Subject: Throw in my Wip poser. --- src/poser_charlesrefine.c | 302 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 302 insertions(+) create mode 100644 src/poser_charlesrefine.c (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c new file mode 100644 index 0000000..4d44722 --- /dev/null +++ b/src/poser_charlesrefine.c @@ -0,0 +1,302 @@ +//EXPERIMENTAL DRIVER - DO NOT USE + +#include +#include +#include + +#include "epnp/epnp.h" +#include "linmath.h" +#include +#include +#include + +#define MAX_PT_PER_SWEEP 32 + + +typedef struct +{ + int sweepaxis; + int sweeplh; + FLT normal_at_errors[MAX_PT_PER_SWEEP][3]; + FLT quantity_errors[MAX_PT_PER_SWEEP] + uint8_t sensor_ids[MAX_PT_PER_SWEEP]; + int ptsweep; +} CharlesPoserData; + + + +int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { + CharlesPoserData * dd = so->PoserData; + if( !dd ) so->PoserData = dd = calloc( sizeof(CharlesPoserData), 1 ); + + SurviveSensorActivations *scene = &so->activations; + switch (pd->pt) { + case POSERDATA_IMU: { + // Really should use this... + PoserDataIMU *imuData = (PoserDataIMU *)pd; + return 0; + } + case POSERDATA_LIGHT: { + int i; + PoserDataLight *ld = (PoserDataLight *)pd; + int lhid = ld->lh; + int senid = ld->sensor_id; + BaseStationData * bsd = &so->ctx->bsd[ld->lh]; + if( !bsd->PositionSet ) break; + SurvivePose * lhp = &bsd->Pose; + FLT angle = ld->angle; + int sensor_id = ld->sensor_id; + int axis = dd->sweepaxis; + const SurvivePose * object_pose = &so->OutPose; + dd->sweeplh = lhid; + + //FOR NOW, drop LH1. + if( lhid == 1 ) break; + + +// const FLT * sensor_normal = &so->sensor_normals[senid*3]; +// FLT sensor_normal_worldspace[3]; +// ApplyPoseToPoint(sensor_normal_worldspace, object_pose, sensor_inpos); + + const FLT * sensor_inpos = &so->sensor_locations[senid*3]; + FLT sensor_position_worldspace[3]; + ApplyPoseToPoint(sensor_position_worldspace, object_pose, sensor_inpos); + //printf( "%f %f %f == > %f %f %f\n", sensor_inpos[0], sensor_inpos[1], sensor_inpos[2], sensor_position_worldspace[0], sensor_position_worldspace[1], sensor_position_worldspace[2] ); + // = sensor position, relative to lighthouse center. + FLT sensorpos_rel_lh[3]; + sub3d( sensorpos_rel_lh, sensor_position_worldspace, lhp->Pos ); + + //Next, define a normal in global space of the plane created by the sweep hit. + //Careful that this must be normalized. + FLT sweep_normal[3]; + + //If 1, the "y" axis. //XXX Check me. + if( axis ) //XXX Just FYI this should include account for skew + { + sweep_normal[0] = 0; + sweep_normal[1] = cos(angle ); + sweep_normal[2] = sin( angle ); + //printf( "+" ); + } + else + { + sweep_normal[0] = cos( angle ); + sweep_normal[1] = 0; + sweep_normal[2] = -sin( angle ); + //printf( "-" ); + } + + //Need to apply the lighthouse's transformation to the sweep's normal. + quatrotatevector( sweep_normal, lhp->Rot, sweep_normal); + + //Compute point-line distance between sensorpos_rel_lh and the plane defined by sweep_normal. + //Do this by projecting sensorpos_rel_lh (w) onto sweep_normal (v). + //You can do this by |v dot w| / |v| ... But we know |v| is 1. So... + FLT dist = dot3d( sensorpos_rel_lh, sweep_normal ); + + if( (i = dd->ptsweep) < MAX_PT_PER_SWEEP ) + { + dd->normal_at_errors[i] = sweep_normal; + dd->quantity_errors[i] = dist; + dd->sensor_ids[i] = sensor_id; + dd->ptsweep++: + } + +#if 0 + printf( "D %d %d: %f [%f %f %f]\n", lhid, axis, dist, sweep_normal[0], sweep_normal[1], sweep_normal[2] ); + + + //Naieve approach... Push it in the right direction + SurvivePose object_pose_out; + quatcopy( object_pose_out.Rot, object_pose->Rot ); + scale3d(sweep_normal, sweep_normal, -0.1*dist); + add3d(object_pose_out.Pos, sweep_normal, object_pose->Pos); + + if( so->PoseConfidence < .01 ) + { + dd->average_nudge[0] = 0; + dd->average_nudge[1] = 0; + dd->average_nudge[2] = 0; + + memcpy( &object_pose_out, &LinmathPose_Identity, sizeof( LinmathPose_Identity ) ); + object_pose_out.Pos[1] = 2.5; + object_pose_out.Pos[2] = 1.8; + so->PoseConfidence = 1.0; + } +// printf( "%f %f %f %f\n", object_pose->Rot[0], object_pose->Rot[1], object_pose->Rot[2], object_pose->Rot[3] ); + + PoserData_poser_raw_pose_func(pd, so, lhid, &object_pose_out); +#endif + +#if 0 +// = { + axis?0.0:sin(angle), + axis?sin(angle):0.0, + cos(angle) }; + + = sensor_locations; + LinmathPoint3d + int8_t sensor_ct; // sensor count + FLT *sensor_locations; // size is sensor_ct*3. Contains x,y,z values for each sensor + FLT *sensor_normals; // size is nrlocations*3. cointains normal vector for each sensor + + + // This is the quat equivalent of 'pout = pose * pin' if pose were a 4x4 matrix in homogenous space +void ApplyPoseToPoint(LinmathPoint3d pout, const LinmathPose *pose, const LinmathPoint3d pin); + + + + SurvivePose obj2world; +// ApplyPoseToPose(&obj2world, &lh2world, &objpose); + memcpy( &obj2world, &LinmathPose_Identity, sizeof( obj2world ) ); + obj2world.Pos[1] = 1; + PoserData_poser_raw_pose_func(pd, so, lhid, &obj2world); +// PoserData_poser_raw_pose_func(pd, so, ld->lh, &posers[lightData->lh]); + + + // Pose Information, also "poser" field. + ///from SurviveObject + // FLT PoseConfidence; // 0..1 + // SurvivePose OutPose; // Final pose? (some day, one can dream!) + // SurvivePose FromLHPose[NUM_LIGHTHOUSES]; // Filled out by poser, contains computed position from each lighthouse. + // void *PoserData; // Initialized to zero, configured by poser, can be anything the poser wants. + // PoserCB PoserFn; + + + // printf( "%d %d %f\n", ld->sensor_id, ld->lh, ld->angle ); + +/* + SurvivePose *object_pose, void *user); + +typedef struct +{ + PoserType pt; + poser_raw_pose_func rawposeproc; + poser_lighthouse_pose_func lighthouseposeproc; + void *userdata; +} PoserData; + + PoserData hdr; + int sensor_id; + int acode; //OOTX Code associated with this sweep. bit 1 indicates vertical(1) or horizontal(0) sweep + int lh; //Lighthouse making this sweep + uint32_t timecode; //In object-local ticks. + FLT length; //In seconds + FLT angle; //In radians from center of lighthouse. +} PoserDataLight; + + + +*/ +#if 0 + SurvivePose posers[2]; + int meas[2] = {0, 0}; + for (int lh = 0; lh < so->ctx->activeLighthouses; lh++) { + if (so->ctx->bsd[lh].PositionSet) { + epnp pnp = {.fu = 1, .fv = 1}; + epnp_set_maximum_number_of_correspondences(&pnp, so->sensor_ct); + + add_correspondences(so, &pnp, scene, lightData->timecode, lh); + 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 > required_meas) { + + SurvivePose objInLh = solve_correspondence(so, &pnp, false); + if (quatmagnitude(objInLh.Rot) != 0) { + SurvivePose *lh2world = &so->ctx->bsd[lh].Pose; + + SurvivePose txPose = {.Rot = {1}}; + ApplyPoseToPose(&txPose, lh2world, &objInLh); + posers[lh] = txPose; + meas[lh] = pnp.number_of_correspondences; + } + } + + epnp_dtor(&pnp); + } + } + + if (meas[0] > 0 && meas[1] > 0) { + SurvivePose interpolate = {0}; + bool winnerTakesAll = true; // Not convinced slerp does the right thing, will change this when i am + + if (winnerTakesAll) { + int winner = meas[0] > meas[1] ? 0 : 1; + PoserData_poser_raw_pose_func(pd, so, winner, &posers[winner]); + } else { + double a, b; + a = meas[0] * meas[0]; + b = meas[1] * meas[1]; + + double t = a + b; + for (size_t i = 0; i < 3; i++) { + interpolate.Pos[i] = (posers[0].Pos[i] * a + posers[1].Pos[i] * b) / (t); + } + quatslerp(interpolate.Rot, posers[0].Rot, posers[1].Rot, b / (t)); + PoserData_poser_raw_pose_func(pd, so, lightData->lh, &interpolate); + } + } else { + if (meas[lightData->lh]) + PoserData_poser_raw_pose_func(pd, so, lightData->lh, &posers[lightData->lh]); + } +#endif + +#endif + return 0; + } + + case POSERDATA_SYNC: { + PoserDataLight *l = (PoserDataLight *)pd; + int lhid = l->lh; + + //you can get sweepaxis and sweeplh. + + if( dd->ptsweep ) + { + int i; + int lhid = dd->lhid; + int pts = dd->ptsweep; + const SurvivePose * object_pose = &so->OutPose; + + FLT avg_err[3] = { 0, 0, 0 }; + FLT avgtot = 0.0; + for( i = 0; i < pts; i++ ) + { + FLT * nrm = dd->normal_at_errors[pts]; + FLT qty = quantity_errors[pts]; + avgtot += qty; + avg_err[0] = avg_err[0] + nrm[0] * qty; + avg_err[1] = avg_err[1] + nrm[1] * qty; + avg_err[2] = avg_err[2] + nrm[2] * qty; + } + scale3d(avg_err, avg_err, 1./pts); + //We have "Average error" now. This is a world space value. + //This can correct for lateral error, but not distance from camera. + + //Next we need to find out what the weighting is to determine "zoom" + //How do we do this? ??? Too tired to math. + FLT weight = 0.0; + for( i = 0; i < pts; i++ ) + { + //??!?!? Sturfff + } + + dd->ptsweep = 0; + + //Update PoserData_poser_raw_pose_func(pd, so, lhid, &object_pose_out); + } + + dd->nextaxis = l->acode & 1; + printf( "SYNC %d %p\n", l->acode, dd ); + break; + } + case POSERDATA_FULL_SCENE: { + //return opencv_solver_fullscene(so, (PoserDataFullScene *)(pd)); + } + } + return -1; +} + +REGISTER_LINKTIME(PoserCharlesRefine); -- cgit v1.2.3 From 3298e29594c0655e92afca195e6ef13582e5f017 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Tue, 3 Apr 2018 21:56:50 -0400 Subject: The reuslts are incredibly stable. Need to test on the opi. --- src/poser_charlesrefine.c | 366 +++++++++++++++++++++++++--------------------- 1 file changed, 201 insertions(+), 165 deletions(-) (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 4d44722..e6f6a57 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -9,6 +9,7 @@ #include #include #include +#include #define MAX_PT_PER_SWEEP 32 @@ -17,8 +18,10 @@ typedef struct { int sweepaxis; int sweeplh; - FLT normal_at_errors[MAX_PT_PER_SWEEP][3]; - FLT quantity_errors[MAX_PT_PER_SWEEP] + FLT normal_at_errors[MAX_PT_PER_SWEEP][3]; //Value is actually normalized, not just normal to sweep plane. + FLT quantity_errors[MAX_PT_PER_SWEEP]; + FLT angles_at_pts[MAX_PT_PER_SWEEP]; + SurvivePose object_pose_at_hit[MAX_PT_PER_SWEEP]; uint8_t sensor_ids[MAX_PT_PER_SWEEP]; int ptsweep; } CharlesPoserData; @@ -51,7 +54,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { dd->sweeplh = lhid; //FOR NOW, drop LH1. - if( lhid == 1 ) break; + //if( lhid == 1 ) break; // const FLT * sensor_normal = &so->sensor_normals[senid*3]; @@ -60,7 +63,11 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { const FLT * sensor_inpos = &so->sensor_locations[senid*3]; FLT sensor_position_worldspace[3]; + //XXX Once I saw this get pretty wild (When in playback) + //I had to invert the values of sensor_inpos. Not sure why. ApplyPoseToPoint(sensor_position_worldspace, object_pose, sensor_inpos); + + //printf( "%f %f %f == > %f %f %f\n", sensor_inpos[0], sensor_inpos[1], sensor_inpos[2], sensor_position_worldspace[0], sensor_position_worldspace[1], sensor_position_worldspace[2] ); // = sensor position, relative to lighthouse center. FLT sensorpos_rel_lh[3]; @@ -96,200 +103,229 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { if( (i = dd->ptsweep) < MAX_PT_PER_SWEEP ) { - dd->normal_at_errors[i] = sweep_normal; + memcpy( dd->normal_at_errors[i], sweep_normal, sizeof(FLT)*3 ); dd->quantity_errors[i] = dist; + dd->angles_at_pts[i] = angle; dd->sensor_ids[i] = sensor_id; - dd->ptsweep++: + memcpy( &dd->object_pose_at_hit[i], object_pose, sizeof(SurvivePose) ); + dd->ptsweep++; } -#if 0 - printf( "D %d %d: %f [%f %f %f]\n", lhid, axis, dist, sweep_normal[0], sweep_normal[1], sweep_normal[2] ); + return 0; + } + case POSERDATA_SYNC: { + PoserDataLight *l = (PoserDataLight *)pd; + int lhid = l->lh; - //Naieve approach... Push it in the right direction - SurvivePose object_pose_out; - quatcopy( object_pose_out.Rot, object_pose->Rot ); - scale3d(sweep_normal, sweep_normal, -0.1*dist); - add3d(object_pose_out.Pos, sweep_normal, object_pose->Pos); - if( so->PoseConfidence < .01 ) + //you can get sweepaxis and sweeplh. + if( dd->ptsweep ) { - dd->average_nudge[0] = 0; - dd->average_nudge[1] = 0; - dd->average_nudge[2] = 0; - - memcpy( &object_pose_out, &LinmathPose_Identity, sizeof( LinmathPose_Identity ) ); - object_pose_out.Pos[1] = 2.5; - object_pose_out.Pos[2] = 1.8; - so->PoseConfidence = 1.0; - } -// printf( "%f %f %f %f\n", object_pose->Rot[0], object_pose->Rot[1], object_pose->Rot[2], object_pose->Rot[3] ); - - PoserData_poser_raw_pose_func(pd, so, lhid, &object_pose_out); -#endif - -#if 0 -// = { - axis?0.0:sin(angle), - axis?sin(angle):0.0, - cos(angle) }; - - = sensor_locations; - LinmathPoint3d - int8_t sensor_ct; // sensor count - FLT *sensor_locations; // size is sensor_ct*3. Contains x,y,z values for each sensor - FLT *sensor_normals; // size is nrlocations*3. cointains normal vector for each sensor - - - // This is the quat equivalent of 'pout = pose * pin' if pose were a 4x4 matrix in homogenous space -void ApplyPoseToPoint(LinmathPoint3d pout, const LinmathPose *pose, const LinmathPoint3d pin); + int i; + int lhid = dd->sweeplh; + int axis = dd->sweepaxis; + int pts = dd->ptsweep; + const SurvivePose * object_pose = &so->OutPose; //XXX TODO Should pull pose from approximate time when LHs were scanning it. + BaseStationData * bsd = &so->ctx->bsd[lhid]; + SurvivePose * lh_pose = &bsd->Pose; + int validpoints = 0; + int ptvalid[MAX_PT_PER_SWEEP]; + FLT avgerr = 0.0; + FLT vec_correct[3] = { 0., 0. , 0. }; + FLT avgang = 0.0; - SurvivePose obj2world; -// ApplyPoseToPose(&obj2world, &lh2world, &objpose); - memcpy( &obj2world, &LinmathPose_Identity, sizeof( obj2world ) ); - obj2world.Pos[1] = 1; - PoserData_poser_raw_pose_func(pd, so, lhid, &obj2world); -// PoserData_poser_raw_pose_func(pd, so, ld->lh, &posers[lightData->lh]); +//Tunable parameters: +#define MIN_HIT_QUALITY 0.5 //Determines which hits to cull. +#define HIT_QUALITY_BASELINE 0.0001 //Determines which hits to cull. Actually SQRT(baseline) if 0.0001, it is really 1cm +#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.2 //Explodes if you exceed 1.0 +#define CORRECT_TELESCOPTION_COEFFICIENT 1.0 //Converges even as high as 10.0 and doesn't explode. +#define CORRECT_ROTATION_COEFFICIENT 5.0 //This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. +#define ROTATIONAL_CORRECTION_MAXFORCE 0.10 - // Pose Information, also "poser" field. - ///from SurviveObject - // FLT PoseConfidence; // 0..1 - // SurvivePose OutPose; // Final pose? (some day, one can dream!) - // SurvivePose FromLHPose[NUM_LIGHTHOUSES]; // Filled out by poser, contains computed position from each lighthouse. - // void *PoserData; // Initialized to zero, configured by poser, can be anything the poser wants. - // PoserCB PoserFn; + //Step 1: Determine standard of deviation, and average in order to + // drop points that are likely in error. + { + //Calculate average + FLT avgerr_orig = 0.0; + FLT stddevsq = 0.0; + for( i = 0; i < pts; i++ ) + avgerr_orig += dd->quantity_errors[i]; + avgerr_orig/=pts; + + //Calculate standard of deviation. + for( i = 0; i < pts; i++ ) + { + FLT diff = dd->quantity_errors[i]-avgerr_orig; + stddevsq += diff*diff; + } + stddevsq/=pts; + + for( i = 0; i < pts; i++ ) + { + FLT err = dd->quantity_errors[i]; + FLT diff = err-avgerr_orig; + diff *= diff; + int isptvalid = (diff * MIN_HIT_QUALITY <= stddevsq + HIT_QUALITY_BASELINE)?1:0; + ptvalid[i] = isptvalid; + if( isptvalid ) + { + avgang += dd->angles_at_pts[i]; + avgerr += err; + validpoints ++; + } + } + avgang /= validpoints; + avgerr /= validpoints; + } + //Step 2: Determine average lateral error. + //We can actually always perform this operation. Even with only one point. + { + FLT avg_err[3] = { 0, 0, 0 }; //Positional error. + for( i = 0; i < pts; i++ ) + { + if( !ptvalid[i] ) continue; + FLT * nrm = dd->normal_at_errors[i]; + FLT err = dd->quantity_errors[i]; + avg_err[0] = avg_err[0] + nrm[0] * err; + avg_err[1] = avg_err[1] + nrm[1] * err; + avg_err[2] = avg_err[2] + nrm[2] * err; + } - // printf( "%d %d %f\n", ld->sensor_id, ld->lh, ld->angle ); + //NOTE: The "avg_err" is not geometrically centered. This is actually + //probably okay, since if you have sevearl data points to one side, you + //can probably trust that more. + scale3d(avg_err, avg_err, 1./validpoints); -/* - SurvivePose *object_pose, void *user); + //We have "Average error" now. A vector in worldspace. + //This can correct for lateral error, but not distance from camera. -typedef struct -{ - PoserType pt; - poser_raw_pose_func rawposeproc; - poser_lighthouse_pose_func lighthouseposeproc; - void *userdata; -} PoserData; - - PoserData hdr; - int sensor_id; - int acode; //OOTX Code associated with this sweep. bit 1 indicates vertical(1) or horizontal(0) sweep - int lh; //Lighthouse making this sweep - uint32_t timecode; //In object-local ticks. - FLT length; //In seconds - FLT angle; //In radians from center of lighthouse. -} PoserDataLight; - - - -*/ -#if 0 - SurvivePose posers[2]; - int meas[2] = {0, 0}; - for (int lh = 0; lh < so->ctx->activeLighthouses; lh++) { - if (so->ctx->bsd[lh].PositionSet) { - epnp pnp = {.fu = 1, .fv = 1}; - epnp_set_maximum_number_of_correspondences(&pnp, so->sensor_ct); - - add_correspondences(so, &pnp, scene, lightData->timecode, lh); - 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 > required_meas) { - - SurvivePose objInLh = solve_correspondence(so, &pnp, false); - if (quatmagnitude(objInLh.Rot) != 0) { - SurvivePose *lh2world = &so->ctx->bsd[lh].Pose; - - SurvivePose txPose = {.Rot = {1}}; - ApplyPoseToPose(&txPose, lh2world, &objInLh); - posers[lh] = txPose; - meas[lh] = pnp.number_of_correspondences; - } - } - - epnp_dtor(&pnp); + //XXX TODO: Should we check to see if we only have one or + //two points to make sure the error on this isn't unusually high? + //If calculated error is unexpectedly high, then we should probably + //Not apply the transform. + scale3d( avg_err, avg_err, -CORRECT_LATERAL_POSITION_COEFFICIENT ); + add3d( vec_correct, vec_correct, avg_err ); } - } - if (meas[0] > 0 && meas[1] > 0) { - SurvivePose interpolate = {0}; - bool winnerTakesAll = true; // Not convinced slerp does the right thing, will change this when i am - - if (winnerTakesAll) { - int winner = meas[0] > meas[1] ? 0 : 1; - PoserData_poser_raw_pose_func(pd, so, winner, &posers[winner]); - } else { - double a, b; - a = meas[0] * meas[0]; - b = meas[1] * meas[1]; - - double t = a + b; - for (size_t i = 0; i < 3; i++) { - interpolate.Pos[i] = (posers[0].Pos[i] * a + posers[1].Pos[i] * b) / (t); + //Step 3: Control telecoption from lighthouse. + // we need to find out what the weighting is to determine "zoom" + if( validpoints > 1 ) //Can't correct "zoom" with only one point. + { + FLT zoom = 0.0; + FLT rmsang = 0.0; + for( i = 0; i < pts; i++ ) + { + if( !ptvalid[i] ) continue; + FLT delang = dd->angles_at_pts[i] - avgang; + FLT delerr = dd->quantity_errors[i] - avgerr; + if( axis ) delang *= -1; //Flip sign on alternate axis because it's measured backwards. + zoom += delerr * delang; + rmsang += delang * delang; } - quatslerp(interpolate.Rot, posers[0].Rot, posers[1].Rot, b / (t)); - PoserData_poser_raw_pose_func(pd, so, lightData->lh, &interpolate); - } - } else { - if (meas[lightData->lh]) - PoserData_poser_raw_pose_func(pd, so, lightData->lh, &posers[lightData->lh]); - } -#endif -#endif - return 0; - } + //Control into or outof lighthouse. + //XXX Check to see if we need to sqrt( the rmsang), need to check convergance behavior close to lighthouse. + //This is a questionable step. + zoom /= sqrt(rmsang); - case POSERDATA_SYNC: { - PoserDataLight *l = (PoserDataLight *)pd; - int lhid = l->lh; + zoom *= CORRECT_TELESCOPTION_COEFFICIENT; - //you can get sweepaxis and sweeplh. + FLT veccamalong[3]; + sub3d( veccamalong, lh_pose->Pos, object_pose->Pos ); + normalize3d( veccamalong, veccamalong ); + scale3d( veccamalong, veccamalong, zoom ); + add3d( vec_correct, veccamalong, vec_correct ); + } - if( dd->ptsweep ) - { - int i; - int lhid = dd->lhid; - int pts = dd->ptsweep; - const SurvivePose * object_pose = &so->OutPose; - FLT avg_err[3] = { 0, 0, 0 }; - FLT avgtot = 0.0; - for( i = 0; i < pts; i++ ) - { - FLT * nrm = dd->normal_at_errors[pts]; - FLT qty = quantity_errors[pts]; - avgtot += qty; - avg_err[0] = avg_err[0] + nrm[0] * qty; - avg_err[1] = avg_err[1] + nrm[1] * qty; - avg_err[2] = avg_err[2] + nrm[2] * qty; + SurvivePose object_pose_out; + add3d(object_pose_out.Pos, vec_correct, object_pose->Pos); + + quatcopy( object_pose_out.Rot, object_pose->Rot ); + + //Stage 4: "Tug" on the rotation of the object, from all of the sensor's pov. + //If we were able to determine likliehood of a hit in the sweep instead of afterward + //we would actually be able to perform this on a per-hit basis. + if( 1 ) { + LinmathQuat correction; + quatcopy( correction, LinmathQuat_Identity ); + for( i = 0; i < pts; i++ ) + { + if( !ptvalid[i] ) continue; + FLT dist = dd->quantity_errors[i]-avgerr; + FLT angle = dd->angles_at_pts[i]; + int sensor_id = dd->sensor_ids[i]; + FLT * normal = dd->normal_at_errors[i]; + const SurvivePose * object_pose_at_hit = &dd->object_pose_at_hit[i]; + const FLT * sensor_inpos = &so->sensor_locations[sensor_id*3]; + + LinmathQuat world_to_object_space; + quatgetreciprocal(world_to_object_space, object_pose_at_hit->Rot); + FLT correction_in_object_space[3]; //The amount across the surface of the object the rotation should happen. + + quatrotatevector(correction_in_object_space, world_to_object_space, normal ); + dist *= CORRECT_ROTATION_COEFFICIENT; + if( dist > ROTATIONAL_CORRECTION_MAXFORCE ) dist = ROTATIONAL_CORRECTION_MAXFORCE; + if( dist <-ROTATIONAL_CORRECTION_MAXFORCE ) dist =-ROTATIONAL_CORRECTION_MAXFORCE; + + //Now, we have a "tug" vector in object-local space. Need to apply the torque. + FLT vector_from_center_of_object[3]; + normalize3d( vector_from_center_of_object, sensor_inpos ); + //scale3d(vector_from_center_of_object, sensor_inpos, 10.0 ); + // vector_from_center_of_object[2]*=-1; + // vector_from_center_of_object[1]*=-1; +// vector_from_center_of_object[0]*=-1; + //vector_from_center_of_object + scale3d(vector_from_center_of_object,vector_from_center_of_object, 1); + + + FLT new_vector_in_object_space[3]; + //printf( "%f %f %f %f\n", object_pose_at_hit->Rot[0], object_pose_at_hit->Rot[1], object_pose_at_hit->Rot[2], object_pose_at_hit->Rot[3] ); + //printf( "%f %f %f // %f %f %f // %f\n", vector_from_center_of_object[0], vector_from_center_of_object[1], vector_from_center_of_object[2], correction_in_object_space[0], correction_in_object_space[1], correction_in_object_space[2], dist ); + scale3d( correction_in_object_space, correction_in_object_space, -dist ); + add3d( new_vector_in_object_space, vector_from_center_of_object, correction_in_object_space ); + + normalize3d( new_vector_in_object_space, new_vector_in_object_space ); + + LinmathQuat corrective_quaternion; + quatfrom2vectors(corrective_quaternion, vector_from_center_of_object, new_vector_in_object_space ); + quatrotateabout( correction, correction, corrective_quaternion ); + //printf( "%f -> %f %f %f => %f %f %f [%f %f %f %f]\n", dist, vector_from_center_of_object[0], vector_from_center_of_object[1], vector_from_center_of_object[2], + //correction_in_object_space[0], correction_in_object_space[1], correction_in_object_space[2], + //corrective_quaternion[0],corrective_quaternion[1],corrective_quaternion[1],corrective_quaternion[3]); + } + printf( "Applying: %f %f %f %f\n", correction[0], correction[1], correction[2], correction[3] ); + //Apply our corrective quaternion to the output. + quatrotateabout( object_pose_out.Rot, object_pose_out.Rot, correction ); + quatnormalize( object_pose_out.Rot, object_pose_out.Rot ); } - scale3d(avg_err, avg_err, 1./pts); - //We have "Average error" now. This is a world space value. - //This can correct for lateral error, but not distance from camera. - - //Next we need to find out what the weighting is to determine "zoom" - //How do we do this? ??? Too tired to math. - FLT weight = 0.0; - for( i = 0; i < pts; i++ ) + + //Janky need to do this somewhere else... This initializes the pose estimator. + if( so->PoseConfidence < .01 ) { - //??!?!? Sturfff + memcpy( &object_pose_out, &LinmathPose_Identity, sizeof( LinmathPose_Identity ) ); + object_pose_out.Pos[0] = -0.14372776; + object_pose_out.Pos[1] = 0.06856518; + object_pose_out.Pos[2] = 0.01960009; + object_pose_out.Rot[0] = 1.0; + object_pose_out.Rot[1] = -0.0; + object_pose_out.Rot[2] = 0.0; + object_pose_out.Rot[3] = 0.0; + so->PoseConfidence = 1.0; } - dd->ptsweep = 0; + PoserData_poser_raw_pose_func(pd, so, lhid, &object_pose_out); - //Update PoserData_poser_raw_pose_func(pd, so, lhid, &object_pose_out); + dd->ptsweep = 0; } - dd->nextaxis = l->acode & 1; - printf( "SYNC %d %p\n", l->acode, dd ); + dd->sweepaxis = l->acode & 1; + //printf( "SYNC %d %p\n", l->acode, dd ); break; } case POSERDATA_FULL_SCENE: { -- cgit v1.2.3 From 438ae07af28fbf6f63b7598bc2de21a61222a989 Mon Sep 17 00:00:00 2001 From: Charles Lohr Date: Wed, 4 Apr 2018 03:10:21 +0000 Subject: Ok, it works! --- src/poser_charlesrefine.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index e6f6a57..61bd575 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -141,9 +141,9 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { #define MIN_HIT_QUALITY 0.5 //Determines which hits to cull. #define HIT_QUALITY_BASELINE 0.0001 //Determines which hits to cull. Actually SQRT(baseline) if 0.0001, it is really 1cm -#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.2 //Explodes if you exceed 1.0 -#define CORRECT_TELESCOPTION_COEFFICIENT 1.0 //Converges even as high as 10.0 and doesn't explode. -#define CORRECT_ROTATION_COEFFICIENT 5.0 //This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. +#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.1 //Explodes if you exceed 1.0 +#define CORRECT_TELESCOPTION_COEFFICIENT 0.5 //Converges even as high as 10.0 and doesn't explode. +#define CORRECT_ROTATION_COEFFICIENT 1.0 //This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. #define ROTATIONAL_CORRECTION_MAXFORCE 0.10 //Step 1: Determine standard of deviation, and average in order to @@ -299,7 +299,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { //correction_in_object_space[0], correction_in_object_space[1], correction_in_object_space[2], //corrective_quaternion[0],corrective_quaternion[1],corrective_quaternion[1],corrective_quaternion[3]); } - printf( "Applying: %f %f %f %f\n", correction[0], correction[1], correction[2], correction[3] ); + //printf( "Applying: %f %f %f %f\n", correction[0], correction[1], correction[2], correction[3] ); //Apply our corrective quaternion to the output. quatrotateabout( object_pose_out.Rot, object_pose_out.Rot, correction ); quatnormalize( object_pose_out.Rot, object_pose_out.Rot ); -- cgit v1.2.3 From bd7afcf2d1cdc49fc0bf7593cba12d576b227461 Mon Sep 17 00:00:00 2001 From: Charles Lohr Date: Wed, 4 Apr 2018 03:34:25 +0000 Subject: Add gyro support (Still need madgwick's) --- src/poser_charlesrefine.c | 15 +++++++++++++++ 1 file changed, 15 insertions(+) (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 61bd575..9aa44d9 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -37,6 +37,21 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { case POSERDATA_IMU: { // Really should use this... PoserDataIMU *imuData = (PoserDataIMU *)pd; + + + //TODO: Actually do Madgwick's algorithm + LinmathQuat applymotion; + const SurvivePose * object_pose = &so->OutPose; + imuData->gyro[0] *= -0.001; + imuData->gyro[1] *= -0.001; + imuData->gyro[2] *= 0.001; + quatfromeuler( applymotion, imuData->gyro ); + //printf( "%f %f %f\n", imuData->gyro [0], imuData->gyro [1], imuData->gyro [2] ); + SurvivePose object_pose_out; + quatrotateabout(object_pose_out.Rot, object_pose->Rot, applymotion ); + copy3d( object_pose_out.Pos, object_pose->Pos ); + PoserData_poser_raw_pose_func(pd, so, 0, &object_pose_out); + return 0; } case POSERDATA_LIGHT: { -- cgit v1.2.3 From 3ee1f6d7e9c2e2318dddc34495e6c6af25be3ffe Mon Sep 17 00:00:00 2001 From: Charles Lohr Date: Wed, 4 Apr 2018 04:06:31 +0000 Subject: Tweak to paraemters to fix performance of gyro --- src/poser_charlesrefine.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 9aa44d9..c1cc6de 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -42,9 +42,9 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { //TODO: Actually do Madgwick's algorithm LinmathQuat applymotion; const SurvivePose * object_pose = &so->OutPose; - imuData->gyro[0] *= -0.001; - imuData->gyro[1] *= -0.001; - imuData->gyro[2] *= 0.001; + imuData->gyro[0] *= -0.0005; + imuData->gyro[1] *= -0.0005; + imuData->gyro[2] *= 0.0005; quatfromeuler( applymotion, imuData->gyro ); //printf( "%f %f %f\n", imuData->gyro [0], imuData->gyro [1], imuData->gyro [2] ); SurvivePose object_pose_out; @@ -156,8 +156,8 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { #define MIN_HIT_QUALITY 0.5 //Determines which hits to cull. #define HIT_QUALITY_BASELINE 0.0001 //Determines which hits to cull. Actually SQRT(baseline) if 0.0001, it is really 1cm -#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.1 //Explodes if you exceed 1.0 -#define CORRECT_TELESCOPTION_COEFFICIENT 0.5 //Converges even as high as 10.0 and doesn't explode. +#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.8 //Explodes if you exceed 1.0 +#define CORRECT_TELESCOPTION_COEFFICIENT 8.0 //Converges even as high as 10.0 and doesn't explode. #define CORRECT_ROTATION_COEFFICIENT 1.0 //This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. #define ROTATIONAL_CORRECTION_MAXFORCE 0.10 -- cgit v1.2.3 From 736443dca52d6800224db4d0646b9617f79a219b Mon Sep 17 00:00:00 2001 From: Charles Lohr Date: Wed, 4 Apr 2018 04:33:01 +0000 Subject: Fix Merge --- src/poser_charlesrefine.c | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index c1cc6de..372ccb1 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -1,4 +1,4 @@ -//EXPERIMENTAL DRIVER - DO NOT USE +//Driver works, but you _must_ start it near the origin looking in +Z. #include #include @@ -50,7 +50,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { SurvivePose object_pose_out; quatrotateabout(object_pose_out.Rot, object_pose->Rot, applymotion ); copy3d( object_pose_out.Pos, object_pose->Pos ); - PoserData_poser_raw_pose_func(pd, so, 0, &object_pose_out); + PoserData_poser_pose_func(pd, so, &object_pose_out); return 0; } @@ -324,17 +324,10 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { if( so->PoseConfidence < .01 ) { memcpy( &object_pose_out, &LinmathPose_Identity, sizeof( LinmathPose_Identity ) ); - object_pose_out.Pos[0] = -0.14372776; - object_pose_out.Pos[1] = 0.06856518; - object_pose_out.Pos[2] = 0.01960009; - object_pose_out.Rot[0] = 1.0; - object_pose_out.Rot[1] = -0.0; - object_pose_out.Rot[2] = 0.0; - object_pose_out.Rot[3] = 0.0; so->PoseConfidence = 1.0; } - PoserData_poser_raw_pose_func(pd, so, lhid, &object_pose_out); + PoserData_poser_pose_func(pd, so, &object_pose_out); dd->ptsweep = 0; } -- cgit v1.2.3 From 779a7466efc0e0657e7048108be35645faede030 Mon Sep 17 00:00:00 2001 From: Charles Lohr Date: Wed, 4 Apr 2018 04:35:12 +0000 Subject: Fix merge conflicts --- src/poser_charlesrefine.c | 19 ------------------- 1 file changed, 19 deletions(-) (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 266248c..f9c60e2 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -50,11 +50,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { SurvivePose object_pose_out; quatrotateabout(object_pose_out.Rot, object_pose->Rot, applymotion ); copy3d( object_pose_out.Pos, object_pose->Pos ); -<<<<<<< HEAD PoserData_poser_pose_func(pd, so, &object_pose_out); -======= - PoserData_poser_raw_pose_func(pd, so, 0, &object_pose_out); ->>>>>>> 22301a092b4009c4f2ca2bc8be4511d75e3082b6 return 0; } @@ -328,25 +324,10 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { if( so->PoseConfidence < .01 ) { memcpy( &object_pose_out, &LinmathPose_Identity, sizeof( LinmathPose_Identity ) ); -<<<<<<< HEAD so->PoseConfidence = 1.0; } PoserData_poser_pose_func(pd, so, &object_pose_out); -======= - object_pose_out.Pos[0] = -0.14372776; - object_pose_out.Pos[1] = 0.06856518; - object_pose_out.Pos[2] = 0.01960009; - object_pose_out.Rot[0] = 1.0; - object_pose_out.Rot[1] = -0.0; - object_pose_out.Rot[2] = 0.0; - object_pose_out.Rot[3] = 0.0; - so->PoseConfidence = 1.0; - } - - PoserData_poser_raw_pose_func(pd, so, lhid, &object_pose_out); ->>>>>>> 22301a092b4009c4f2ca2bc8be4511d75e3082b6 - dd->ptsweep = 0; } -- cgit v1.2.3