aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--include/libsurvive/poser.h7
-rw-r--r--include/libsurvive/survive.h3
-rw-r--r--include/libsurvive/survive_types.h3
-rw-r--r--src/poser.c59
-rw-r--r--src/poser_charlesslow.c3
-rw-r--r--src/poser_epnp.c23
-rw-r--r--src/poser_sba.c29
-rw-r--r--src/poser_turveytori.c24
-rw-r--r--src/survive_process.c7
-rw-r--r--test.c4
10 files changed, 112 insertions, 50 deletions
diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h
index 9667f1a..6b5b09d 100644
--- a/include/libsurvive/poser.h
+++ b/include/libsurvive/poser.h
@@ -18,7 +18,8 @@ typedef enum PoserType_t
} PoserType;
typedef void (*poser_raw_pose_func)(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose, void *user);
-typedef void (*poser_lighthouse_pose_func)(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose, void *user);
+typedef void (*poser_lighthouse_pose_func)(SurviveObject *so, uint8_t lighthouse, SurvivePose *lighthouse_pose,
+ SurvivePose *object_pose, void *user);
typedef struct
{
@@ -29,7 +30,9 @@ typedef struct
} PoserData;
void PoserData_poser_raw_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose);
-void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose);
+void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse,
+ /* OUTPARAM */ SurvivePose *objUp2world, SurvivePose *lighthouse_poses,
+ SurvivePose *object_pose);
typedef struct PoserDataIMU {
PoserData hdr;
diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index a3639dc..747d076 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -259,7 +259,8 @@ void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro,
void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh );
void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val);
void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose);
-void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose);
+void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lh_pose,
+ SurvivePose *obj_pose);
int survive_default_htc_config_process(SurviveObject *so, char *ct0conf, int len);
diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h
index 5f5dd1f..7253df0 100644
--- a/include/libsurvive/survive_types.h
+++ b/include/libsurvive/survive_types.h
@@ -53,7 +53,8 @@ typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro,
typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh);
typedef void(*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val);
typedef void (*raw_pose_func)(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose);
-typedef void (*lighthouse_pose_func)(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose);
+typedef void (*lighthouse_pose_func)(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose,
+ SurvivePose *object_pose);
typedef int(*haptic_func)(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh , uint16_t pulseLow, uint16_t repeatCount);
diff --git a/src/poser.c b/src/poser.c
index ec7865e..e52edd2 100644
--- a/src/poser.c
+++ b/src/poser.c
@@ -1,7 +1,7 @@
+#include "math.h"
+#include <linmath.h>
#include <stdint.h>
-
-#include "poser.h"
-#include "survive_internal.h"
+#include <survive.h>
void PoserData_poser_raw_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) {
if (poser_data->rawposeproc) {
@@ -11,10 +11,57 @@ void PoserData_poser_raw_pose_func(PoserData *poser_data, SurviveObject *so, uin
}
}
-void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) {
+void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse,
+ SurvivePose *objUp2world, SurvivePose *lighthouse_pose, SurvivePose *object_pose) {
if (poser_data->lighthouseposeproc) {
- poser_data->lighthouseposeproc(so, lighthouse, pose, poser_data->userdata);
+ poser_data->lighthouseposeproc(so, lighthouse, lighthouse_pose, object_pose, poser_data->userdata);
} else {
- so->ctx->lighthouseposeproc(so->ctx, lighthouse, pose);
+ const FLT up[3] = {0, 0, 1};
+
+ // Assume that the space solved for is valid but completely arbitrary. We are going to do a few things:
+ // a) Using the gyro data, normalize it so that gravity is pushing straight down along Z
+ // c) Assume the object is at origin
+ // b) Place the first lighthouse on the X axis by rotating around Z
+ //
+ // This calibration setup has the benefit that as long as the user is calibrating on the same flat surface,
+ // calibration results will be roughly identical between all posers no matter the orientation the object is
+ // lying
+ // in.
+ //
+ // We might want to go a step further and affix the first lighthouse in a given pose that preserves up so that
+ // it doesn't matter where on that surface the object is.
+
+ SurvivePose object2arb = *object_pose;
+ SurvivePose lighthouse2arb = *lighthouse_pose;
+
+ // Start by just moving from whatever arbitrary space into object space.
+ SurvivePose arb2object;
+ InvertPose(arb2object.Pos, object2arb.Pos);
+
+ SurvivePose lighthouse2obj;
+ ApplyPoseToPose(lighthouse2obj.Pos, arb2object.Pos, lighthouse2arb.Pos);
+
+ // Now find the space with the same origin, but rotated so that gravity is up
+ SurvivePose lighthouse2objUp = {}, object2objUp = {};
+ quatfrom2vectors(object2objUp.Rot, so->activations.accel, up);
+
+ // Calculate the pose of the lighthouse in this space
+ ApplyPoseToPose(lighthouse2objUp.Pos, object2objUp.Pos, lighthouse2obj.Pos);
+
+ // Purposefully only set this once. It should only depend on the first (calculated) lighthouse
+ if (quatmagnitude(objUp2world->Rot) == 0) {
+ // Find what angle we need to rotate about Z by to get to 90 degrees.
+ FLT ang = atan2(lighthouse2objUp.Pos[1], lighthouse2objUp.Pos[0]);
+ FLT euler[3] = {0, 0, M_PI / 2. - ang};
+
+ quatfromeuler(objUp2world->Rot, euler);
+ }
+
+ // Find find the poses that map to the above
+ SurvivePose obj2world, lighthouse2world;
+ ApplyPoseToPose(obj2world.Pos, objUp2world->Pos, object2objUp.Pos);
+ ApplyPoseToPose(lighthouse2world.Pos, objUp2world->Pos, lighthouse2objUp.Pos);
+
+ so->ctx->lighthouseposeproc(so->ctx, lighthouse, &lighthouse2world, &obj2world);
}
}
diff --git a/src/poser_charlesslow.c b/src/poser_charlesslow.c
index 3b781f4..c7d9033 100644
--- a/src/poser_charlesslow.c
+++ b/src/poser_charlesslow.c
@@ -53,6 +53,7 @@ int PoserCharlesSlow( SurviveObject * so, PoserData * pd )
printf( "%f %f %f\n", hmd_points[p*3+0], hmd_points[p*3+1], hmd_points[p*3+2] );
}
+ SurvivePose additionalTx = {};
int lh, cycle;
FLT dz, dy, dx;
@@ -174,7 +175,7 @@ int PoserCharlesSlow( SurviveObject * so, PoserData * pd )
quatrotateabout(tmp, lighthousePose.Rot, rt);
memcpy(lighthousePose.Rot, tmp, sizeof(FLT) * 4);
- PoserData_lighthouse_pose_func(pd, so, lh, &lighthousePose);
+ PoserData_lighthouse_pose_func(pd, so, lh, &additionalTx, &lighthousePose, 0);
#define ALT_COORDS
#ifdef ALT_COORDS
diff --git a/src/poser_epnp.c b/src/poser_epnp.c
index 193e30c..cf4294f 100644
--- a/src/poser_epnp.c
+++ b/src/poser_epnp.c
@@ -90,13 +90,13 @@ static int survive_standardize_calibration(SurviveObject* so,
}
*/
static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) {
- SurvivePose object2world = {};
+ SurvivePose object2world = {//.Rot = { 0.7325378, 0.4619398, 0.1913417, 0.4619398}
+ .Rot = {1.}};
const FLT up[3] = {0, 0, 1};
- quatfrom2vectors(object2world.Rot, so->activations.accel, up);
-
- FLT tx[4][4];
- quattomatrix(tx, object2world.Rot);
+ SurvivePose actual;
+ // quatfrom2vectors(object2world.Rot, so->activations.accel, up);
+ // quatfrom2vectors(object2world.Rot, up, so->activations.accel);
SurvivePose additionalTx = {0};
@@ -122,27 +122,26 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs)
}
SurvivePose lighthouse2object = solve_correspondence(so, &pnp, true);
-
+ FLT euler[3];
+ quattoeuler(euler, lighthouse2object.Rot);
SurvivePose lighthouse2world = {};
// Lighthouse is now a tx from camera -> object
ApplyPoseToPose(lighthouse2world.Pos, object2world.Pos, lighthouse2object.Pos);
- if (quatmagnitude(additionalTx.Rot) == 0) {
+ if (false && quatmagnitude(additionalTx.Rot) == 0) {
SurvivePose desiredPose = lighthouse2world;
desiredPose.Pos[0] = 0.;
quatfrom2vectors(additionalTx.Rot, lighthouse2world.Pos, desiredPose.Pos);
}
- SurvivePose finalTx = {};
- ApplyPoseToPose(finalTx.Pos, additionalTx.Pos, lighthouse2world.Pos);
+ SurvivePose finalTx = lighthouse2world;
+ // ApplyPoseToPose(finalTx.Pos, additionalTx.Pos, lighthouse2world.Pos);
- PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &finalTx);
+ PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &additionalTx, &finalTx, &object2world);
epnp_dtor(&pnp);
}
- so->OutPose = object2world;
-
return 0;
}
diff --git a/src/poser_sba.c b/src/poser_sba.c
index 0dcc38c..fa71dd2 100644
--- a/src/poser_sba.c
+++ b/src/poser_sba.c
@@ -20,13 +20,15 @@ typedef struct {
survive_calibration_config calibration_config;
PoserData *pdfs;
SurviveObject *so;
+ SurvivePose obj_pose;
+ SurvivePose camera_params[2];
} sba_context;
void metric_function(int j, int i, double *aj, double *xij, void *adata) {
sba_context *ctx = (sba_context *)(adata);
SurviveObject *so = ctx->so;
- SurvivePose obj2world = so->OutPose;
+ SurvivePose obj2world = ctx->obj_pose;
FLT sensorInWorld[3] = {};
ApplyPoseToPoint(sensorInWorld, obj2world.Pos, &so->sensor_locations[i * 3]);
survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, j, (SurvivePose *)aj, sensorInWorld,
@@ -74,9 +76,10 @@ size_t construct_input_from_scene(const SurviveObject *so, PoserDataLight *pdl,
return rtn;
}
-void sba_set_cameras(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose, void *user) {
- SurvivePose *poses = (SurvivePose *)(user);
- poses[lighthouse] = *pose;
+void sba_set_cameras(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose, SurvivePose *obj_pose, void *user) {
+ sba_context *ctx = (sba_context *)user;
+ ctx->camera_params[lighthouse] = *pose;
+ ctx->obj_pose = *obj_pose;
}
typedef struct {
@@ -225,9 +228,10 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd
double *meas = malloc(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES);
size_t meas_size = construct_input(so, pdfs, vmask, meas);
- SurvivePose camera_params[2] = {so->ctx->bsd[0].Pose, so->ctx->bsd[1].Pose};
+ sba_context sbactx = {options, &pdfs->hdr, so, .camera_params = {so->ctx->bsd[0].Pose, so->ctx->bsd[1].Pose},
+ .obj_pose = so->OutPose};
- if (true || so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0) {
+ {
const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP");
PoserCB driver = (PoserCB)GetDriver(subposer);
SurviveContext *ctx = so->ctx;
@@ -237,7 +241,7 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd
memset(&pdfs->hdr, 0, sizeof(pdfs->hdr)); // Clear callback functions
pdfs->hdr.pt = hdr.pt;
pdfs->hdr.lighthouseposeproc = sba_set_cameras;
- pdfs->hdr.userdata = camera_params;
+ pdfs->hdr.userdata = &sbactx;
driver(so, &pdfs->hdr);
pdfs->hdr = hdr;
} else {
@@ -254,8 +258,6 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd
double opts[SBA_OPTSSZ] = {};
double info[SBA_INFOSZ] = {};
- sba_context ctx = {options, &pdfs->hdr, so};
-
opts[0] = SBA_INIT_MU;
opts[1] = SBA_STOP_THRESH;
opts[2] = SBA_STOP_THRESH;
@@ -267,22 +269,23 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd
NUM_LIGHTHOUSES, // Number of cameras -- 2 lighthouses
0, // Number of cameras to not modify
vmask, // boolean vis mask
- (double *)&camera_params[0], // camera parameters
+ (double *)&sbactx.camera_params[0], // camera parameters
sizeof(SurvivePose) / sizeof(double), // The number of floats that are in a camera param
meas, // 2d points for 3d objs
covx, // covariance of measurement. Null sets to identity
2, // 2 points per image
metric_function,
0, // jacobia of metric_func
- &ctx, // user data
+ &sbactx, // user data
max_iterations, // Max iterations
0, // verbosity
opts, // options
info); // info
if (status >= 0) {
- PoserData_lighthouse_pose_func(&pdfs->hdr, so, 0, &camera_params[0]);
- PoserData_lighthouse_pose_func(&pdfs->hdr, so, 1, &camera_params[1]);
+ SurvivePose additionalTx = {};
+ PoserData_lighthouse_pose_func(&pdfs->hdr, so, 0, &additionalTx, &sbactx.camera_params[0], &sbactx.obj_pose);
+ PoserData_lighthouse_pose_func(&pdfs->hdr, so, 1, &additionalTx, &sbactx.camera_params[1], &sbactx.obj_pose);
}
// Docs say info[0] should be divided by meas; I don't buy it really...
// std::cerr << info[0] / meas.size() * 2 << " original reproj error" << std::endl;
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index db8d638..2d3f802 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -1235,7 +1235,7 @@ void SolveForRotationQuat(FLT rotOut[4], TrackedObject *obj, Point lh)
}
static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, PoserData *pd,
- char doLogOutput, const int lh, const int setLhCalibration) {
+ char doLogOutput, SurvivePose *additionalTx, const int lh, const int setLhCalibration) {
ToriData *toriData = so->PoserData;
//printf("Solving for Lighthouse\n");
@@ -1443,7 +1443,11 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
lighthousePose.Pos[1] = refinedEstimateGd.y;
lighthousePose.Pos[2] = refinedEstimateGd.z;
- PoserData_lighthouse_pose_func(pd, so, lh, &lighthousePose);
+ SurvivePose assumedObj = {};
+ FLT negZ[3] = {0, 0, 1};
+ quatfrom2vectors(assumedObj.Rot, toriData->down, negZ);
+
+ PoserData_lighthouse_pose_func(pd, so, lh, additionalTx, &lighthousePose, &assumedObj);
}
@@ -1528,7 +1532,7 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
return refinedEstimateGd;
}
-static void QuickPose(SurviveObject *so, PoserData *pd, int lh) {
+static void QuickPose(SurviveObject *so, PoserData *pd, SurvivePose *additionalTx, int lh) {
ToriData * td = so->PoserData;
@@ -1622,7 +1626,7 @@ static void QuickPose(SurviveObject *so, PoserData *pd, int lh) {
// SolveForLighthouse(pos, quat, to, so, 0, lh, 0);
//}
- SolveForLighthouse(&pose.Pos[0], &pose.Rot[0], to, so, pd, 0, lh, 0);
+ 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)
@@ -1693,6 +1697,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
}
int axis = l->acode & 0x1;
//printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode );
+
+ SurvivePose additionalTx;
if ((td->lastAxis[l->lh] != (l->acode & 0x1)) )
{
@@ -1705,7 +1711,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
// let's just do this occasionally for now...
if (counter % 4 == 0)
- QuickPose(so, poserData, 0);
+ QuickPose(so, poserData, &additionalTx, 0);
}
if (1 == l->lh && axis) // only once per full cycle...
{
@@ -1715,7 +1721,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
// let's just do this occasionally for now...
if (counter % 4 == 0)
- QuickPose(so, poserData, 1);
+ QuickPose(so, poserData, &additionalTx, 1);
}
// axis changed, time to increment the circular buffer index.
td->angleIndex[l->lh][axis]++;
@@ -1755,7 +1761,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
FLT axis[3];
angleaxisfrom2vect(&angle, axis, td->down, negZ);
//angleaxisfrom2vect(&angle, &axis, negZ, td->down);
-
+ SurvivePose additionalTx;
{
int sensorCount = 0;
@@ -1790,7 +1796,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
FLT pos[3], quat[4];
- SolveForLighthouse(pos, quat, to, so, poserData, 0, 0, 1);
+ SolveForLighthouse(pos, quat, to, so, poserData, 0, &additionalTx, 0, 1);
}
{
int sensorCount = 0;
@@ -1826,7 +1832,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
FLT pos[3], quat[4];
- SolveForLighthouse(pos, quat, to, so, poserData, 0, 1, 1);
+ SolveForLighthouse(pos, quat, to, so, poserData, 0, &additionalTx, 1, 1);
}
diff --git a/src/survive_process.c b/src/survive_process.c
index f315884..a49b632 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -115,9 +115,10 @@ void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, Sur
so->FromLHPose[lighthouse] = *pose;
}
-void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose) {
- if (pose) {
- ctx->bsd[lighthouse].Pose = *pose;
+void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose,
+ SurvivePose *object_pose) {
+ if (lighthouse_pose) {
+ ctx->bsd[lighthouse].Pose = *lighthouse_pose;
ctx->bsd[lighthouse].PositionSet = 1;
} else {
ctx->bsd[lighthouse].PositionSet = 0;
diff --git a/test.c b/test.c
index ea60a14..8e620bc 100644
--- a/test.c
+++ b/test.c
@@ -84,8 +84,8 @@ void testprog_button_process(SurviveObject * so, uint8_t eventType, uint8_t butt
}
}
-void testprog_lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose) {
- survive_default_lighthouse_pose_process(ctx, lighthouse, pose);
+void testprog_lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose, SurvivePose *obj_pose) {
+ survive_default_lighthouse_pose_process(ctx, lighthouse, pose, obj_pose);
printf("Lighthouse: [%1.1x][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, pose->Pos[0],
pose->Pos[1], pose->Pos[2], pose->Rot[0], pose->Rot[1], pose->Rot[2], pose->Rot[3]);