aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--include/libsurvive/poser.h3
-rw-r--r--include/libsurvive/survive.h7
-rw-r--r--src/poser_epnp.c65
-rw-r--r--src/poser_sba.c11
-rw-r--r--src/survive_process.c28
-rw-r--r--src/survive_sensor_activations.c11
-rw-r--r--tools/viz/survive_viewer.js18
7 files changed, 116 insertions, 27 deletions
diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h
index 6c74c52..9667f1a 100644
--- a/include/libsurvive/poser.h
+++ b/include/libsurvive/poser.h
@@ -31,8 +31,7 @@ typedef struct
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);
-typedef struct
-{
+typedef struct PoserDataIMU {
PoserData hdr;
uint8_t datamask; //0 = accel present, 1 = gyro present, 2 = mag present.
FLT accel[3];
diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index 180d83c..a3639dc 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -16,14 +16,21 @@ extern "C" {
typedef struct {
FLT angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; // 2 Axes (Angles in LH space)
uint32_t timecode[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; // Timecode per axis in ticks
+
+ FLT accel[3];
+ FLT gyro[3];
+ FLT mag[3];
} SurviveSensorActivations;
struct PoserDataLight;
+struct PoserDataIMU;
/**
* Adds a lightData packet to the table.
*/
void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDataLight *lightData);
+void SurviveSensorActivations_add_imu(SurviveSensorActivations *self, struct PoserDataIMU *imuData);
+
/**
* Returns true iff both angles for the given sensor and lighthouse were seen at most `tolerance` ticks before the given
* `timecode_now`.
diff --git a/src/poser_epnp.c b/src/poser_epnp.c
index dfe86ff..193e30c 100644
--- a/src/poser_epnp.c
+++ b/src/poser_epnp.c
@@ -56,8 +56,49 @@ static SurvivePose solve_correspondence(SurviveObject *so, epnp *pnp, bool camer
return rtn;
}
+/*
+static int survive_standardize_calibration(SurviveObject* so,
+ FLT upvec[3],
+ SurvivePose* _object2world,
+ SurvivePose* _lighthouses2world0,
+ SurvivePose* _lighthouses2world1) {
+ SurvivePose object2world = {};
+ SurvivePose lighthouses2world0 = *_lighthouses2world0;
+ SurvivePose lighthouses2world1 = *_lighthouses2world1;
+ const FLT up[3] = {0, 0, 1};
+ quatfrom2vectors(object2world.Rot, so->activations.accel, _object2world->Pos);
+
+ FLT tx[4][4];
+ quattomatrix(tx, object2world.Rot);
+
+ SurvivePose additionalTx = {0};
+
+ SurvivePose lighthouse2world = {};
+ // Lighthouse is now a tx from camera -> object
+ ApplyPoseToPose(lighthouse2world.Pos, object2world.Pos, lighthouse2object.Pos);
+
+ if(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);
+
+}
+*/
static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) {
+ SurvivePose object2world = {};
+ const FLT up[3] = {0, 0, 1};
+
+ quatfrom2vectors(object2world.Rot, so->activations.accel, up);
+
+ FLT tx[4][4];
+ quattomatrix(tx, object2world.Rot);
+
+ SurvivePose additionalTx = {0};
for (int lh = 0; lh < 2; lh++) {
epnp pnp = {.fu = 1, .fv = 1};
@@ -80,11 +121,28 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs)
continue;
}
- SurvivePose lighthouse = solve_correspondence(so, &pnp, true);
- PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &lighthouse);
+ SurvivePose lighthouse2object = solve_correspondence(so, &pnp, true);
+
+ SurvivePose lighthouse2world = {};
+ // Lighthouse is now a tx from camera -> object
+ ApplyPoseToPose(lighthouse2world.Pos, object2world.Pos, lighthouse2object.Pos);
+
+ if (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);
+
+ PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &finalTx);
epnp_dtor(&pnp);
}
+
+ so->OutPose = object2world;
+
return 0;
}
@@ -103,6 +161,8 @@ static void add_correspondences(SurviveObject *so, epnp *pnp, SurviveSensorActiv
}
int PoserEPNP(SurviveObject *so, PoserData *pd) {
+
+ SurviveSensorActivations *scene = &so->activations;
switch (pd->pt) {
case POSERDATA_IMU: {
// Really should use this...
@@ -110,7 +170,6 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) {
return 0;
}
case POSERDATA_LIGHT: {
- SurviveSensorActivations *scene = &so->activations;
PoserDataLight *lightData = (PoserDataLight *)pd;
SurvivePose posers[2];
diff --git a/src/poser_sba.c b/src/poser_sba.c
index 44af9c1..0dcc38c 100644
--- a/src/poser_sba.c
+++ b/src/poser_sba.c
@@ -26,8 +26,11 @@ void metric_function(int j, int i, double *aj, double *xij, void *adata) {
sba_context *ctx = (sba_context *)(adata);
SurviveObject *so = ctx->so;
- survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, j, (SurvivePose *)aj,
- &so->sensor_locations[i * 3], xij);
+ SurvivePose obj2world = so->OutPose;
+ 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,
+ xij);
}
size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, char *vmask, double *meas) {
@@ -109,6 +112,7 @@ void str_metric_function(int j, int i, double *bi, double *xij, void *adata) {
survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, lh, camera, xyz, xij);
}
+// Optimizes for object assuming given LH
static double run_sba_find_3d_structure(survive_calibration_config options, PoserDataLight *pdl, SurviveObject *so,
SurviveSensorActivations *scene, int max_iterations /* = 50*/,
double max_reproj_error /* = 0.005*/) {
@@ -132,7 +136,7 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose
failure_count = 0;
SurvivePose soLocation = so->OutPose;
- bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]);
+ bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]) != 0;
{
const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP");
@@ -212,6 +216,7 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose
return info[1] / meas_size * 2;
}
+// Optimizes for LH position assuming object is posed at 0
static double run_sba(survive_calibration_config options, PoserDataFullScene *pdfs, SurviveObject *so,
int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) {
double *covx = 0;
diff --git a/src/survive_process.c b/src/survive_process.c
index b697f4a..f315884 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -132,19 +132,21 @@ int survive_default_htc_config_process(SurviveObject *so, char *ct0conf, int len
}
void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id )
{
- if( so->PoserFn )
- {
- PoserDataIMU imu = {
- .hdr =
- {
- .pt = POSERDATA_IMU,
- },
- .datamask = mask,
- .accel = {accelgyromag[0], accelgyromag[1], accelgyromag[2]},
- .gyro = {accelgyromag[3], accelgyromag[4], accelgyromag[5]},
- .mag = {accelgyromag[6], accelgyromag[7], accelgyromag[8]},
- .timecode = timecode,
- };
+ PoserDataIMU imu = {
+ .hdr =
+ {
+ .pt = POSERDATA_IMU,
+ },
+ .datamask = mask,
+ .accel = {accelgyromag[0], accelgyromag[1], accelgyromag[2]},
+ .gyro = {accelgyromag[3], accelgyromag[4], accelgyromag[5]},
+ .mag = {accelgyromag[6], accelgyromag[7], accelgyromag[8]},
+ .timecode = timecode,
+ };
+
+ SurviveSensorActivations_add_imu(&so->activations, &imu);
+
+ if (so->PoserFn) {
so->PoserFn( so, (PoserData *)&imu );
}
}
diff --git a/src/survive_sensor_activations.c b/src/survive_sensor_activations.c
index 42de833..dce353c 100644
--- a/src/survive_sensor_activations.c
+++ b/src/survive_sensor_activations.c
@@ -6,6 +6,17 @@ bool SurviveSensorActivations_isPairValid(const SurviveSensorActivations *self,
return !(timecode_now - data_timecode[0] > tolerance || timecode_now - data_timecode[1] > tolerance);
}
+void SurviveSensorActivations_add_imu(SurviveSensorActivations *self, struct PoserDataIMU *imuData) {
+ for (int i = 0; i < 3; i++) {
+ self->accel[i] = .98 * self->accel[i] + .02 * imuData->accel[i];
+ }
+ for (int i = 0; i < 3; i++) {
+ self->gyro[i] = .98 * self->gyro[i] + .02 * imuData->gyro[i];
+ }
+ for (int i = 0; i < 3; i++) {
+ self->mag[i] = .98 * self->mag[i] + .02 * imuData->mag[i];
+ }
+}
void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDataLight *lightData) {
int axis = (lightData->acode & 1);
uint32_t *data_timecode = &self->timecode[lightData->sensor_id][lightData->lh][axis];
diff --git a/tools/viz/survive_viewer.js b/tools/viz/survive_viewer.js
index 3aa2cf7..ab6a200 100644
--- a/tools/viz/survive_viewer.js
+++ b/tools/viz/survive_viewer.js
@@ -280,18 +280,21 @@ $(function() {
create_object(obj);
} else if (obj.type === "imu") {
if (objs[obj.tracker]) {
- if (!downAxes[obj.tracker]) {
+ if (!downAxes[obj.tracker] && objs[obj.tracker]) {
downAxes[obj.tracker] = new THREE.Geometry();
- downAxes[obj.tracker].vertices.push(
- new THREE.Vector3(0, 0, 0),
- new THREE.Vector3(obj.accelgyro[0], obj.accelgyro[1], obj.accelgyro[2]));
+ downAxes[obj.tracker].vertices.push(new THREE.Vector3(0, 0, 0), new THREE.Vector3(0, 0, 0));
var line =
new THREE.Line(downAxes[obj.tracker], new THREE.LineBasicMaterial({color : 0xffffff}));
- objs[obj.tracker].add(line);
- } else {
+ scene.add(line);
+ }
+
+ if (objs[obj.tracker].position) {
var q = obj.accelgyro;
+
+ downAxes[obj.tracker].vertices[0] = objs[obj.tracker].position;
downAxes[obj.tracker].vertices[1].fromArray(q);
+ downAxes[obj.tracker].vertices[1].add(objs[obj.tracker].position);
downAxes[obj.tracker].verticesNeedUpdate = true;
}
}
@@ -382,6 +385,9 @@ init() {
var skyBoxMaterial = new THREE.MeshBasicMaterial({color : 0x888888, side : THREE.BackSide});
var skyBox = new THREE.Mesh(skyBoxGeometry, skyBoxMaterial);
scene.add(skyBox);
+
+ var axes = new THREE.AxesHelper(5);
+ scene.add(axes);
}
function animate() {