aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--Makefile2
-rw-r--r--data_recorder.c20
-rw-r--r--redist/json_helpers.c5
-rw-r--r--redist/linmath.c31
-rw-r--r--redist/linmath.h1
-rw-r--r--src/epnp/opencv_shim.h2
-rw-r--r--src/poser_epnp.c130
-rwxr-xr-xsrc/survive.c4
8 files changed, 186 insertions, 9 deletions
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 <poser.h>
+#include <survive.h>
+
+#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;
}