aboutsummaryrefslogtreecommitdiff
path: root/tools
diff options
context:
space:
mode:
authorJustin Berger <j.david.berger@gmail.com>2018-04-02 10:10:33 -0600
committerJustin Berger <j.david.berger@gmail.com>2018-04-02 10:10:33 -0600
commit75460f240c9d003e4ca2e6dda9b2146a74df7ffa (patch)
tree957b26f0539df176b61ad2ec72fbb0658b147919 /tools
parent2b63278497130d01b1fbc7e6a94b6ad8e32ab4dd (diff)
parent1724abef15a4090640bd82ba408681438316de7e (diff)
downloadlibsurvive-75460f240c9d003e4ca2e6dda9b2146a74df7ffa.tar.gz
libsurvive-75460f240c9d003e4ca2e6dda9b2146a74df7ffa.tar.bz2
Merge remote-tracking branch 'origin/master' into imu
Diffstat (limited to 'tools')
-rw-r--r--tools/findoptimalconfig/Makefile16
-rw-r--r--tools/findoptimalconfig/findoptimalconfig.cc380
-rw-r--r--tools/lightlengthparams/Makefile15
-rw-r--r--tools/lightlengthparams/lightlengthparams.c112
-rw-r--r--tools/showreproject/Makefile16
-rw-r--r--tools/showreproject/showreproject.cc271
-rw-r--r--tools/viz/index.html2
-rw-r--r--tools/viz/survive_viewer.js320
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);