diff options
author | Justin Berger <j.david.berger@gmail.com> | 2018-04-02 10:10:33 -0600 |
---|---|---|
committer | Justin Berger <j.david.berger@gmail.com> | 2018-04-02 10:10:33 -0600 |
commit | 75460f240c9d003e4ca2e6dda9b2146a74df7ffa (patch) | |
tree | 957b26f0539df176b61ad2ec72fbb0658b147919 /tools | |
parent | 2b63278497130d01b1fbc7e6a94b6ad8e32ab4dd (diff) | |
parent | 1724abef15a4090640bd82ba408681438316de7e (diff) | |
download | libsurvive-75460f240c9d003e4ca2e6dda9b2146a74df7ffa.tar.gz libsurvive-75460f240c9d003e4ca2e6dda9b2146a74df7ffa.tar.bz2 |
Merge remote-tracking branch 'origin/master' into imu
Diffstat (limited to 'tools')
-rw-r--r-- | tools/findoptimalconfig/Makefile | 16 | ||||
-rw-r--r-- | tools/findoptimalconfig/findoptimalconfig.cc | 380 | ||||
-rw-r--r-- | tools/lightlengthparams/Makefile | 15 | ||||
-rw-r--r-- | tools/lightlengthparams/lightlengthparams.c | 112 | ||||
-rw-r--r-- | tools/showreproject/Makefile | 16 | ||||
-rw-r--r-- | tools/showreproject/showreproject.cc | 271 | ||||
-rw-r--r-- | tools/viz/index.html | 2 | ||||
-rw-r--r-- | tools/viz/survive_viewer.js | 320 |
8 files changed, 986 insertions, 146 deletions
diff --git a/tools/findoptimalconfig/Makefile b/tools/findoptimalconfig/Makefile new file mode 100644 index 0000000..f174812 --- /dev/null +++ b/tools/findoptimalconfig/Makefile @@ -0,0 +1,16 @@ +all : findoptimalconfig + +SRT:=../.. + +LIBSURVIVE:=$(SRT)/lib/libsurvive.so + +CFLAGS:=-I$(SRT)/redist -I$(SRT)/include -O0 -g -DFLT=double -DUSE_DOUBLE #-fsanitize=address -fsanitize=undefined +LDFLAGS:=-lm -lpthread -llapacke -lcblas + +findoptimalconfig : findoptimalconfig.cc $(LIBSURVIVE) + cd ../..;make + g++ $(CFLAGS) -o $@ $^ $(LDFLAGS) + +clean : + rm -rf findoptimalconfig + diff --git a/tools/findoptimalconfig/findoptimalconfig.cc b/tools/findoptimalconfig/findoptimalconfig.cc new file mode 100644 index 0000000..b94590f --- /dev/null +++ b/tools/findoptimalconfig/findoptimalconfig.cc @@ -0,0 +1,380 @@ +#include <assert.h> +#include <iostream> +#include <libsurvive/survive.h> +#include <libsurvive/survive_reproject.h> +#include <map> +#include <math.h> +#include <memory> +#include <set> +#include <vector> + +#include <sba/sba.h> +#include <survive_reproject.h> + +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 = SurviveSensorActivations_default_tolerance; + + int required_meas = 8; +}; + +struct PlaybackDataInput { + SurviveObject *so = nullptr; + SurvivePose position; + uint32_t timestamp; + std::vector<char> vmask; + std::vector<double> meas, cov; + 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); + 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 { + SurviveObject *so = nullptr; + BaseStationData bsd[2]; + std::vector<PlaybackDataInput> 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); +} + +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); + + 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) { + survive_default_lighthouse_pose_process(ctx, lighthouse, pose, obj_pose); + PlaybackData *d = (PlaybackData *)ctx->user_ptr; + d->bsd[lighthouse] = ctx->bsd[lighthouse]; +} + +std::map<size_t, std::map<size_t, double>> 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; + + 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 + 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<double> sensors; + std::vector<int> lighthouses; + SurviveContext *ctx; +}; + +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; + + survive_reproject_from_pose_with_bsd(&bsd, &cfg, &ctx->ctx->bsd[lh].Pose, sensorInWorld, xij); +} + +double find_optimal_cal(SurviveContext *ctx, PlaybackData &data) { + optimal_cal_ctx _ctx; + std::vector<char> vmask; + std::vector<double> cov, meas; + _ctx.ctx = ctx; + 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]); + 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)) { + 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}; + + 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 * 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 + 50, // Max iterations + 0, // verbosity + opts, // options + info); // info + + if (status > 0) { + } else { + assert(false); + } + int meas_size = _ctx.sensors.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])); + std::cerr << "Used " << meas_size << " measurements" << std::endl; + + return info[1] / meas_size * 2; +} +double find_avg_reproj_error(SurviveContext *ctx, const survive_calibration_config &config, PlaybackDataInput &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++) { + 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; + } + } + }*/ +} + +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<std::pair<size_t, size_t>> sections = { + {5, 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", + "8", + "--sba-max-error", + ".1", + "--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) { + } + + find_optimal_cal(ctx, data); + + survive_close(ctx); + } + + return 0; +} diff --git a/tools/lightlengthparams/Makefile b/tools/lightlengthparams/Makefile new file mode 100644 index 0000000..b743f4f --- /dev/null +++ b/tools/lightlengthparams/Makefile @@ -0,0 +1,15 @@ +all : lightlengthparams + +SRT:=../.. + +LIBSURVIVE:=$(SRT)/lib/libsurvive.so + +CFLAGS:=-I$(SRT)/redist -I$(SRT)/include -O0 -g -DFLT=double -DUSE_DOUBLE +LDFLAGS:=-lm -lpthread -llapacke -lcblas + +lightlengthparams : lightlengthparams.c $(LIBSURVIVE) + gcc $(CFLAGS) -o $@ $^ $(LDFLAGS) + +clean : + rm -rf lightlengthparams + diff --git a/tools/lightlengthparams/lightlengthparams.c b/tools/lightlengthparams/lightlengthparams.c new file mode 100644 index 0000000..3b28b3f --- /dev/null +++ b/tools/lightlengthparams/lightlengthparams.c @@ -0,0 +1,112 @@ +#include <assert.h> +#include <libsurvive/survive.h> +#include <math.h> +#include <stdio.h> +#include <string.h> + +uint32_t current_timecode; + +void light_process(SurviveObject *so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, + uint32_t lighthouse) { + current_timecode = timecode; + survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode, length, lighthouse); +} + +void raw_pose_process(SurviveObject *so, uint8_t _lh, SurvivePose *pose2w) { + survive_default_raw_pose_process(so, _lh, pose2w); + + /*printf("Pose: "); + for(int i = 0;i < 7;i++) { + printf("%f, ", pose2w->Pos[i]); + }; + printf("\n"); + */ + for (int lh = 0; lh < 2; lh++) { + SurvivePose pose2lh = {}; + SurvivePose w2lh = {}; + InvertPose(&w2lh, &so->ctx->bsd[lh].Pose); + ApplyPoseToPose(&pose2lh, &w2lh, pose2w); + + SurviveSensorActivations *scene = &so->activations; + for (size_t sensor_idx = 0; sensor_idx < so->sensor_ct; sensor_idx++) { + if (SurviveSensorActivations_isPairValid(scene, SurviveSensorActivations_default_tolerance / 2, + current_timecode, sensor_idx, lh)) { + uint32_t *lengths = scene->lengths[sensor_idx][lh]; + + const FLT *sensor_location = so->sensor_locations + 3 * sensor_idx; + const FLT *sensor_normals = so->sensor_normals + 3 * sensor_idx; + + LinmathPoint3d sensor = {}, sensorN = {}; + + scale3d(sensorN, sensor_normals, .01); + add3d(sensorN, sensor_location, sensor_normals); + + /* + printf("\n"); + printf("%f, %f, %f\n", sensor_location[0], sensor_location[1], sensor_location[2]); + printf("%f, %f, %f\n", sensorN[0], sensorN[1], sensorN[2]); + + FLT m[4][4]; + PoseToMatrix((FLT*)m, &pose2lh); + + for(int i = 0;i < 7;i++) { + printf("%f, ", pose2lh.Pos[i]); + }; + printf("\n"); + for(int i = 0;i < 4;i++) { + for(int j = 0;j < 4;j++) { + printf("%f, ", m[i][j]); + } + printf("\n"); + } + printf("\n"); + */ + ApplyPoseToPoint(sensor, &pose2lh, sensor_location); + ApplyPoseToPoint(sensorN, &pose2lh, sensorN); + + // printf("%f, %f, %f\n", sensor[0], sensor[1], sensor[2]); + // printf("%f, %f, %f\n", sensorN[0], sensorN[1], sensorN[2]); + FLT dist = magnitude3d(sensor); + LinmathVec3d zout = {0, 0, -dist}; + LinmathQuat r; + quatfrom2vectors(r, sensor, zout); + quatrotatevector(sensorN, r, sensorN); + sub3d(sensorN, sensorN, zout); + + FLT dot = dot3d(sensor, sensorN); + if (dist > 20 || dist < 0.25) + continue; + + FLT angs[2] = {}; + for (int axis = 0; axis < 2; axis++) { + angs[axis] = cos(atan2(fabs(sensorN[axis ? 0 : 1]), sensorN[2])); + } + FLT area = angs[0] * angs[1]; + + printf("%u\t%u\t%lu\t%f\t%f\t%f\t%f\t%u\t%u\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n", current_timecode, lh, + sensor_idx, dist, angs[0], angs[1], area, lengths[0], lengths[1], dot, sensor[0], sensor[1], + sensor[2], sensorN[0], sensorN[1], sensorN[2]); + + // assert(angs[0] >= 0); + } + } + } +} + +int main(int argc, char **argv) { + SurviveContext *ctx = survive_init(argc, argv); + if (ctx == 0) // implies -help or similiar + return 0; + + printf("timecode\tlh\tsensor_idx\tdistance\tnorm_x\tnorm_y\tarea\tlength_x\tlength_y\tdot\tx\ty\tz\tnx\tny\tnz\n"); + + survive_startup(ctx); + + survive_install_raw_pose_fn(ctx, raw_pose_process); + survive_install_light_fn(ctx, light_process); + while (survive_poll(ctx) == 0) { + } + + survive_close(ctx); + return 0; +} diff --git a/tools/showreproject/Makefile b/tools/showreproject/Makefile new file mode 100644 index 0000000..f6d5925 --- /dev/null +++ b/tools/showreproject/Makefile @@ -0,0 +1,16 @@ +all : showreproject + +SRT:=../.. + +LIBSURVIVE:=$(SRT)/lib/libsurvive.so + +CFLAGS:=-I$(SRT)/redist -I$(SRT)/include -O0 -g -DFLT=double -DUSE_DOUBLE #-fsanitize=address -fsanitize=undefined +LDFLAGS:=-lm -lpthread -llapacke -lcblas -lopencv_core -lopencv_highgui -lopencv_imgproc -lopencv_imgcodecs + +showreproject : showreproject.cc $(LIBSURVIVE) + cd ../..;make + g++ $(CFLAGS) -o $@ $^ $(LDFLAGS) + +clean : + rm -rf showreproject + diff --git a/tools/showreproject/showreproject.cc b/tools/showreproject/showreproject.cc new file mode 100644 index 0000000..caa7a66 --- /dev/null +++ b/tools/showreproject/showreproject.cc @@ -0,0 +1,271 @@ + +#include <chrono> +#include <iostream> +#include <libsurvive/survive.h> +#include <libsurvive/survive_reproject.h> +#include <map> +#include <math.h> +#include <memory> +#include <set> +#include <vector> + +#include "opencv2/imgproc.hpp" +#include <opencv2/highgui/highgui.hpp> +#include <opencv2/imgcodecs.hpp> + +uint32_t timestamp; + +cv::Mat_<cv::Vec3b> img; +cv::Mat_<cv::Vec3b> err[2]; + +cv::Vec3f flow2rgb(float x, float y, float scale = 1) { + cv::Mat_<cv::Vec3f> hsv(1, 1); + hsv(0, 0) = {atan2(y, x) * 180. / M_PI, 1.0, std::min(1.0f, sqrt(x * x + y * y) * scale)}; + cv::Mat_<cv::Vec3f> bgr(1, 1); + cv::cvtColor(hsv, bgr, CV_HSV2RGB_FULL); + return bgr(0, 0) * 255; +} + +static void draw_viz(cv::Mat &img, double scale = 1) { + double size = 50; + cv::Point2f origin(size, size); + for (double r = size; r > size / 100.; r -= size / 100.) { + for (double theta = 0; theta < 2 * M_PI; theta += .01) { + auto x = cos(theta) * r; + auto y = sin(theta) * r; + auto clr = flow2rgb(x, y, scale / size); + cv::line(img, cv::Point2f(x, y) + origin, origin, clr); + } + } +} + +double error = 0; +int error_count = 0; + +static void redraw(SurviveContext *ctx) { + int SIZE = 1000; + int shift = ctx->user_ptr ? 1 : 0; + + auto show_pov = 130. / 180; + auto fov = 120. / 180. * SIZE / show_pov; + + auto map = [&](const double *a) { + auto x = a[0] * SIZE / (M_PI) / show_pov + SIZE / 2; + return cv::Point(x, SIZE - (a[1] * SIZE / (M_PI) + SIZE / 2)); + }; + + auto region = img.data ? img(cv::Rect(SIZE * shift, 0, SIZE, SIZE)) : cv::Mat_<cv::Vec3b>(); + + if (region.data) { + + region.setTo(cv::Vec3b(0, 0, 0)); + cv::rectangle(region, cv::Point(SIZE / 2 - fov / 2, SIZE / 2 - fov / 2), + cv::Point(SIZE / 2 + fov / 2, SIZE / 2 + fov / 2), cv::Vec3b(255, 255, 255)); + } + + // eregion.copyTo(region); + + for (int i = 0; i < ctx->objs_ct; i++) { + auto so = ctx->objs[i]; + auto scene = &so->activations; + for (size_t lh = 0; lh < 2; lh++) { + auto eregion = err[lh](cv::Rect(SIZE * shift, 0, SIZE, SIZE)); + + for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) { + + auto ncolor = cv::Vec3b(lh == 0 ? 255 : 0, + 255, // sensor / (double)so->sensor_ct * 255, + i / (double)ctx->objs_ct * 255); + auto rcolor = cv::Vec3b(lh == 0 ? 255 : 0, + 128, // sensor / (double)so->sensor_ct * 255, + i / (double)ctx->objs_ct * 255); + + if (SurviveSensorActivations_isPairValid(scene, SurviveSensorActivations_default_tolerance, timestamp, + sensor, lh)) { + const double *a = scene->angles[sensor][lh]; + // FLT a[2]; + // survive_apply_bsd_calibration(so->ctx, lh, _a, a); + + auto l = scene->lengths[sensor][lh]; + double r = std::max(1., (l[0] + l[1]) / 1000.); + // std::cerr << lh << "\t" << sensor << "\t" << ((l[0] + l[1]) / 2000.) << "\t" << l[0] << "\t" << + // l[1] << std::endl; + if (region.data) + cv::circle(region, map(a), r, ncolor); + + FLT point3d[3]; + FLT out[2]; + ApplyPoseToPoint(point3d, &so->OutPose, so->sensor_locations + 3 * sensor); + survive_reproject(ctx, lh, point3d, out); + + double ex = out[0] - a[0]; + double ey = out[1] - a[1]; + double err_add = sqrt(ex * ex + ey * ey); + error += err_add; + error_count++; + + auto &e = eregion(map(a).y, map(a).x); + e = flow2rgb(ex, ey, 100); + + cv::putText(img, std::to_string(error / error_count), cv::Point2f(10, 20), CV_FONT_HERSHEY_PLAIN, 1, + cv::Scalar(255, 255, 255)); + + if (region.data) { + cv::line(region, map(a) + cv::Point(ex * 10000, -ey * 10000), map(a), + cv::Scalar(255, 255, 255)); + cv::rectangle(region, map(out) - cv::Point(r, r), map(out) + cv::Point(r, r), rcolor); + } + } + } + }; + } + if (img.data) { + cv::imshow("Reprojection", img); + } +} + +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); +} + +SurvivePose lastPose = {}; + +void raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { + survive_default_raw_pose_process(so, lighthouse, pose); + auto d = dist3d(lastPose.Pos, pose->Pos); + // std::cerr << d << std::endl; + if (d < .01) { + redraw(so->ctx); + } + lastPose = *pose; +} + +void lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose, SurvivePose *obj_pose) { + survive_default_lighthouse_pose_process(ctx, lighthouse, pose, obj_pose); +} + +SurviveContext *create(int argc, char **argv) { + auto ctx = survive_init(argc, argv); + + survive_install_raw_pose_fn(ctx, raw_pose_process); + survive_install_lighthouse_pose_fn(ctx, lighthouse_process); + survive_install_light_fn(ctx, light_process); + + return ctx; +} + +void drawbsds(SurviveContext *ctx) { + int SIZE = 1000; + + std::vector<SurviveCalFlag> show_flags = { + SVCal_All, SVCal_Phase, SVCal_Gib, SVCal_Curve, SVCal_Tilt, + }; + + for (auto f : show_flags) { + for (int lh = 0; lh < 2; lh++) { + cv::Mat_<cv::Vec3b> img = cv::Mat_<cv::Vec3b>(SIZE, SIZE); + img.setTo(cv::Vec3b(0, 0, 0)); + for (int x = 0; x < SIZE; x++) { + for (int y = 0; y < SIZE; y++) { + FLT in[2] = {x * M_PI / SIZE - M_PI / 2., y * M_PI / SIZE - M_PI / 2.}; + if (fabs(in[0]) > 60. / 180 * M_PI || fabs(in[1]) > 60. / 180 * M_PI) + continue; + + FLT out[2]; + auto config = survive_calibration_config_ctor(); + config.use_flag = f; + survive_apply_bsd_calibration_by_config(ctx, lh, &config, in, out); + double ex = out[0] - in[0]; + double ey = out[1] - in[1]; + if (f == SVCal_All) { + ex -= ctx->bsd[lh].fcal.phase[0]; + ey -= ctx->bsd[lh].fcal.phase[1]; + } + + // Make it opposite of angles + ex *= -1; + ey *= -1; + + img(y, x) = flow2rgb(ex, ey, 100); + } + } + draw_viz(img); + cv::imwrite("BSD" + std::to_string(lh) + "_" + std::to_string(f) + ".png", img); + if (f == SVCal_All) + cv::imshow("BSD" + std::to_string(lh), img); + } + } +} + +int main(int argc, char **argv) { + // for (int i = 0; i < 1 << 5; i++) { + // size_t cidx = 15 | (i << 5); + + // auto conf = survive_calibration_config_create_from_idx(cidx); + // if (survive_calibration_config_index(&conf) != cidx) + // continue; + + auto ctx1 = create(argc, argv); + size_t cidx = survive_configi(ctx1, "default-cal-conf", SC_GET, 0); + size_t idx = survive_configi(ctx1, "default-cal-conf2", SC_GET, 0); + + SurviveContext *ctx2 = 0; + int numCtx = 1; + int ctx2_flag = 1; + if (idx != 0) { + numCtx++; + ctx2 = create(argc, argv); + ctx2->user_ptr = (void *)&ctx2_flag; + } + + size_t showui = survive_configi(ctx1, "show-ui", SC_GET, 0); + + drawbsds(ctx1); + + int waitUpdate = 100; + int SIZE = 1000; + if (showui) { + img = cv::Mat_<cv::Vec3b>(SIZE, numCtx * SIZE); + img.setTo(cv::Vec3b(0, 0, 0)); + } + for (int lh = 0; lh < 2; lh++) { + err[lh] = cv::Mat_<cv::Vec3b>(SIZE, numCtx * SIZE); + err[lh].setTo(cv::Vec3b(0, 0, 0)); + } + + if (img.data && false) { + cv::imshow("Reprojection", img); + cv::waitKey(0); + } + + auto start = std::chrono::high_resolution_clock::now(); + while (survive_poll(ctx1) == 0 && (ctx2 == 0 || survive_poll(ctx2) == 0)) { + auto now = std::chrono::high_resolution_clock::now(); + if ((now - start) > std::chrono::milliseconds(33)) { + cv::waitKey(1); + start = now; + } + } + + for (int i = 0; i < 2; i++) { + draw_viz(err[i]); + cv::putText(err[i], std::to_string(error / error_count), cv::Point2f(100, 20), CV_FONT_HERSHEY_PLAIN, 1, + cv::Scalar(255, 255, 255)); + cv::putText(err[i], std::to_string(cidx), cv::Point2f(100, 40), CV_FONT_HERSHEY_PLAIN, 1, + cv::Scalar(255, 255, 255)); + + auto name = "LH" + std::to_string(i); + cv::imwrite(name + "_" + std::to_string(cidx) + ".png", err[i]); + cv::imshow(name, err[i]); + } + + std::cerr << "Error: " << error / error_count << std::endl; + + int c = '\0'; + while (((c = cv::waitKey(0)) & 0xff) != 'q') { + std::cerr << (uint8_t)c << std::endl; + } + return 0; +} diff --git a/tools/viz/index.html b/tools/viz/index.html index b146b5d..2987555 100644 --- a/tools/viz/index.html +++ b/tools/viz/index.html @@ -12,6 +12,7 @@ <body> <div id="ThreeJS" style="z-index: 1; position: absolute; left:0px; top:0px"></div> <div id="cam-control" style="z-index: 2;border:1px solid white;position:absolute"> + <input type="checkbox" id="trails">Trails</input><br/> <button id="toggleBtn"> Toggle 2D View </button> @@ -25,6 +26,7 @@ position: absolute; bottom: 20px; overflow: auto; + white-space: pre-wrap; background-color: rgba(0,200,200, .25); z-index: 3;"> diff --git a/tools/viz/survive_viewer.js b/tools/viz/survive_viewer.js index 81de2a0..c8a7b23 100644 --- a/tools/viz/survive_viewer.js +++ b/tools/viz/survive_viewer.js @@ -5,6 +5,9 @@ var angles = {}; var ctx; var canvas; var oldDrawTime = 0; +var timecode = {}; +var oldPoseTime = 0, poseCnt = 0; +var scene, camera, renderer, floor; $(function() { $("#toggleBtn").click(function() { $("#cam").toggle(); }); }); @@ -16,16 +19,16 @@ function add_lighthouse(idx, p, q) { group.position.fromArray(p); group.quaternion.fromArray([ q[1], q[2], q[3], q[0] ]); - var height = 3; - var geometry = new THREE.ConeGeometry(Math.sin(1.0472) * height, height, 4, 1, true); + var height = 10; + + var geometry = new THREE.ConeGeometry(height / Math.cos(60 / 180 * Math.PI), height, 4, 1, true); var material = new THREE.MeshBasicMaterial({ - wireframe : true, - vertexColor : true, - color : 0x111111, - opacity : 0.09, + color : 0x1F1FFF, + opacity : 0.02, transparent : true, blending : THREE.AdditiveBlending, - side : THREE.BothSides + side : THREE.DoubleSide, + depthTest : false }); var cone = new THREE.Mesh(geometry, material); @@ -34,16 +37,14 @@ function add_lighthouse(idx, p, q) { var lhBox = new THREE.Mesh(lhBoxGeom, lhBoxMaterial); group.add(lhBox); - cone.translateZ(-height / 2) - cone.rotateZ(Math.PI / 4) - cone.rotateX(Math.PI / 2) - // cone.position.z + cone.translateZ(-height / 2); + cone.rotateZ(Math.PI / 4); + cone.rotateX(Math.PI / 2); group.add(cone); group.add(lh); scene.add(group); - // DrawCoordinateSystem(p[0], p[1], p[2], q[0], q[1], q[2], q[3]); } function recolorTrackers(when) { @@ -129,7 +130,7 @@ function redrawCanvas(when) { for (var key in angles) { for (var lh = 0; lh < 2; lh++) { - var bvalue = {"WW0" : "FF", "TR0" : "00"}; + var bvalue = {"WW0" : "FF", "TR0" : "00", "HMD" : "88"}; ctx.strokeStyle = (lh === 0 ? "#FF00" : "#00FF") + bvalue[key]; if (angles[key][lh]) @@ -157,22 +158,27 @@ function redrawCanvas(when) { } } -function create_object(info) { +function create_tracked_object(info) { var sensorGeometry = new THREE.SphereGeometry(.01, 32, 16); var group = new THREE.Group(); group.sensors = []; if (info.config && info.config.lighthouse_config) { for (var idx in info.config.lighthouse_config.modelPoints) { var p = info.config.lighthouse_config.modelPoints[idx]; - var color = 0xFFFFFF; // / info.points.length * idx; - if (idx === 10) + var pn = info.config.lighthouse_config.modelNormals[idx]; + var color = idx / info.config.lighthouse_config.modelPoints * 0xFFFFFF; + if (idx === 0) color = 0x00ff00; - if (idx === 12) - color = 0x0000ff; var sensorMaterial = new THREE.MeshBasicMaterial({color : color}); var newSensor = new THREE.Mesh(sensorGeometry, sensorMaterial); newSensor.position.set(p[0], p[1], p[2]); + var normalGeom = new THREE.Geometry(); + normalGeom.vertices.push(newSensor.position, + new THREE.Vector3(p[0] + pn[0] * .02, p[1] + pn[1] * .02, p[2] + pn[2] * .02)); + var normal = + new THREE.Line(normalGeom, new THREE.LineBasicMaterial({color : idx == 4 ? 0xFF0000 : 0x00FF00})); + group.add(normal); group.sensors[idx] = sensorMaterial; group.add(newSensor); } @@ -184,149 +190,171 @@ function create_object(info) { scene.add(group); } -var timecode = {}; +var trails; +var MAX_LINE_POINTS = 100000; +$(function() { + $("#trails").change(function() { + if (this.checked) { + var geometry = new THREE.Geometry(); + var material = new THREE.LineBasicMaterial({color : 0x305ea8}); -function parseLine(msg) { - var s = msg.split(' '); + for (i = 0; i < MAX_LINE_POINTS; i++) { + geometry.vertices.push(new THREE.Vector3(0, 0, 0)); + } + geometry.dynamic = true; + + trails = new THREE.Line(geometry, material); + + scene.add(trails); + } else { + if (trails) + scene.remove(trails); + } + }); +}); + +var survive_log_handlers = { + "LH_POSE" : function(v) { + var obj = { + lighthouse : parseInt(v[1]), + position : [ parseFloat(v[3]), parseFloat(v[4]), parseFloat(v[5]) ], + quat : [ parseFloat(v[6]), parseFloat(v[7]), parseFloat(v[8]), parseFloat(v[9]) ] + }; - var command_mappings = { - "LH_POSE" : function(v) { - return { - type : "lighthouse_pose", - lighthouse : parseInt(v[1]), - position : [ parseFloat(v[3]), parseFloat(v[4]), parseFloat(v[5]) ], - quat : [ parseFloat(v[6]), parseFloat(v[7]), parseFloat(v[8]), parseFloat(v[9]) ] - }; - }, - "POSE" : function(v) { - return { - type: "pose", position: [ parseFloat(v[3]), parseFloat(v[4]), parseFloat(v[5]) ], - quat: [ parseFloat(v[6]), parseFloat(v[7]), parseFloat(v[8]), parseFloat(v[9]) ] + add_lighthouse(obj.lighthouse, obj.position, obj.quat); + }, + "POSE" : function(v, tracker) { + var obj = { + tracker : v[1], + position : [ parseFloat(v[3]), parseFloat(v[4]), parseFloat(v[5]) ], + quat : [ parseFloat(v[6]), parseFloat(v[7]), parseFloat(v[8]), parseFloat(v[9]) ] + }; + + if (objs[obj.tracker]) { + var now = new Date().getTime(); + if (oldPoseTime + 5000 < now) { + oldPoseTime = now; + console.log((poseCnt / 5) + "hz"); + poseCnt = 0; } - }, - "CONFIG" : function(v) { - var configStr = s.slice(3).join(' '); - var config = JSON.parse(configStr); - - return { type: "htc_config", config: config } - - }, - 'A' : function(v) { - return { - type: 'angle', sensor_id: parseInt(v[3]), acode: parseInt(v[4]), timecode: parseInt(v[5]), - length: parseFloat(v[6]), angle: parseFloat(v[7]), lighthouse: parseInt(v[8]) + poseCnt++; + objs[obj.tracker].position.set(obj.position[0], obj.position[1], obj.position[2]); + objs[obj.tracker].quaternion.set(obj.quat[1], obj.quat[2], obj.quat[3], obj.quat[0]); + objs[obj.tracker].verticesNeedUpdate = true; + + if (trails) { + + trails.geometry.vertices.push(trails.geometry.vertices.shift()); // shift the array + trails.geometry.vertices[MAX_LINE_POINTS - 1] = + new THREE.Vector3(obj.position[0], obj.position[1], obj.position[2]); + trails.geometry.verticesNeedUpdate = true; } - }, - 'LOG' : function(v) { - return { type: "info", msg: s.slice(3).join(' ') } - }, - "I" : function(v) { - return { - type : "imu", - mask : parseInt(v[3]), - timecode : parseInt(v[4]), - accelgyro : [ - parseFloat(v[5]), parseFloat(v[6]), parseFloat(v[7]), parseFloat(v[8]), parseFloat(v[9]), - parseFloat(v[10]) - ] - - }; } - }; - if (command_mappings[s[2]]) { - var rtn = command_mappings[s[2]](s); - rtn.time = parseFloat(s[0]); - rtn.tracker = s[1]; - return rtn; + }, + "CONFIG" : function(v, tracker) { + var configStr = v.slice(3).join(' '); + var config = JSON.parse(configStr); + var obj = {config : config, tracker : v[1]}; + + create_tracked_object(obj); + }, + 'A' : function(v, tracker) { + var obj = { + tracker : v[1], + sensor_id : parseInt(v[3]), + acode : parseInt(v[4]), + timecode : parseInt(v[5]), + length : parseFloat(v[6]), + angle : parseFloat(v[7]), + lighthouse : parseInt(v[8]) + }; + + angles[obj.tracker] = angles[obj.tracker] || {}; + angles[obj.tracker][obj.lighthouse] = angles[obj.tracker][obj.lighthouse] || {}; + angles[obj.tracker][obj.lighthouse][obj.sensor_id] = angles[obj.tracker][obj.lighthouse][obj.sensor_id] || {}; + + angles[obj.tracker][obj.lighthouse][obj.sensor_id][obj.acode & 1] = [ obj.angle, obj.timecode ]; + timecode[obj.tracker] = obj.timecode; + }, + 'LOG' : function(v) { + var msg = v.slice(3).join(' '); + + var consoleDiv = $("#console"); + consoleDiv.append(msg + "</br>"); + consoleDiv[0].scrollTop = consoleDiv[0].scrollHeight; + + }, + "I" : function(v, tracker) { + var obj = { + mask : parseInt(v[3]), + timecode : parseInt(v[4]), + accelgyro : [ + parseFloat(v[5]), parseFloat(v[6]), parseFloat(v[7]), parseFloat(v[8]), parseFloat(v[9]), + parseFloat(v[10]) + ], + tracker : v[1] + }; + + if (objs[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(0, 0, 0)); + + var line = new THREE.Line(downAxes[obj.tracker], new THREE.LineBasicMaterial({color : 0xffffff})); + 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; + } } + + } +}; + +function add_survive_log_handler(name, entry) { survive_log_handlers[name] = entry; } +function process_survive_handlers(msg) { + var s = msg.split(' '); + + if (survive_log_handlers[s[2]]) { + survive_log_handlers[s[2]](s); + } + return {}; } -var oldPoseTime = 0, poseCnt = 0; + +var survive_ws; + +// Dial up the websocket $(function() { setTimeout(function() { - var ws; - if (window.location.protocol === "file:") { - ws = new WebSocket("ws://localhost:8080/ws"); + var url = new URL(window.location.href); + var remote = url.searchParams.get("remote"); + + if (remote && remote.length) { + survive_ws = new WebSocket("ws://" + remote + "/ws"); + } else if (window.location.protocol === "file:") { + survive_ws = new WebSocket("ws://localhost:8080/ws"); } else { - ws = new WebSocket(((window.location.protocol === "https:") ? "wss://" : "ws://") + window.location.host + - "/ws"); + survive_ws = new WebSocket(((window.location.protocol === "https:") ? "wss://" : "ws://") + + window.location.host + "/ws"); } - ws.onopen = function(evt) { - // ws.send("!"); - }; - ws.onmessage = function(evt) { + survive_ws.onmessage = function(evt) { var msg = evt.data; - var obj; - if (msg[0] == "{") - obj = JSON.parse(msg); - else - obj = parseLine(msg); - - // console.log(obj); - if (obj.type === "pose") { - if (objs[obj.tracker]) { - var now = new Date().getTime(); - if(oldPoseTime + 5000 < now) { - oldPoseTime = now; - console.log( (poseCnt / 5) + "hz"); - poseCnt = 0; - } - poseCnt++; - objs[obj.tracker].position.set(obj.position[0], obj.position[1], obj.position[2]); - objs[obj.tracker].quaternion.set(obj.quat[1], obj.quat[2], obj.quat[3], obj.quat[0]); - objs[obj.tracker].verticesNeedUpdate = true; - timecode[obj.tracker] = obj.timecode; - } - } else if (obj.type === "info") { - var consoleDiv = $("#console"); - consoleDiv.append(obj.msg + "</br>"); - consoleDiv[0].scrollTop = consoleDiv[0].scrollHeight; - } else if (obj.type === "lighthouse_pose") { - add_lighthouse(obj.lighthouse, obj.position, obj.quat); - } else if (obj.type === "htc_config") { - create_object(obj); - } else if (obj.type === "imu") { - if (objs[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(0, 0, 0)); - - var line = - new THREE.Line(downAxes[obj.tracker], new THREE.LineBasicMaterial({color : 0xffffff})); - 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; - } - } - - } else if (obj.type === "angle") { - angles[obj.tracker] = angles[obj.tracker] || {}; - angles[obj.tracker][obj.lighthouse] = angles[obj.tracker][obj.lighthouse] || {}; - angles[obj.tracker][obj.lighthouse][obj.sensor_id] = - angles[obj.tracker][obj.lighthouse][obj.sensor_id] || {}; - - angles[obj.tracker][obj.lighthouse][obj.sensor_id][obj.acode & 1] = [ obj.angle, obj.timecode ]; - timecode[obj.tracker] = obj.timecode; - } - - // ws.send("!"); + process_survive_handlers(msg); }; + }, 60); // Hacky, but this gives the server time to restart on CTRL+R }); -// standard global variables -var container, scene, camera, renderer, controls; - -// custom global variables +// Init and start the render loop $(function() { // initialization init(); @@ -367,7 +395,7 @@ init() { renderer.setSize(SCREEN_WIDTH, SCREEN_HEIGHT); // attach div element to variable to contain the renderer - container = document.getElementById('ThreeJS'); + var container = document.getElementById('ThreeJS'); // attach renderer to the container div container.appendChild(renderer.domElement); @@ -375,17 +403,17 @@ init() { // move mouse and: left click to rotate, // middle click to zoom, // right click to pan - controls = new THREE.OrbitControls(camera, renderer.domElement); + var controls = new THREE.OrbitControls(camera, renderer.domElement); // create a light var light = new THREE.PointLight(0xffffff); - light.position.set(0, 5, 0); + light.position.set(0, 0, 5); scene.add(light); var floorMaterial = new THREE.MeshBasicMaterial({color : 0x000000, opacity : 0.15, transparent : true, side : THREE.FrontSide}); var floorGeometry = new THREE.PlaneGeometry(10, 10); - var floor = new THREE.Mesh(floorGeometry, floorMaterial); + floor = new THREE.Mesh(floorGeometry, floorMaterial); floor.position.z = -1; scene.add(floor); |