aboutsummaryrefslogtreecommitdiff
path: root/src/poser_epnp.c
diff options
context:
space:
mode:
Diffstat (limited to 'src/poser_epnp.c')
-rw-r--r--src/poser_epnp.c82
1 files changed, 54 insertions, 28 deletions
diff --git a/src/poser_epnp.c b/src/poser_epnp.c
index 401ea2a..c05450a 100644
--- a/src/poser_epnp.c
+++ b/src/poser_epnp.c
@@ -6,6 +6,7 @@
#include <poser.h>
#include <survive.h>
+#include <survive_reproject.h>
#include "epnp/epnp.h"
#include "linmath.h"
@@ -13,7 +14,7 @@
#include "stdio.h"
static SurvivePose solve_correspondence(SurviveObject *so, epnp *pnp, bool cameraToWorld) {
- SurvivePose rtn = {};
+ SurvivePose rtn = {0};
// std::cerr << "Solving for " << cal_imagePoints.size() << " correspondents" << std::endl;
if (pnp->number_of_correspondences <= 3) {
SurviveContext *ctx = so->ctx;
@@ -27,6 +28,12 @@ static SurvivePose solve_correspondence(SurviveObject *so, epnp *pnp, bool camer
CvMat R = cvMat(3, 3, CV_64F, r);
CvMat T = cvMat(3, 1, CV_64F, rtn.Pos);
+
+ // Super degenerate inputs will project us basically right in the camera. Detect and reject
+ if (magnitude3d(rtn.Pos) < 0.25) {
+ return rtn;
+ }
+
// Requested output is camera -> world, so invert
if (cameraToWorld) {
FLT tmp[3];
@@ -65,7 +72,9 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs)
for (size_t i = 0; i < so->sensor_ct; i++) {
FLT *lengths = pdfs->lengths[i][lh];
- FLT *ang = pdfs->angles[i][lh];
+ FLT *_ang = pdfs->angles[i][lh];
+ FLT ang[2];
+ survive_apply_bsd_calibration(so->ctx, lh, _ang, ang);
if (lengths[0] < 0 || lengths[1] < 0)
continue;
@@ -81,7 +90,10 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs)
}
SurvivePose lighthouse2object = solve_correspondence(so, &pnp, true);
- PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &additionalTx, &lighthouse2object, 0);
+
+ if (quatmagnitude(lighthouse2object.Rot) != 0.0) {
+ PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &additionalTx, &lighthouse2object, 0);
+ }
epnp_dtor(&pnp);
}
@@ -89,13 +101,15 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs)
return 0;
}
-static void add_correspondences(SurviveObject *so, epnp *pnp, SurviveSensorActivations *scene,
- const PoserDataLight *lightData) {
- int lh = lightData->lh;
+static void add_correspondences(SurviveObject *so, epnp *pnp, SurviveSensorActivations *scene, uint32_t timecode,
+ int lh) {
for (size_t sensor_idx = 0; sensor_idx < so->sensor_ct; sensor_idx++) {
- if (SurviveSensorActivations_isPairValid(scene, SurviveSensorActivations_default_tolerance, lightData->timecode,
+ if (SurviveSensorActivations_isPairValid(scene, SurviveSensorActivations_default_tolerance, timecode,
sensor_idx, lh)) {
- double *angles = scene->angles[sensor_idx][lh];
+ FLT *_angles = scene->angles[sensor_idx][lh];
+ FLT angles[2];
+ survive_apply_bsd_calibration(so->ctx, lh, _angles, angles);
+
epnp_add_correspondence(pnp, so->sensor_locations[sensor_idx * 3 + 0],
so->sensor_locations[sensor_idx * 3 + 1], so->sensor_locations[sensor_idx * 3 + 2],
tan(angles[0]), tan(angles[1]));
@@ -116,43 +130,55 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) {
PoserDataLight *lightData = (PoserDataLight *)pd;
SurvivePose posers[2];
- bool hasData[2] = {0, 0};
- for (int lh = 0; lh < 1; lh++) {
+ 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);
+ 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 > 4) {
+ if (pnp.number_of_correspondences > required_meas) {
- SurvivePose pose = solve_correspondence(so, &pnp, false);
+ SurvivePose objInLh = solve_correspondence(so, &pnp, false);
+ if (quatmagnitude(objInLh.Rot) != 0) {
+ SurvivePose *lh2world = &so->ctx->bsd[lh].Pose;
- 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];
+ SurvivePose txPose = {.Rot = {1}};
+ ApplyPoseToPose(&txPose, lh2world, &objInLh);
+ posers[lh] = txPose;
+ meas[lh] = pnp.number_of_correspondences;
}
-
- quatrotateabout(txPose.Rot, so->ctx->bsd[lh].Pose.Rot, pose.Rot);
-
- posers[lh] = txPose;
- hasData[lh] = 1;
}
epnp_dtor(&pnp);
}
}
- if (hasData[0] && hasData[1]) {
+ if (meas[0] > 0 && meas[1] > 0) {
SurvivePose interpolate = {0};
- for (size_t i = 0; i < 3; i++) {
- interpolate.Pos[i] = (posers[0].Pos[i] + posers[1].Pos[i]) / 2.;
+ 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);
}
- quatslerp(interpolate.Rot, posers[0].Rot, posers[1].Rot, .5);
- PoserData_poser_raw_pose_func(pd, so, lightData->lh, &interpolate);
} else {
- if (hasData[lightData->lh])
+ if (meas[lightData->lh])
PoserData_poser_raw_pose_func(pd, so, lightData->lh, &posers[lightData->lh]);
}
return 0;