From f6babcbe142b335aba09d9c9798ac5bd31f25144 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 30 Mar 2018 09:43:59 -0600 Subject: Added tool to help find the optimal calibration permutation --- tools/findoptimalconfig/findoptimalconfig.cc | 332 +++++++++++++++++++++++++++ 1 file changed, 332 insertions(+) create mode 100644 tools/findoptimalconfig/findoptimalconfig.cc (limited to 'tools/findoptimalconfig/findoptimalconfig.cc') diff --git a/tools/findoptimalconfig/findoptimalconfig.cc b/tools/findoptimalconfig/findoptimalconfig.cc new file mode 100644 index 0000000..543958b --- /dev/null +++ b/tools/findoptimalconfig/findoptimalconfig.cc @@ -0,0 +1,332 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +std::ostream &operator<<(std::ostream &o, const survive_calibration_options_config &self) { + o << "\t"; + if (!self.enable[0] && !self.enable[1]) { + o << "disabled"; + return o; + } + + o << "swap: " << self.swap << std::endl; + for (int i = 0; i < 2; i++) { + if (self.enable[i]) { + o << "\tinvert[" << i << "]: " << self.invert[i]; + } else { + o << "\t" << i << ": disabled"; + } + } + return o; +} + +std::ostream &operator<<(std::ostream &o, const survive_calibration_config &self) { + o << "Index: " << survive_calibration_config_index(&self) << std::endl; + o << "Phase: " << std::endl << self.phase << std::endl; + o << "Tilt: " << std::endl << self.tilt << std::endl; + o << "Curve: " << std::endl << self.curve << std::endl; + o << "gibPhase: " << std::endl << self.gibPhase << std::endl; + o << "gibMag: " << std::endl << self.gibMag << std::endl; + o << "gibUseSin: " << self.gibUseSin << std::endl; + return o; +} + +struct SBAData { + int last_acode = -1; + int last_lh = -1; + + int failures_to_reset = 1; + int failures_to_reset_cntr = 0; + int successes_to_reset = 1; + int successes_to_reset_cntr = 0; + + FLT sensor_variance = 1.; + FLT sensor_variance_per_second = 0; + int sensor_time_window = 1600000; + + int required_meas = 8; +}; + +struct PlaybackDataInput { + SurviveObject *so = nullptr; + SurvivePose position; + + std::vector vmask; + std::vector meas, cov; + PlaybackDataInput(SurviveObject *so, const SurvivePose &position) : so(so), position(position) { + int32_t sensor_count = so->sensor_ct; + vmask.resize(sensor_count * NUM_LIGHTHOUSES); + cov.resize(4 * sensor_count * NUM_LIGHTHOUSES); + meas.resize(2 * sensor_count * NUM_LIGHTHOUSES); + } + void shrink(size_t new_size) { + cov.resize(4 * new_size); + meas.resize(2 * new_size); + } + ~PlaybackDataInput() {} +}; + +struct PlaybackData { + BaseStationData bsd[2]; + std::vector inputs; +}; + +SBAData settings; +static size_t construct_input_from_scene(SurviveObject *so, uint32_t timestamp, char *vmask, double *meas, + double *cov) { + size_t rtn = 0; + auto scene = &so->activations; + for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) { + for (size_t lh = 0; lh < 2; lh++) { + if (SurviveSensorActivations_isPairValid(scene, settings.sensor_time_window, timestamp, sensor, lh)) { + double *a = scene->angles[sensor][lh]; + vmask[sensor * NUM_LIGHTHOUSES + lh] = 1; + + if (cov) { + *(cov++) = settings.sensor_variance + + std::abs((double)timestamp - scene->timecode[sensor][lh][0]) * + settings.sensor_variance_per_second / (double)so->timebase_hz; + *(cov++) = 0; + *(cov++) = 0; + *(cov++) = settings.sensor_variance + + std::abs((double)timestamp - scene->timecode[sensor][lh][1]) * + settings.sensor_variance_per_second / (double)so->timebase_hz; + } + meas[rtn++] = a[0]; + meas[rtn++] = a[1]; + } else { + vmask[sensor * NUM_LIGHTHOUSES + lh] = 0; + } + } + } + return rtn; +} +uint32_t timestamp; + +void light_process(SurviveObject *so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, + uint32_t lighthouse) { + timestamp = timecode; + survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode, length, lighthouse); +} + +void raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { + survive_default_raw_pose_process(so, lighthouse, pose); + PlaybackData *d = (PlaybackData *)so->ctx->user_ptr; + d->inputs.emplace_back(so, *pose); + auto &input = d->inputs.back(); + int meas = construct_input_from_scene(so, timestamp, &input.vmask.front(), &input.meas.front(), &input.cov.front()); + input.shrink(meas / 2); + if (meas / 2 < 12) + d->inputs.pop_back(); +} + +void lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose, SurvivePose *obj_pose) { + survive_default_lighthouse_pose_process(ctx, lighthouse, pose, obj_pose); + PlaybackData *d = (PlaybackData *)ctx->user_ptr; + d->bsd[lighthouse] = ctx->bsd[lighthouse]; +} + +std::map> errors; + +typedef struct { + survive_calibration_config calibration_config; + SurviveObject *so; + SurvivePose obj_pose; +} sba_context; + +static void str_metric_function(int j, int i, double *bi, double *xij, void *adata) { + SurvivePose obj = *(SurvivePose *)bi; + int sensor_idx = j >> 1; + int lh = j & 1; + + sba_context *ctx = (sba_context *)(adata); + SurviveObject *so = ctx->so; + + assert(lh < 2); + assert(sensor_idx < so->sensor_ct); + + quatnormalize(obj.Rot, obj.Rot); + FLT xyz[3]; + ApplyPoseToPoint(xyz, &obj, &so->sensor_locations[sensor_idx * 3]); + + // std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl; + SurvivePose *camera = &so->ctx->bsd[lh].Pose; + survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, lh, camera, xyz, xij); +} + +double sba_opt(SurviveContext *ctx, const survive_calibration_config &config, PlaybackDataInput &data) { + double *covx = 0; + SurviveObject *so = data.so; + + SurvivePose soLocation = data.position; + bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]) != 0; + + double opts[SBA_OPTSSZ] = {0}; + double info[SBA_INFOSZ] = {0}; + + sba_context _ctx = {config, so}; + + 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_str_levmar(1, // Number of 3d points + 0, // Number of 3d points to fix in spot + NUM_LIGHTHOUSES * so->sensor_ct, &data.vmask.front(), + soLocation.Pos, // Reads as the full pose though + 7, // pnp -- SurvivePose + &data.meas.front(), // x* -- measurement data + &data.cov.front(), // cov data + 2, // mnp -- 2 points per image + str_metric_function, + 0, // jacobia of metric_func + &_ctx, // user data + 50, // Max iterations + 0, // verbosity + opts, // options + info); // info + + if (status > 0) { + } else { + assert(false); + } + int meas_size = data.meas.size() / 2; + if (meas_size == 0) + return 0; + + { + SurviveContext *ctx = so->ctx; + // Docs say info[0] should be divided by meas; I don't buy it really... + static int cnt = 0; + if (cnt++ > 1000) { + SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (int)meas_size); + SV_INFO("%f cur reproj error", (info[1] / meas_size * 2)); + cnt = 0; + } + } + assert(!isinf(info[1])); + return info[1] / meas_size * 2; +} + +double find_avg_reproj_error(SurviveContext *ctx, const survive_calibration_config &config, PlaybackDataInput &data) { + auto vmask = &data.vmask.front(); + auto cov = &data.cov.front(); + auto meas = &data.meas.front(); + double err = 0; + size_t cnt = 0; + + err += sba_opt(ctx, config, data); + /* + for (size_t sensor = 0; sensor < data.so->sensor_ct; sensor++) { + for (size_t lh = 0; lh < 2; lh++) { + if( *(vmask++) ) { + cnt++; + FLT pt[3]; + ApplyPoseToPoint(pt, &data.position, data.so->sensor_locations + sensor * 3); + + FLT reproj_meas[2]; + survive_reproject_from_pose_with_config(ctx, &config, lh, &ctx->bsd[lh].Pose, pt, reproj_meas); + + auto x = reproj_meas[0] - meas[0]; + auto y = reproj_meas[1] - meas[1]; + err += cov[0]*x*x + cov[2]*y*y; + + meas += 2; + cov += 4; + } + } + }*/ + return err; +} + +double find_avg_reproj_error(SurviveContext *ctx, const survive_calibration_config &config, PlaybackData &data) { + double err = 0; + for (auto &in : data.inputs) { + err += find_avg_reproj_error(ctx, config, in); + } + return err / data.inputs.size(); +} + +int main(int argc, char **argv) { + std::vector> sections = { + {28, 0}, // phase + // { 5, 5 }, // tilt + // { 5, 10 }, // curve + // { 11, 15 } // gibs + useSin + }; + + for (int i = 1; i < argc; i++) { + PlaybackData data; + + char const *args[] = {argv[0], + "--use-bsd-cal", + "0", + "--calibrate", + "--playback-factor", + "0", + "--disambiguator", + "StateBased", + "--defaultposer", + "SBA", + "--sba-required-meas", + "12", + "--playback", + argv[i]}; + + auto ctx = survive_init(sizeof(args) / sizeof(args[0]), (char *const *)args); + ctx->user_ptr = &data; + + survive_install_raw_pose_fn(ctx, raw_pose_process); + survive_install_lighthouse_pose_fn(ctx, lighthouse_process); + survive_install_light_fn(ctx, light_process); + + while (survive_poll(ctx) == 0) { + } + + for (int j = 0; j < sections.size(); j++) { + auto &range = sections[j]; + for (size_t _i = 0; _i < (1 << range.first); _i++) { + int i = _i << range.second; + survive_calibration_config config = survive_calibration_config_create_from_idx(i); + if (i == survive_calibration_config_index(&config)) { + double error = find_avg_reproj_error(ctx, config, data); + errors[j][i] += error; + } + } + std::cerr << "Finished grouping " << j << std::endl; + } + + survive_close(ctx); + } + + for (int i = 0; i < errors.size(); i++) { + std::cout << "Grouping " << i << std::endl; + auto compFunctor = [](std::pair elem1, std::pair elem2) { + if (elem1.second == elem2.second) + return elem1.first < elem2.first; + return elem1.second < elem2.second; + }; + + std::set, typeof(compFunctor)> set(errors[i].begin(), errors[i].end(), compFunctor); + + for (auto err : set) { + survive_calibration_config config = survive_calibration_config_create_from_idx(err.first); + if (err.first == survive_calibration_config_index(&config)) { + double error = err.second; + std::cout << "Config " << err.first << " " << error << std::endl; + std::cout << config << std::endl; + } + } + } + return 0; +} -- cgit v1.2.3 From 22d40fb360fdb65da7916fb87f9b199f4f401f05 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 1 Apr 2018 07:52:00 -0600 Subject: overhal to calibration --- tools/findoptimalconfig/findoptimalconfig.cc | 160 +++++++++++++++++++++++---- 1 file changed, 139 insertions(+), 21 deletions(-) (limited to 'tools/findoptimalconfig/findoptimalconfig.cc') diff --git a/tools/findoptimalconfig/findoptimalconfig.cc b/tools/findoptimalconfig/findoptimalconfig.cc index 543958b..874ed84 100644 --- a/tools/findoptimalconfig/findoptimalconfig.cc +++ b/tools/findoptimalconfig/findoptimalconfig.cc @@ -9,6 +9,8 @@ #include #include +#include + std::ostream &operator<<(std::ostream &o, const survive_calibration_options_config &self) { o << "\t"; if (!self.enable[0] && !self.enable[1]) { @@ -49,7 +51,7 @@ struct SBAData { FLT sensor_variance = 1.; FLT sensor_variance_per_second = 0; - int sensor_time_window = 1600000; + int sensor_time_window = SurviveSensorActivations_default_tolerance; int required_meas = 8; }; @@ -57,10 +59,12 @@ struct SBAData { struct PlaybackDataInput { SurviveObject *so = nullptr; SurvivePose position; - + uint32_t timestamp; std::vector vmask; std::vector meas, cov; - PlaybackDataInput(SurviveObject *so, const SurvivePose &position) : so(so), position(position) { + SurviveSensorActivations activations; + PlaybackDataInput(SurviveObject *so, const SurvivePose &position) + : so(so), position(position), activations(so->activations) { int32_t sensor_count = so->sensor_ct; vmask.resize(sensor_count * NUM_LIGHTHOUSES); cov.resize(4 * sensor_count * NUM_LIGHTHOUSES); @@ -74,6 +78,7 @@ struct PlaybackDataInput { }; struct PlaybackData { + SurviveObject *so = nullptr; BaseStationData bsd[2]; std::vector inputs; }; @@ -116,15 +121,24 @@ void light_process(SurviveObject *so, int sensor_id, int acode, int timeinsweep, survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode, length, lighthouse); } +SurvivePose lastPose = {}; void raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { survive_default_raw_pose_process(so, lighthouse, pose); PlaybackData *d = (PlaybackData *)so->ctx->user_ptr; + d->so = so; d->inputs.emplace_back(so, *pose); auto &input = d->inputs.back(); + input.timestamp = timestamp; int meas = construct_input_from_scene(so, timestamp, &input.vmask.front(), &input.meas.front(), &input.cov.front()); input.shrink(meas / 2); - if (meas / 2 < 12) + + double dist = 0; + if (d->inputs.empty() == false) { + dist = dist3d(pose->Pos, lastPose.Pos); + } + if (meas / 2 < 8 || dist > .00009) d->inputs.pop_back(); + lastPose = *pose; } void lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose, SurvivePose *obj_pose) { @@ -166,7 +180,6 @@ double sba_opt(SurviveContext *ctx, const survive_calibration_config &config, Pl SurviveObject *so = data.so; SurvivePose soLocation = data.position; - bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]) != 0; double opts[SBA_OPTSSZ] = {0}; double info[SBA_INFOSZ] = {0}; @@ -191,6 +204,102 @@ double sba_opt(SurviveContext *ctx, const survive_calibration_config &config, Pl str_metric_function, 0, // jacobia of metric_func &_ctx, // user data + 100, // Max iterations + 0, // verbosity + opts, // options + info); // info + + int meas_size = data.meas.size() / 2; + if (meas_size == 0) + return 0; + + { + SurviveContext *ctx = so->ctx; + // Docs say info[0] should be divided by meas; I don't buy it really... + static int cnt = 0; + if (cnt++ > 1000) { + SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (int)meas_size); + SV_INFO("%f cur reproj error", (info[1] / meas_size * 2)); + cnt = 0; + } + } + assert(!isinf(info[1])); + return info[1] / meas_size * 2; +} + +struct optimal_cal_ctx { + std::vector sensors; + 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]}; + + 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); +} + +double find_optimal_cal(SurviveContext *ctx, const survive_calibration_config &config, PlaybackData &data) { + optimal_cal_ctx _ctx; + std::vector vmask; + std::vector 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++) { + auto scene = &in.activations; + if (SurviveSensorActivations_isPairValid(scene, settings.sensor_time_window, in.timestamp, sensor, + lh)) { + double *a = scene->angles[sensor][lh]; + vmask.emplace_back(1); //[sensor * NUM_LIGHTHOUSES + lh] = 1; + + meas.emplace_back(a[0]); + meas.emplace_back(a[1]); + } else { + vmask.emplace_back(0); + } + } + } + } + + double *covx = 0; + SurviveObject *so = data.so; + + double opts[SBA_OPTSSZ] = {0}; + double info[SBA_INFOSZ] = {0}; + + BaseStationCal cal[2] = {}; + + 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 + metric_function, + 0, // jacobia of metric_func + &_ctx, // user data 50, // Max iterations 0, // verbosity opts, // options @@ -200,7 +309,7 @@ double sba_opt(SurviveContext *ctx, const survive_calibration_config &config, Pl } else { assert(false); } - int meas_size = data.meas.size() / 2; + int meas_size = _ctx.sensors.size() / 2; if (meas_size == 0) return 0; @@ -215,17 +324,16 @@ double sba_opt(SurviveContext *ctx, const survive_calibration_config &config, Pl } } 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) { - auto vmask = &data.vmask.front(); - auto cov = &data.cov.front(); - auto meas = &data.meas.front(); - double err = 0; - size_t cnt = 0; - - err += sba_opt(ctx, config, data); + return sba_opt(ctx, config, data); /* for (size_t sensor = 0; sensor < data.so->sensor_ct; sensor++) { for (size_t lh = 0; lh < 2; lh++) { @@ -246,7 +354,6 @@ double find_avg_reproj_error(SurviveContext *ctx, const survive_calibration_conf } } }*/ - return err; } double find_avg_reproj_error(SurviveContext *ctx, const survive_calibration_config &config, PlaybackData &data) { @@ -259,10 +366,10 @@ double find_avg_reproj_error(SurviveContext *ctx, const survive_calibration_conf int main(int argc, char **argv) { std::vector> sections = { - {28, 0}, // phase - // { 5, 5 }, // tilt - // { 5, 10 }, // curve - // { 11, 15 } // gibs + useSin + {5, 0}, // phase + //{ 5, 5 }, // tilt + //{ 5, 10 }, // curve + //{ 11, 15 } // gibs + useSin }; for (int i = 1; i < argc; i++) { @@ -279,7 +386,9 @@ int main(int argc, char **argv) { "--defaultposer", "SBA", "--sba-required-meas", - "12", + "8", + "--sba-max-error", + ".1", "--playback", argv[i]}; @@ -293,10 +402,19 @@ int main(int argc, char **argv) { while (survive_poll(ctx) == 0) { } + survive_calibration_config config = {}; + // config.tilt.enable[0] = config.tilt.enable[1] = 1; + // config.curve.enable[0] = config.curve.enable[1] = 1; + config.phase.enable[0] = config.phase.enable[1] = 1; + // config.gibPhase.enable[0] = config.gibPhase.enable[1] = 1; + // config.gibMag.enable[0] = config.gibMag.enable[1] = 1; + + find_optimal_cal(ctx, config, data); + for (int j = 0; j < sections.size(); j++) { auto &range = sections[j]; for (size_t _i = 0; _i < (1 << range.first); _i++) { - int i = _i << range.second; + int i = (_i << range.second); survive_calibration_config config = survive_calibration_config_create_from_idx(i); if (i == survive_calibration_config_index(&config)) { double error = find_avg_reproj_error(ctx, config, data); -- cgit v1.2.3 From 47c7fb15182700fb403894f65beaf143a7fad6ab Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 1 Apr 2018 12:23:48 -0600 Subject: Tweaked how reproject / calibate interact --- tools/findoptimalconfig/findoptimalconfig.cc | 72 +--------------------------- 1 file changed, 2 insertions(+), 70 deletions(-) (limited to 'tools/findoptimalconfig/findoptimalconfig.cc') diff --git a/tools/findoptimalconfig/findoptimalconfig.cc b/tools/findoptimalconfig/findoptimalconfig.cc index 874ed84..88a43eb 100644 --- a/tools/findoptimalconfig/findoptimalconfig.cc +++ b/tools/findoptimalconfig/findoptimalconfig.cc @@ -11,35 +11,6 @@ #include #include -std::ostream &operator<<(std::ostream &o, const survive_calibration_options_config &self) { - o << "\t"; - if (!self.enable[0] && !self.enable[1]) { - o << "disabled"; - return o; - } - - o << "swap: " << self.swap << std::endl; - for (int i = 0; i < 2; i++) { - if (self.enable[i]) { - o << "\tinvert[" << i << "]: " << self.invert[i]; - } else { - o << "\t" << i << ": disabled"; - } - } - return o; -} - -std::ostream &operator<<(std::ostream &o, const survive_calibration_config &self) { - o << "Index: " << survive_calibration_config_index(&self) << std::endl; - o << "Phase: " << std::endl << self.phase << std::endl; - o << "Tilt: " << std::endl << self.tilt << std::endl; - o << "Curve: " << std::endl << self.curve << std::endl; - o << "gibPhase: " << std::endl << self.gibPhase << std::endl; - o << "gibMag: " << std::endl << self.gibMag << std::endl; - o << "gibUseSin: " << self.gibUseSin << std::endl; - return o; -} - struct SBAData { int last_acode = -1; int last_lh = -1; @@ -279,9 +250,7 @@ double find_optimal_cal(SurviveContext *ctx, const survive_calibration_config &c double opts[SBA_OPTSSZ] = {0}; double info[SBA_INFOSZ] = {0}; - BaseStationCal cal[2] = {}; - - opts[0] = SBA_INIT_MU; + survive_calibration_config opts[0] = SBA_INIT_MU; opts[1] = SBA_STOP_THRESH; opts[2] = SBA_STOP_THRESH; opts[3] = SBA_STOP_THRESH; @@ -402,49 +371,12 @@ int main(int argc, char **argv) { while (survive_poll(ctx) == 0) { } - survive_calibration_config config = {}; - // config.tilt.enable[0] = config.tilt.enable[1] = 1; - // config.curve.enable[0] = config.curve.enable[1] = 1; - config.phase.enable[0] = config.phase.enable[1] = 1; - // config.gibPhase.enable[0] = config.gibPhase.enable[1] = 1; - // config.gibMag.enable[0] = config.gibMag.enable[1] = 1; + survive_calibration_config config = survive_calibration_config_ctor(); find_optimal_cal(ctx, config, data); - for (int j = 0; j < sections.size(); j++) { - auto &range = sections[j]; - for (size_t _i = 0; _i < (1 << range.first); _i++) { - int i = (_i << range.second); - survive_calibration_config config = survive_calibration_config_create_from_idx(i); - if (i == survive_calibration_config_index(&config)) { - double error = find_avg_reproj_error(ctx, config, data); - errors[j][i] += error; - } - } - std::cerr << "Finished grouping " << j << std::endl; - } - survive_close(ctx); } - for (int i = 0; i < errors.size(); i++) { - std::cout << "Grouping " << i << std::endl; - auto compFunctor = [](std::pair elem1, std::pair elem2) { - if (elem1.second == elem2.second) - return elem1.first < elem2.first; - return elem1.second < elem2.second; - }; - - std::set, typeof(compFunctor)> set(errors[i].begin(), errors[i].end(), compFunctor); - - for (auto err : set) { - survive_calibration_config config = survive_calibration_config_create_from_idx(err.first); - if (err.first == survive_calibration_config_index(&config)) { - double error = err.second; - std::cout << "Config " << err.first << " " << error << std::endl; - std::cout << config << std::endl; - } - } - } return 0; } -- cgit v1.2.3 From aea08a70a033cc0aef0998267fadb54af5fb2c69 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 1 Apr 2018 15:37:16 -0600 Subject: More optimization of scale params for calibration --- tools/findoptimalconfig/findoptimalconfig.cc | 54 ++++++++++++++-------------- 1 file changed, 26 insertions(+), 28 deletions(-) (limited to 'tools/findoptimalconfig/findoptimalconfig.cc') 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 sensors; + std::vector 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 vmask; std::vector 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); } -- cgit v1.2.3