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/poser_charlesrefine.c') 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