aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--include/libsurvive/survive.h2
-rw-r--r--src/survive_reproject.c15
-rw-r--r--tools/findoptimalconfig/findoptimalconfig.cc54
3 files changed, 35 insertions, 36 deletions
diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index 669821d..7248b1c 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -186,8 +186,8 @@ enum SurviveCalFlag {
};
typedef struct survive_calibration_config {
- enum SurviveCalFlag use_flag;
FLT phase_scale, tilt_scale, curve_scale, gib_scale;
+ enum SurviveCalFlag use_flag;
} survive_calibration_config;
survive_calibration_config survive_calibration_config_ctor();
diff --git a/src/survive_reproject.c b/src/survive_reproject.c
index ee9704f..751abc0 100644
--- a/src/survive_reproject.c
+++ b/src/survive_reproject.c
@@ -19,7 +19,7 @@ void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const surv
FLT x = -t_pt[0] / -t_pt[2];
FLT y = t_pt[1] / -t_pt[2];
-
+ double xy[] = {x, y};
double ang[] = {atan(x), atan(y)};
const FLT *phase = bsd->fcal.phase;
@@ -37,9 +37,9 @@ void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const surv
if (f & SVCal_Phase)
out[axis] -= config->phase_scale * phase[axis];
if (f & SVCal_Tilt)
- out[axis] -= (config->tilt_scale * tilt[axis]) * ang[opp_axis];
+ out[axis] -= tan(config->tilt_scale * tilt[axis]) * xy[opp_axis];
if (f & SVCal_Curve)
- out[axis] -= config->curve_scale * curve[axis] * ang[opp_axis] * ang[opp_axis];
+ out[axis] -= config->curve_scale * curve[axis] * xy[opp_axis] * xy[opp_axis];
if (f & SVCal_Gib)
out[axis] -= config->gib_scale * sin(gibPhase[axis] + ang[axis]) * gibMag[axis];
}
@@ -60,6 +60,7 @@ void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, struct s
const int iterations = 4;
for (int i = 0; i < iterations; i++) {
FLT last_out[2] = {out[0], out[1]};
+ FLT tlast_out[2] = {tan(out[0]), tan(out[1])};
bool last_iteration = i == iterations - 1;
for (int j = 0; j < 2; j++) {
int oj = j == 0 ? 1 : 0;
@@ -67,9 +68,9 @@ void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, struct s
if (!last_iteration || (f & SVCal_Phase))
out[j] += phase_scale * cal->phase[j];
if (!last_iteration || (f & SVCal_Tilt))
- out[j] += (tilt_scale * cal->tilt[j]) * last_out[oj];
+ out[j] += tan(tilt_scale * cal->tilt[j]) * tlast_out[oj];
if (!last_iteration || (f & SVCal_Curve))
- out[j] += (cal->curve[j] * curve_scale) * last_out[oj] * last_out[oj];
+ out[j] += (cal->curve[j] * curve_scale) * tlast_out[oj] * tlast_out[oj];
if (!last_iteration || (f & SVCal_Gib))
out[j] += sin(cal->gibpha[j] + last_out[j]) * cal->gibmag[j] * gib_scale;
}
@@ -88,8 +89,8 @@ void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d,
survive_calibration_config survive_calibration_config_ctor() {
return (survive_calibration_config){.use_flag = SVCal_All,
.phase_scale = 1.,
- .tilt_scale = 1. / 10000.,
- .curve_scale = 1. / 1000.,
+ .tilt_scale = 1. / 10.,
+ .curve_scale = 1. / 10.,
.gib_scale = -1. / 10.};
}
diff --git a/tools/findoptimalconfig/findoptimalconfig.cc b/tools/findoptimalconfig/findoptimalconfig.cc
index 88a43eb..b94590f 100644
--- a/tools/findoptimalconfig/findoptimalconfig.cc
+++ b/tools/findoptimalconfig/findoptimalconfig.cc
@@ -200,35 +200,36 @@ double sba_opt(SurviveContext *ctx, const survive_calibration_config &config, Pl
struct optimal_cal_ctx {
std::vector<double> sensors;
+ std::vector<int> lighthouses;
SurviveContext *ctx;
- survive_calibration_config config;
};
static void metric_function(int j, int i, double *aj, double *xij, void *adata) {
optimal_cal_ctx *ctx = (optimal_cal_ctx *)(adata);
FLT sensorInWorld[3] = {ctx->sensors[i * 3 + 0], ctx->sensors[i * 3 + 1], ctx->sensors[i * 3 + 2]};
+ int lh = ctx->lighthouses[i];
+ BaseStationData bsd = ctx->ctx->bsd[lh];
+ survive_calibration_config cfg = *(survive_calibration_config *)aj;
- BaseStationData bsd = ctx->ctx->bsd[j];
- bsd.fcal = *(BaseStationCal *)aj;
-
- survive_reproject_from_pose_with_bsd(&bsd, &ctx->config, &ctx->ctx->bsd[j].Pose, sensorInWorld, xij);
+ survive_reproject_from_pose_with_bsd(&bsd, &cfg, &ctx->ctx->bsd[lh].Pose, sensorInWorld, xij);
}
-double find_optimal_cal(SurviveContext *ctx, const survive_calibration_config &config, PlaybackData &data) {
+double find_optimal_cal(SurviveContext *ctx, PlaybackData &data) {
optimal_cal_ctx _ctx;
std::vector<char> vmask;
std::vector<double> cov, meas;
_ctx.ctx = ctx;
- _ctx.config = config;
for (auto &in : data.inputs) {
for (size_t sensor = 0; sensor < in.so->sensor_ct; sensor++) {
FLT p[3];
ApplyPoseToPoint(p, &in.position, &data.so->sensor_locations[sensor * 3]);
- _ctx.sensors.emplace_back(p[0]);
- _ctx.sensors.emplace_back(p[1]);
- _ctx.sensors.emplace_back(p[2]);
- for (size_t lh = 0; lh < 1; lh++) {
+ for (size_t lh = 0; lh < 2; lh++) {
+ _ctx.sensors.emplace_back(p[0]);
+ _ctx.sensors.emplace_back(p[1]);
+ _ctx.sensors.emplace_back(p[2]);
+ _ctx.lighthouses.emplace_back(lh);
+
auto scene = &in.activations;
if (SurviveSensorActivations_isPairValid(scene, settings.sensor_time_window, in.timestamp, sensor,
lh)) {
@@ -250,22 +251,25 @@ double find_optimal_cal(SurviveContext *ctx, const survive_calibration_config &c
double opts[SBA_OPTSSZ] = {0};
double info[SBA_INFOSZ] = {0};
- survive_calibration_config opts[0] = SBA_INIT_MU;
+ survive_calibration_config config = {0};
+ config.use_flag = SVCal_All;
+
+ opts[0] = SBA_INIT_MU;
opts[1] = SBA_STOP_THRESH;
opts[2] = SBA_STOP_THRESH;
opts[3] = SBA_STOP_THRESH;
opts[3] = SBA_STOP_THRESH; // max_reproj_error * meas.size();
opts[4] = 0.0;
- int status = sba_mot_levmar(data.inputs.size() * so->sensor_ct, // number of 3d points
- 1, // Number of cameras -- 2 lighthouses
- 0, // Number of cameras to not modify
- &vmask[0], // boolean vis mask
- (double *)cal, // camera parameters
- 2, // sizeof(BaseStationCal) / sizeof(FLT),
- &meas[0], // 2d points for 3d objs
- covx, // covariance of measurement. Null sets to identity
- 2, // 2 points per image
+ int status = sba_mot_levmar(data.inputs.size() * so->sensor_ct * NUM_LIGHTHOUSES, // number of 3d points
+ 1, // Number of cameras -- 2 lighthouses
+ 0, // Number of cameras to not modify
+ &vmask[0], // boolean vis mask
+ (double *)&config, // camera parameters
+ 4, // sizeof(BaseStationCal) / sizeof(FLT),
+ &meas[0], // 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
@@ -295,10 +299,6 @@ double find_optimal_cal(SurviveContext *ctx, const survive_calibration_config &c
assert(!isinf(info[1]));
std::cerr << "Used " << meas_size << " measurements" << std::endl;
- double *_cal = (double *)cal;
- for (int i = 0; i < sizeof(BaseStationCal) / sizeof(FLT); i++)
- std::cerr << _cal[2 * i] << ", " << _cal[2 * i + 1] << " = " << (info[1] / meas_size * 2) << std::endl;
-
return info[1] / meas_size * 2;
}
double find_avg_reproj_error(SurviveContext *ctx, const survive_calibration_config &config, PlaybackDataInput &data) {
@@ -371,9 +371,7 @@ int main(int argc, char **argv) {
while (survive_poll(ctx) == 0) {
}
- survive_calibration_config config = survive_calibration_config_ctor();
-
- find_optimal_cal(ctx, config, data);
+ find_optimal_cal(ctx, data);
survive_close(ctx);
}