From 2e980136715acd4692dd61be4442dc823219764d Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 15 Mar 2018 14:43:58 -0600 Subject: Results out of epnp --- Makefile | 2 +- data_recorder.c | 20 ++++++-- redist/json_helpers.c | 5 +- redist/linmath.c | 31 ++++++++++++ redist/linmath.h | 1 + src/epnp/opencv_shim.h | 2 + src/poser_epnp.c | 130 +++++++++++++++++++++++++++++++++++++++++++++++++ src/survive.c | 4 +- 8 files changed, 186 insertions(+), 9 deletions(-) create mode 100644 src/poser_epnp.c diff --git a/Makefile b/Makefile index c61ce8e..b619970 100644 --- a/Makefile +++ b/Makefile @@ -31,7 +31,7 @@ GRAPHICS_LOFI:=redist/CNFGFunctions.o redist/CNFGXDriver.o endif -POSERS:=src/poser_dummy.o src/poser_daveortho.o src/poser_charlesslow.o src/poser_octavioradii.o src/poser_turveytori.o +POSERS:=src/poser_dummy.o src/poser_daveortho.o src/poser_charlesslow.o src/poser_octavioradii.o src/poser_turveytori.o src/poser_epnp.o REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/os_generic.o ifeq ($(UNAME), Darwin) REDISTS:=$(REDISTS) redist/hid-osx.c diff --git a/data_recorder.c b/data_recorder.c index 8116946..fbc9bdc 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -62,6 +62,16 @@ void write_to_output(const char *format, ...) { va_end(args); } +void my_lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose) { + survive_default_lighthouse_pose_process(ctx, lighthouse, pose); + write_to_output("LH_POSE %d %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f\n", lighthouse, pose->Pos[0], pose->Pos[1], + pose->Pos[2], pose->Rot[0], pose->Rot[1], pose->Rot[2], pose->Rot[3]); +} +void testprog_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { + survive_default_raw_pose_process(so, lighthouse, pose); + write_to_output("POSE %s %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f\n", so->codename, pose->Pos[0], pose->Pos[1], + pose->Pos[2], pose->Rot[0], pose->Rot[1], pose->Rot[2], pose->Rot[3]); +} void my_light_process(struct SurviveObject *so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, @@ -69,13 +79,14 @@ void my_light_process(struct SurviveObject *so, int sensor_id, int acode, survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode, length, lh); - if (acode == -1) { + if (acode == -1 || sensor_id < 0) { write_to_output("A %s %d %d %d %u %u %u\n", so->codename, sensor_id, acode, timeinsweep, timecode, length, lh); return; } int jumpoffset = sensor_id; + if (strcmp(so->codename, "WM0") == 0) jumpoffset += 32; else if (strcmp(so->codename, "WM1") == 0) @@ -170,7 +181,8 @@ void *SurviveThread(void *junk) { survive_install_light_fn(ctx, my_light_process); survive_install_imu_fn(ctx, my_imu_process); - + survive_install_lighthouse_pose_fn(ctx, my_lighthouse_process); + survive_cal_install(ctx); if (!ctx) { fprintf(stderr, "Fatal. Could not start\n"); exit(1); @@ -194,7 +206,8 @@ int main(int argc, char **argv) { } else { output_file = stdout; } - + SurviveThread(0); + /* // Create the libsurvive thread OGCreateThread(SurviveThread, 0); @@ -205,4 +218,5 @@ int main(int argc, char **argv) { // Run the Gui in the main thread GuiThread(0); + */ } diff --git a/redist/json_helpers.c b/redist/json_helpers.c index 109708a..4aeb399 100644 --- a/redist/json_helpers.c +++ b/redist/json_helpers.c @@ -185,9 +185,6 @@ void json_load_file(const char* path) { char* tag = substr(JSON_STRING, tag_t->start, tag_t->end, JSON_STRING_LEN); char* value = substr(JSON_STRING, value_t->start, value_t->end, JSON_STRING_LEN); - printf("%d %d c:%d %d %s \n", tag_t->start, tag_t->end, tag_t->size, tag_t->type, tag); - - if (value_t->type == JSMN_ARRAY) { i += json_load_array(JSON_STRING, tokens+i+2,value_t->size, tag); //look at array children } else if (value_t->type == JSMN_OBJECT) { @@ -244,4 +241,4 @@ int parse_float_array(char* str, jsmntok_t* token, FLT** f, uint8_t count) { return count; -} \ No newline at end of file +} diff --git a/redist/linmath.c b/redist/linmath.c index 76a723d..c302f5b 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -300,6 +300,37 @@ void quattomatrix(FLT * matrix44, const FLT * qin) matrix44[15] = 1; } +void quatfrommatrix33(FLT *q, const FLT *m) { + FLT m00 = m[0], m01 = m[1], m02 = m[2], m10 = m[3], m11 = m[4], m12 = m[5], m20 = m[6], m21 = m[7], m22 = m[8]; + + FLT tr = m00 + m11 + m22; + + if (tr > 0) { + FLT S = sqrt(tr + 1.0) * 2; // S=4*qw + q[0] = 0.25 * S; + q[1] = (m21 - m12) / S; + q[2] = (m02 - m20) / S; + q[3] = (m10 - m01) / S; + } else if ((m00 > m11) & (m00 > m22)) { + FLT S = sqrt(1.0 + m00 - m11 - m22) * 2; // S=4*q[1] + q[0] = (m21 - m12) / S; + q[1] = 0.25 * S; + q[2] = (m01 + m10) / S; + q[3] = (m02 + m20) / S; + } else if (m11 > m22) { + FLT S = sqrt(1.0 + m11 - m00 - m22) * 2; // S=4*q[2] + q[0] = (m02 - m20) / S; + q[1] = (m01 + m10) / S; + q[2] = 0.25 * S; + q[3] = (m12 + m21) / S; + } else { + FLT S = sqrt(1.0 + m22 - m00 - m11) * 2; // S=4*q[3] + q[0] = (m10 - m01) / S; + q[1] = (m02 + m20) / S; + q[2] = (m12 + m21) / S; + q[3] = 0.25 * S; + } +} void quatfrommatrix( FLT * q, const FLT * matrix44 ) { diff --git a/redist/linmath.h b/redist/linmath.h index ff00c94..57b98d3 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -87,6 +87,7 @@ FLT quatinvsqmagnitude( const FLT * q ); void quatnormalize( FLT * qout, const FLT * qin ); //Safe for in to be same as out. void quattomatrix( FLT * matrix44, const FLT * q ); void quatfrommatrix( FLT * q, const FLT * matrix44 ); +void quatfrommatrix33(FLT *q, const FLT *matrix33); void quatgetconjugate( FLT * qout, const FLT * qin ); void quatgetreciprocal( FLT * qout, const FLT * qin ); void quatsub( FLT * qout, const FLT * a, const FLT * b ); diff --git a/src/epnp/opencv_shim.h b/src/epnp/opencv_shim.h index 5806971..5701f1d 100644 --- a/src/epnp/opencv_shim.h +++ b/src/epnp/opencv_shim.h @@ -15,6 +15,8 @@ CvMat *cvCloneMat(const CvMat *mat); void cvReleaseMat(CvMat **mat); void cvSVD(CvMat *aarr, CvMat *warr, CvMat *uarr, CvMat *varr, int flags); void cvMulTransposed(const CvMat *src, CvMat *dst, int order, const CvMat *delta, double scale); +void cvTranspose(const CvMat *M, CvMat *dst); +void print_mat(const CvMat *M); #define CV_SVD 1 #define CV_SVD_MODIFY_A 1 diff --git a/src/poser_epnp.c b/src/poser_epnp.c new file mode 100644 index 0000000..4c3c8b7 --- /dev/null +++ b/src/poser_epnp.c @@ -0,0 +1,130 @@ +#include "PersistentScene.h" + +#ifndef USE_DOUBLE +#define FLT double +#define USE_DOUBLE +#endif + +#include +#include + +#include "epnp/epnp.h" +#include "linmath.h" +#include "math.h" +#include "stdio.h" + +static SurvivePose solve_correspondence(SurviveObject *so, epnp *pnp, bool cameraToWorld) { + SurvivePose rtn = {}; + // std::cerr << "Solving for " << cal_imagePoints.size() << " correspondents" << std::endl; + if (pnp->number_of_correspondences <= 4) { + SurviveContext *ctx = so->ctx; + SV_INFO("Can't solve for only %u points\n", pnp->number_of_correspondences); + return rtn; + } + + double r[3][3]; + + double err = epnp_compute_pose(pnp, r, rtn.Pos); + + CvMat R = cvMat(3, 3, CV_64F, r); + CvMat T = cvMat(3, 1, CV_64F, rtn.Pos); + // Requested output is camera -> world, so invert + if (cameraToWorld) { + FLT tmp[3]; + CvMat Tmp = cvMat(3, 1, CV_64F, tmp); + cvCopyTo(&T, &Tmp); + + // Flip the Rotation matrix + cvTranspose(&R, &R); + // Then 'tvec = -R * tvec' + cvGEMM(&R, &Tmp, -1, 0, 0, &T, 0); + print_mat(&R); + print_mat(&T); + } + + FLT tmp[4]; + quatfrommatrix33(tmp, r[0]); + + // Typical camera applications have Z facing forward; the vive is contrarian and has Z going out of the + // back of the lighthouse. Think of this as a rotation on the Y axis a full 180 degrees -- the quat for that is + // [0 0x 1y 0z] + const FLT rt[4] = {0, 0, 1, 0}; + quatrotateabout(rtn.Rot, tmp, rt); + if (!cameraToWorld) { + // We have to pre-multiply the rt transform here, which means we have to also offset our position by + quatrotateabout(rtn.Rot, rt, tmp); + rtn.Pos[0] = -rtn.Pos[0]; + rtn.Pos[2] = -rtn.Pos[2]; + } + + return rtn; +} + +static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) { + + for (int lh = 0; lh < 2; lh++) { + epnp pnp = {.fu = 1, .fv = 1}; + epnp_set_maximum_number_of_correspondences(&pnp, so->nr_locations); + + for (size_t i = 0; i < so->nr_locations; i++) { + FLT *lengths = pdfs->lengths[i][lh]; + FLT *ang = pdfs->angles[i][lh]; + if (lengths[0] < 0 || lengths[1] < 0) + continue; + + epnp_add_correspondence(&pnp, so->sensor_locations[i * 3 + 0], so->sensor_locations[i * 3 + 1], + so->sensor_locations[i * 3 + 2], tan(ang[0]), tan(ang[1])); + } + + SurviveContext *ctx = so->ctx; + SV_INFO("Solving for %d correspondents", pnp.number_of_correspondences); + if (pnp.number_of_correspondences <= 4) { + SV_INFO("Can't solve for only %d points on lh %d\n", pnp.number_of_correspondences, lh); + continue; + } + + SurvivePose lighthouse = solve_correspondence(so, &pnp, true); + PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &lighthouse); + } + return 0; +} + +int PoserEPNP(SurviveObject *so, PoserData *pd) { + switch (pd->pt) { + case POSERDATA_IMU: { + // Really should use this... + PoserDataIMU *imuData = (PoserDataIMU *)pd; + return 0; + } + case POSERDATA_LIGHT: { + /* + PersistentScene* scene; + PoserDataLight * lightData = pd; + + PersistentScene_add(scene, so, lightData); + + if (so->ctx->bsd[lh].PositionSet) { + auto pose = solve_correspondence(so, cal_objectPoints, cal_imagePoints, false); + + SurvivePose txPose = {}; + quatrotatevector(txPose.Pos, so->ctx->bsd[lh].Pose.Rot, pose.Pos); + for (int i = 0; i < 3; i++) { + txPose.Pos[i] += so->ctx->bsd[lh].Pose.Pos[i]; + } + quatrotateabout(txPose.Rot, so->ctx->bsd[lh].Pose.Rot, pose.Rot); + + // scene->integratePose(txPose, lightData->timecode); + // txPose = scene->currentPose; + PoserData_poser_raw_pose_func(pd, so, lh, &txPose); + } + */ + return -1; + } + case POSERDATA_FULL_SCENE: { + return opencv_solver_fullscene(so, (PoserDataFullScene *)(pd)); + } + } + return -1; +} + +REGISTER_LINKTIME(PoserEPNP); diff --git a/src/survive.c b/src/survive.c index 61bfc86..e09ae13 100755 --- a/src/survive.c +++ b/src/survive.c @@ -297,7 +297,9 @@ int survive_send_magic( SurviveContext * ctx, int magic_code, void * data, int d int i; for( i = 0; i < oldct; i++ ) { - ctx->drivermagics[i]( ctx, ctx->drivers[i], magic_code, data, datalen ); + if (ctx->drivermagics[i]) { + ctx->drivermagics[i](ctx, ctx->drivers[i], magic_code, data, datalen); + } } return 0; } -- cgit v1.2.3