From d3155686ea518b1c5335c5b61bd57131423d64fc Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 23 Mar 2018 11:26:59 -0600 Subject: Refactor viz tool to be able to use it as a library --- tools/viz/survive_viewer.js | 275 +++++++++++++++++++++----------------------- 1 file changed, 134 insertions(+), 141 deletions(-) (limited to 'tools') diff --git a/tools/viz/survive_viewer.js b/tools/viz/survive_viewer.js index 81de2a0..0710c68 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) { @@ -157,7 +158,7 @@ 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 = []; @@ -184,149 +185,141 @@ function create_object(info) { scene.add(group); } -var timecode = {}; +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]) ] + }; -function parseLine(msg) { - var s = msg.split(' '); + 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]) ] + }; - 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]) ] + 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; + timecode[obj.tracker] = obj.timecode; + } + }, + "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 + "
"); + 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; } - }, - '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; + + } +}; + +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.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 + "
"); - 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 +360,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 +368,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); -- cgit v1.3.1 From 5339216719825ba8aac0ae6507014d4a8115890f Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 24 Mar 2018 01:09:42 -0600 Subject: Viz with chemtrails --- tools/viz/index.html | 1 + tools/viz/survive_viewer.js | 31 +++++++++++++++++++++++++++++++ 2 files changed, 32 insertions(+) (limited to 'tools') diff --git a/tools/viz/index.html b/tools/viz/index.html index b146b5d..0221b41 100644 --- a/tools/viz/index.html +++ b/tools/viz/index.html @@ -12,6 +12,7 @@
+ Trails
diff --git a/tools/viz/survive_viewer.js b/tools/viz/survive_viewer.js index 0710c68..70aecfa 100644 --- a/tools/viz/survive_viewer.js +++ b/tools/viz/survive_viewer.js @@ -185,6 +185,29 @@ function create_tracked_object(info) { scene.add(group); } +var trails; +var MAX_LINE_POINTS = 1000; +$(function() { + $("#trails").change(function() { + if (this.checked) { + var geometry = new THREE.Geometry(); + var material = new THREE.LineBasicMaterial({color : 0x305ea8}); + + 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 = { @@ -214,6 +237,14 @@ var survive_log_handlers = { 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; + + 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; + } } }, "CONFIG" : function(v, tracker) { -- cgit v1.3.1 From 2dfd1ed1027e94b73048299b0cb8300694832a7c Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 25 Mar 2018 12:38:48 -0600 Subject: Added normals to viz --- tools/viz/survive_viewer.js | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) (limited to 'tools') diff --git a/tools/viz/survive_viewer.js b/tools/viz/survive_viewer.js index 70aecfa..df10ec3 100644 --- a/tools/viz/survive_viewer.js +++ b/tools/viz/survive_viewer.js @@ -164,16 +164,20 @@ function create_tracked_object(info) { 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 p = info.config.lighthouse_config.modelPoints[idx]; + var pn = info.config.lighthouse_config.modelNormals[idx]; + var color = idx / info.config.lighthouse_config.modelPoints * 0xFFFFFF; + if (idx === 6) 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 == 6 ? 0xFF0000 : 0x00FF00})); + group.add(normal); group.sensors[idx] = sensorMaterial; group.add(newSensor); } @@ -296,7 +300,7 @@ var survive_log_handlers = { 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})); + var line = new THREE.Line(downAxes[obj.tracker], new THREE.LineBasicMaterial({color : 0xffffff})); scene.add(line); } -- cgit v1.3.1 From 93ce31bf11cb22c5a09b5af04a76c7b7a6b912fd Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 25 Mar 2018 21:28:18 -0600 Subject: Made it so viz tool didn't require remote arg --- tools/viz/survive_viewer.js | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) (limited to 'tools') diff --git a/tools/viz/survive_viewer.js b/tools/viz/survive_viewer.js index df10ec3..67e65f0 100644 --- a/tools/viz/survive_viewer.js +++ b/tools/viz/survive_viewer.js @@ -167,7 +167,7 @@ function create_tracked_object(info) { var p = info.config.lighthouse_config.modelPoints[idx]; var pn = info.config.lighthouse_config.modelNormals[idx]; var color = idx / info.config.lighthouse_config.modelPoints * 0xFFFFFF; - if (idx === 6) + if (idx === 0) color = 0x00ff00; var sensorMaterial = new THREE.MeshBasicMaterial({color : color}); var newSensor = new THREE.Mesh(sensorGeometry, sensorMaterial); @@ -176,7 +176,7 @@ function create_tracked_object(info) { 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 == 6 ? 0xFF0000 : 0x00FF00})); + var normal= new THREE.Line(normalGeom, new THREE.LineBasicMaterial({color : idx == 4 ? 0xFF0000 : 0x00FF00})); group.add(normal); group.sensors[idx] = sensorMaterial; group.add(newSensor); @@ -240,7 +240,6 @@ var survive_log_handlers = { 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; if (trails) { @@ -337,7 +336,7 @@ $(function() { var url = new URL(window.location.href); var remote = url.searchParams.get("remote"); - if (remote.length) { + 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"); -- cgit v1.3.1 From d9ecb4d321bfa04b5d67fb501be0cd9c46140775 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 25 Mar 2018 21:29:32 -0600 Subject: Added tool to calculate various things about length of received pulses --- include/libsurvive/survive.h | 1 + src/survive_sensor_activations.c | 4 +- tools/lightlengthparams/Makefile | 15 ++++ tools/lightlengthparams/lightlengthparams.c | 112 ++++++++++++++++++++++++++++ 4 files changed, 131 insertions(+), 1 deletion(-) create mode 100644 tools/lightlengthparams/Makefile create mode 100644 tools/lightlengthparams/lightlengthparams.c (limited to 'tools') diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index b1c32cd..8a1474b 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -16,6 +16,7 @@ extern "C" { typedef struct { FLT angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; // 2 Axes (Angles in LH space) uint32_t timecode[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; // Timecode per axis in ticks + uint32_t lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; // Timecode per axis in ticks FLT accel[3]; FLT gyro[3]; diff --git a/src/survive_sensor_activations.c b/src/survive_sensor_activations.c index 4d1801c..e42b50e 100644 --- a/src/survive_sensor_activations.c +++ b/src/survive_sensor_activations.c @@ -21,9 +21,11 @@ void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDa int axis = (lightData->acode & 1); uint32_t *data_timecode = &self->timecode[lightData->sensor_id][lightData->lh][axis]; FLT *angle = &self->angles[lightData->sensor_id][lightData->lh][axis]; + uint32_t *length = &self->lengths[lightData->sensor_id][lightData->lh][axis]; *angle = lightData->angle; *data_timecode = lightData->timecode; + *length = lightData->length * 48000000; } -uint32_t SurviveSensorActivations_default_tolerance = (uint32_t)(48000000 /*mhz*/ * (16.7 * 2 /*ms*/) / 1000); \ No newline at end of file +uint32_t SurviveSensorActivations_default_tolerance = (uint32_t)(48000000 /*mhz*/ * (16.7 * 2 /*ms*/) / 1000); 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 +#include +#include +#include +#include + +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; +} -- cgit v1.3.1 From 1ca9196f93a4d9fa09736112c880634392536d8e Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 28 Mar 2018 00:11:04 -0600 Subject: Bigger chemtrails; colors on HMD trackers --- tools/viz/survive_viewer.js | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'tools') diff --git a/tools/viz/survive_viewer.js b/tools/viz/survive_viewer.js index 67e65f0..d7f80ad 100644 --- a/tools/viz/survive_viewer.js +++ b/tools/viz/survive_viewer.js @@ -130,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]) @@ -190,7 +190,7 @@ function create_tracked_object(info) { } var trails; -var MAX_LINE_POINTS = 1000; +var MAX_LINE_POINTS = 100000; $(function() { $("#trails").change(function() { if (this.checked) { -- cgit v1.3.1 From 3c4e37a277745cb22e7a4b1b7337f64fd653c205 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 28 Mar 2018 10:35:23 -0600 Subject: Prettier word wrap --- tools/viz/index.html | 1 + 1 file changed, 1 insertion(+) (limited to 'tools') diff --git a/tools/viz/index.html b/tools/viz/index.html index 0221b41..2987555 100644 --- a/tools/viz/index.html +++ b/tools/viz/index.html @@ -26,6 +26,7 @@ position: absolute; bottom: 20px; overflow: auto; + white-space: pre-wrap; background-color: rgba(0,200,200, .25); z-index: 3;"> -- cgit v1.3.1 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/Makefile | 15 ++ tools/findoptimalconfig/findoptimalconfig.cc | 332 +++++++++++++++++++++++++++ 2 files changed, 347 insertions(+) create mode 100644 tools/findoptimalconfig/Makefile create mode 100644 tools/findoptimalconfig/findoptimalconfig.cc (limited to 'tools') diff --git a/tools/findoptimalconfig/Makefile b/tools/findoptimalconfig/Makefile new file mode 100644 index 0000000..5e14814 --- /dev/null +++ b/tools/findoptimalconfig/Makefile @@ -0,0 +1,15 @@ +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) + 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..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.3.1 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 --- include/libsurvive/survive.h | 308 ++++++++++++++------------- include/libsurvive/survive_reproject.h | 35 +-- src/poser_epnp.c | 10 +- src/poser_sba.c | 30 ++- src/survive.c | 2 + src/survive_cal.c | 26 +-- src/survive_config.c | 22 +- src/survive_process.c | 9 +- src/survive_reproject.c | 129 ++++++++--- src/survive_sensor_activations.c | 3 +- tools/findoptimalconfig/Makefile | 3 +- tools/findoptimalconfig/findoptimalconfig.cc | 160 ++++++++++++-- 12 files changed, 496 insertions(+), 241 deletions(-) (limited to 'tools') diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index ed43ddc..e435752 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -10,11 +10,10 @@ extern "C" { #endif - #ifdef _WIN32 #define SURVIVE_EXPORT __declspec(dllexport) #else -#define SURVIVE_EXPORT +#define SURVIVE_EXPORT __attribute__((visibility("default"))) #endif /** @@ -32,6 +31,7 @@ typedef struct { struct PoserDataLight; struct PoserDataIMU; + /** * Adds a lightData packet to the table. */ @@ -53,69 +53,68 @@ bool SurviveSensorActivations_isPairValid(const SurviveSensorActivations *self, */ extern uint32_t SurviveSensorActivations_default_tolerance; -//DANGER: This structure may be redefined. Note that it is logically split into 64-bit chunks -//for optimization on 32- and 64-bit systems. +// DANGER: This structure may be redefined. Note that it is logically split into 64-bit chunks +// for optimization on 32- and 64-bit systems. -struct SurviveObject -{ - SurviveContext * ctx; +struct SurviveObject { + SurviveContext *ctx; - char codename[4]; //3 letters, null-terminated. Currently HMD, WM0, WM1. - char drivername[4]; //3 letters for driver. Currently "HTC" - void *driver; + char codename[4]; // 3 letters, null-terminated. Currently HMD, WM0, WM1. + char drivername[4]; // 3 letters for driver. Currently "HTC" + void *driver; int32_t buttonmask; int16_t axis1; int16_t axis2; int16_t axis3; - int8_t charge; - int8_t charging:1; - int8_t ison:1; - int8_t additional_flags:6; - - //Pose Information, also "poser" field. - FLT PoseConfidence; //0..1 - SurvivePose OutPose; //Final pose? (some day, one can dream!) - SurvivePose FromLHPose[NUM_LIGHTHOUSES]; //Filled out by poser, contains computed position from each lighthouse. - void * PoserData; //Initialized to zero, configured by poser, can be anything the poser wants. + int8_t charge; + int8_t charging : 1; + int8_t ison : 1; + int8_t additional_flags : 6; + + // Pose Information, also "poser" field. + FLT PoseConfidence; // 0..1 + SurvivePose OutPose; // Final pose? (some day, one can dream!) + SurvivePose FromLHPose[NUM_LIGHTHOUSES]; // Filled out by poser, contains computed position from each lighthouse. + void *PoserData; // Initialized to zero, configured by poser, can be anything the poser wants. PoserCB PoserFn; - //Device-specific information about the location of the sensors. This data will be used by the poser. - int8_t sensor_ct; // sensor count - FLT * sensor_locations; // size is sensor_ct*3. Contains x,y,z values for each sensor - FLT * sensor_normals;// size is nrlocations*3. cointains normal vector for each sensor - - //Timing sensitive data (mostly for disambiguation) - int32_t timebase_hz; //48,000,000 for normal vive hardware. (checked) - int32_t timecenter_ticks; //200,000 for normal vive hardware. (checked) (This doubles-up as 2x this = full sweep length) - int32_t pulsedist_max_ticks; //500,000 for normal vive hardware. (guessed) - int32_t pulselength_min_sync; //2,200 for normal vive hardware. (guessed) - int32_t pulse_in_clear_time; //35,000 for normal vive hardware. (guessed) - int32_t pulse_max_for_sweep; //1,800 for normal vive hardware. (guessed) - int32_t pulse_synctime_offset; //20,000 for normal vive hardware. (guessed) - int32_t pulse_synctime_slack; //5,000 for normal vive hardware. (guessed) - - //Flood info, for calculating which laser is currently sweeping. - void * disambiguator_data; - int8_t oldcode; - int8_t sync_set_number; //0 = master, 1 = slave, -1 = fault. - int8_t did_handle_ootx; //If unset, will send lightcap data for sync pulses next time a sensor is hit. + // Device-specific information about the location of the sensors. This data will be used by the poser. + int8_t sensor_ct; // sensor count + FLT *sensor_locations; // size is sensor_ct*3. Contains x,y,z values for each sensor + FLT *sensor_normals; // size is nrlocations*3. cointains normal vector for each sensor + + // Timing sensitive data (mostly for disambiguation) + int32_t timebase_hz; // 48,000,000 for normal vive hardware. (checked) + int32_t timecenter_ticks; // 200,000 for normal vive hardware. (checked) (This doubles-up as 2x this = full + // sweep length) + int32_t pulsedist_max_ticks; // 500,000 for normal vive hardware. (guessed) + int32_t pulselength_min_sync; // 2,200 for normal vive hardware. (guessed) + int32_t pulse_in_clear_time; // 35,000 for normal vive hardware. (guessed) + int32_t pulse_max_for_sweep; // 1,800 for normal vive hardware. (guessed) + int32_t pulse_synctime_offset; // 20,000 for normal vive hardware. (guessed) + int32_t pulse_synctime_slack; // 5,000 for normal vive hardware. (guessed) + + // Flood info, for calculating which laser is currently sweeping. + void *disambiguator_data; + int8_t oldcode; + int8_t sync_set_number; // 0 = master, 1 = slave, -1 = fault. + int8_t did_handle_ootx; // If unset, will send lightcap data for sync pulses next time a sensor is hit. uint32_t last_sync_time[NUM_LIGHTHOUSES]; uint32_t last_sync_length[NUM_LIGHTHOUSES]; uint32_t recent_sync_time; - uint32_t last_lighttime; //May be a 24- or 32- bit number depending on what device. - + uint32_t last_lighttime; // May be a 24- or 32- bit number depending on what device. - FLT* acc_bias; // size is FLT*3. contains x,y,z - FLT* acc_scale; // size is FLT*3. contains x,y,z - FLT* gyro_bias; // size is FLT*3. contains x,y,z - FLT* gyro_scale; // size is FLT*3. contains x,y,z + FLT *acc_bias; // size is FLT*3. contains x,y,z + FLT *acc_scale; // size is FLT*3. contains x,y,z + FLT *gyro_bias; // size is FLT*3. contains x,y,z + FLT *gyro_scale; // size is FLT*3. contains x,y,z haptic_func haptic; SurviveSensorActivations activations; - //Debug + // Debug int tsl; }; @@ -125,34 +124,37 @@ SURVIVE_EXPORT int8_t survive_object_sensor_ct(SurviveObject *so); SURVIVE_EXPORT const FLT *survive_object_sensor_locations(SurviveObject *so); SURVIVE_EXPORT const FLT *survive_object_sensor_normals(SurviveObject *so); -struct BaseStationData -{ - uint8_t PositionSet:1; +typedef struct BaseStationCal { + FLT phase[2]; + FLT tilt[2]; + FLT curve[2]; + FLT gibpha[2]; + FLT gibmag[2]; +} BaseStationCal; + +struct BaseStationData { + uint8_t PositionSet : 1; SurvivePose Pose; - uint8_t OOTXSet:1; + uint8_t OOTXSet : 1; uint32_t BaseStationID; - FLT fcalphase[2]; - FLT fcaltilt[2]; - FLT fcalcurve[2]; - FLT fcalgibpha[2]; - FLT fcalgibmag[2]; - int8_t accel[3]; //"Up" vector + + BaseStationCal fcal; + + int8_t accel[3]; //"Up" vector + uint8_t mode; }; struct config_group; #define BUTTON_QUEUE_MAX_LEN 32 - - // note: buttonId and axisId are 1-indexed values. // a value of 0 for an id means that no data is present in that value // additionally, when x and y values are both present in axis data, // axis1 will be x, axis2 will be y. -typedef struct -{ - uint8_t isPopulated; //probably can remove this given the semaphore in the parent struct. helps with debugging +typedef struct { + uint8_t isPopulated; // probably can remove this given the semaphore in the parent struct. helps with debugging uint8_t eventType; uint8_t buttonId; uint8_t axis1Id; @@ -162,20 +164,28 @@ typedef struct SurviveObject *so; } ButtonQueueEntry; -typedef struct -{ - uint8_t nextReadIndex; //init to 0 +typedef struct { + uint8_t nextReadIndex; // init to 0 uint8_t nextWriteIndex; // init to 0 - void* buttonservicesem; + void *buttonservicesem; ButtonQueueEntry entry[BUTTON_QUEUE_MAX_LEN]; } ButtonQueue; typedef enum { SURVIVE_STOPPED = 0, SURVIVE_RUNNING, SURVIVE_CLOSING, SURVIVE_STATE_MAX } SurviveState; struct SurviveRecordingData; +struct survive_calibration_config; + +enum SurviveCalFlag { + SVCal_None = 0, + SVCal_Phase = 1, + SVCal_Tilt = 2, + SVCal_Curve = 4, + SVCal_Gib = 8, + SVCal_All = SVCal_Gib | SVCal_Curve | SVCal_Tilt | SVCal_Phase +}; -struct SurviveContext -{ +struct SurviveContext { text_feedback_func faultfunction; text_feedback_func notefunction; light_process_func lightproc; @@ -187,35 +197,39 @@ struct SurviveContext htc_config_func configfunction; handle_lightcap_func lightcapfunction; - struct config_group* global_config_values; - struct config_group* lh_config; //lighthouse configs - struct config_group* temporary_config_values; //Set per-session, from command-line. Not saved but override global_config_values + struct config_group *global_config_values; + struct config_group *lh_config; // lighthouse configs + struct config_group + *temporary_config_values; // Set per-session, from command-line. Not saved but override global_config_values - //Calibration data: + // Calibration data: int activeLighthouses; BaseStationData bsd[NUM_LIGHTHOUSES]; - SurviveCalData * calptr; //If and only if the calibration subsystem is attached. + SurviveCalData *calptr; // If and only if the calibration subsystem is attached. struct SurviveRecordingData *recptr; // Iff recording is attached - SurviveObject ** objs; + SurviveObject **objs; int objs_ct; - void ** drivers; - DeviceDriverCb * driverpolls; - DeviceDriverCb * drivercloses; - DeviceDriverMagicCb * drivermagics; + void **drivers; + DeviceDriverCb *driverpolls; + DeviceDriverCb *drivercloses; + DeviceDriverMagicCb *drivermagics; int driver_ct; SurviveState state; - void* buttonservicethread; + void *buttonservicethread; ButtonQueue buttonQueue; void *user_ptr; + enum SurviveCalFlag calibration_flag; + struct survive_calibration_config *calibration_config; }; -void survive_verify_FLT_size(uint32_t user_size); // Baked in size of FLT to verify users of the library have the correct setting. +void survive_verify_FLT_size( + uint32_t user_size); // Baked in size of FLT to verify users of the library have the correct setting. -SURVIVE_EXPORT SurviveContext * survive_init_internal( int argc, char * const * argv ); +SURVIVE_EXPORT SurviveContext *survive_init_internal(int argc, char *const *argv); /** * Call survive_init to get a populated SurviveContext pointer. @@ -227,98 +241,106 @@ SURVIVE_EXPORT SurviveContext * survive_init_internal( int argc, char * const * * Note that this function _can_ return null based on command line arguments, * notably if -h was passed in. */ -static inline SurviveContext * survive_init( int argc, char * const * argv ) -{ +static inline SurviveContext *survive_init(int argc, char *const *argv) { survive_verify_FLT_size(sizeof(FLT)); return survive_init_internal(argc, argv); } - -//For any of these, you may pass in 0 for the function pointer to use default behavior. -//In general unless you are doing wacky things like recording or playing back data, you won't need to use this. -SURVIVE_EXPORT void survive_install_htc_config_fn( SurviveContext *ctx, htc_config_func fbp ); -SURVIVE_EXPORT void survive_install_info_fn( SurviveContext * ctx, text_feedback_func fbp ); -SURVIVE_EXPORT void survive_install_error_fn( SurviveContext * ctx, text_feedback_func fbp ); -SURVIVE_EXPORT void survive_install_light_fn( SurviveContext * ctx, light_process_func fbp ); -SURVIVE_EXPORT void survive_install_imu_fn( SurviveContext * ctx, imu_process_func fbp ); -SURVIVE_EXPORT void survive_install_angle_fn( SurviveContext * ctx, angle_process_func fbp ); -SURVIVE_EXPORT void survive_install_button_fn(SurviveContext * ctx, button_process_func fbp); -SURVIVE_EXPORT void survive_install_raw_pose_fn(SurviveContext * ctx, raw_pose_func fbp); +// For any of these, you may pass in 0 for the function pointer to use default behavior. +// In general unless you are doing wacky things like recording or playing back data, you won't need to use this. +SURVIVE_EXPORT void survive_install_htc_config_fn(SurviveContext *ctx, htc_config_func fbp); +SURVIVE_EXPORT void survive_install_info_fn(SurviveContext *ctx, text_feedback_func fbp); +SURVIVE_EXPORT void survive_install_error_fn(SurviveContext *ctx, text_feedback_func fbp); +SURVIVE_EXPORT void survive_install_light_fn(SurviveContext *ctx, light_process_func fbp); +SURVIVE_EXPORT void survive_install_imu_fn(SurviveContext *ctx, imu_process_func fbp); +SURVIVE_EXPORT void survive_install_angle_fn(SurviveContext *ctx, angle_process_func fbp); +SURVIVE_EXPORT void survive_install_button_fn(SurviveContext *ctx, button_process_func fbp); +SURVIVE_EXPORT void survive_install_raw_pose_fn(SurviveContext *ctx, raw_pose_func fbp); SURVIVE_EXPORT void survive_install_lighthouse_pose_fn(SurviveContext *ctx, lighthouse_pose_func fbp); -SURVIVE_EXPORT int survive_startup( SurviveContext * ctx ); -SURVIVE_EXPORT int survive_poll( SurviveContext * ctx ); -SURVIVE_EXPORT void survive_close( SurviveContext * ctx ); - -SURVIVE_EXPORT SurviveObject * survive_get_so_by_name( SurviveContext * ctx, const char * name ); +SURVIVE_EXPORT int survive_startup(SurviveContext *ctx); +SURVIVE_EXPORT int survive_poll(SurviveContext *ctx); +SURVIVE_EXPORT void survive_close(SurviveContext *ctx); -//Utilitiy functions. -int survive_simple_inflate( SurviveContext * ctx, const char * input, int inlen, char * output, int outlen ); -int survive_send_magic( SurviveContext * ctx, int magic_code, void * data, int datalen ); +SURVIVE_EXPORT SurviveObject *survive_get_so_by_name(SurviveContext *ctx, const char *name); -//These functions search both the stored-general and temporary sections for a parameter and return it. -#define SC_GET 0 //Get, only. -#define SC_SET 1 //Set, if not present -#define SC_OVERRIDE 2 //Set, to new default value. -#define SC_SETCONFIG 4 //Set, both in-memory and config file. Use in conjunction with SC_OVERRIDE. +// Utilitiy functions. +int survive_simple_inflate(SurviveContext *ctx, const char *input, int inlen, char *output, int outlen); +int survive_send_magic(SurviveContext *ctx, int magic_code, void *data, int datalen); -SURVIVE_EXPORT FLT survive_configf( SurviveContext * ctx, const char *tag, char flags, FLT def ); -SURVIVE_EXPORT uint32_t survive_configi( SurviveContext * ctx, const char *tag, char flags, uint32_t def ); -SURVIVE_EXPORT const char * survive_configs( SurviveContext * ctx, const char *tag, char flags, const char *def ); +// These functions search both the stored-general and temporary sections for a parameter and return it. +#define SC_GET 0 // Get, only. +#define SC_SET 1 // Set, if not present +#define SC_OVERRIDE 2 // Set, to new default value. +#define SC_SETCONFIG 4 // Set, both in-memory and config file. Use in conjunction with SC_OVERRIDE. +SURVIVE_EXPORT FLT survive_configf(SurviveContext *ctx, const char *tag, char flags, FLT def); +SURVIVE_EXPORT uint32_t survive_configi(SurviveContext *ctx, const char *tag, char flags, uint32_t def); +SURVIVE_EXPORT const char *survive_configs(SurviveContext *ctx, const char *tag, char flags, const char *def); -//Install the calibrator. -SURVIVE_EXPORT void survive_cal_install( SurviveContext * ctx ); //XXX This will be removed if not already done so. +// Install the calibrator. +SURVIVE_EXPORT void survive_cal_install(SurviveContext *ctx); // XXX This will be removed if not already done so. // Read back a human-readable string description of the calibration status -SURVIVE_EXPORT int survive_cal_get_status( SurviveContext * ctx, char * description, int description_length ); +SURVIVE_EXPORT int survive_cal_get_status(SurviveContext *ctx, char *description, int description_length); // Induce haptic feedback -SURVIVE_EXPORT int survive_haptic(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh, uint16_t pulseLow, uint16_t repeatCount); - -//Call these from your callback if overridden. -//Accept higher-level data. -SURVIVE_EXPORT void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , uint32_t lh); -SURVIVE_EXPORT void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id ); -SURVIVE_EXPORT void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ); -SURVIVE_EXPORT void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); +SURVIVE_EXPORT int survive_haptic(SurviveObject *so, uint8_t reserved, uint16_t pulseHigh, uint16_t pulseLow, + uint16_t repeatCount); + +// Call these from your callback if overridden. +// Accept higher-level data. +SURVIVE_EXPORT void survive_default_light_process(SurviveObject *so, int sensor_id, int acode, int timeinsweep, + uint32_t timecode, uint32_t length, uint32_t lh); +SURVIVE_EXPORT void survive_default_imu_process(SurviveObject *so, int mode, FLT *accelgyro, uint32_t timecode, int id); +SURVIVE_EXPORT void survive_default_angle_process(SurviveObject *so, int sensor_id, int acode, uint32_t timecode, + FLT length, FLT angle, uint32_t lh); +SURVIVE_EXPORT void survive_default_button_process(SurviveObject *so, uint8_t eventType, uint8_t buttonId, + uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, + uint16_t axis2Val); SURVIVE_EXPORT void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose); -SURVIVE_EXPORT void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lh_pose, - SurvivePose *obj_pose); -SURVIVE_EXPORT int survive_default_htc_config_process(SurviveObject *so, char *ct0conf, int len); - - +SURVIVE_EXPORT void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, + SurvivePose *lh_pose, SurvivePose *obj_pose); +SURVIVE_EXPORT int survive_default_htc_config_process(SurviveObject *so, char *ct0conf, int len); ////////////////////// Survive Drivers //////////////////////////// -void RegisterDriver(const char * name, void * data); +void RegisterDriver(const char *name, void *data); #ifdef _MSC_VER -#define REGISTER_LINKTIME( func ) \ - __pragma(comment(linker,"/export:REGISTER"#func));\ +#define REGISTER_LINKTIME(func) \ + __pragma(comment(linker, "/export:REGISTER" #func)); \ void REGISTER##func() { RegisterDriver(#func, &func); } #else -#define REGISTER_LINKTIME( func ) \ +#define REGISTER_LINKTIME(func) \ void __attribute__((constructor)) REGISTER##func() { RegisterDriver(#func, &func); } #endif - - ///////////////////////// General stuff for writing drivers /////// -//For device drivers to call. This actually attaches them. -int survive_add_object( SurviveContext * ctx, SurviveObject * obj ); -void survive_add_driver( SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic ); - -//This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. -void handle_lightcap( SurviveObject * so, LightcapElement * le ); - -#define SV_INFO( ... ) { char stbuff[1024]; sprintf( stbuff, __VA_ARGS__ ); ctx->notefunction( ctx, stbuff ); } -#define SV_ERROR( ... ) { char stbuff[1024]; sprintf( stbuff, __VA_ARGS__ ); ctx->faultfunction( ctx, stbuff ); } -#define SV_KILL() exit(0) //XXX This should likely be re-defined. +// For device drivers to call. This actually attaches them. +int survive_add_object(SurviveContext *ctx, SurviveObject *obj); +void survive_add_driver(SurviveContext *ctx, void *payload, DeviceDriverCb poll, DeviceDriverCb close, + DeviceDriverMagicCb magic); + +// This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. +void handle_lightcap(SurviveObject *so, LightcapElement *le); + +#define SV_INFO(...) \ + { \ + char stbuff[1024]; \ + sprintf(stbuff, __VA_ARGS__); \ + ctx->notefunction(ctx, stbuff); \ + } +#define SV_ERROR(...) \ + { \ + char stbuff[1024]; \ + sprintf(stbuff, __VA_ARGS__); \ + ctx->faultfunction(ctx, stbuff); \ + } +#define SV_KILL() exit(0) // XXX This should likely be re-defined. #ifdef __cplusplus }; #endif #endif - diff --git a/include/libsurvive/survive_reproject.h b/include/libsurvive/survive_reproject.h index 961db2a..c90d39c 100644 --- a/include/libsurvive/survive_reproject.h +++ b/include/libsurvive/survive_reproject.h @@ -1,6 +1,8 @@ #pragma once + #include "survive.h" #include +#include #include #ifdef __cplusplus @@ -13,7 +15,7 @@ typedef struct { bool swap; } survive_calibration_options_config; -typedef struct { +typedef struct survive_calibration_config { survive_calibration_options_config phase, tilt, curve, gibMag, gibPhase; @@ -21,10 +23,10 @@ typedef struct { } survive_calibration_config; -void survive_calibration_options_config_apply( - const survive_calibration_options_config *option, const FLT *input, - FLT *output); -const survive_calibration_config *survive_calibration_default_config(); +void survive_calibration_options_config_apply(const survive_calibration_options_config *option, const FLT *input, + FLT *output); + +const survive_calibration_config *survive_calibration_default_config(const SurviveContext *ctx); size_t survive_calibration_config_max_idx(); @@ -32,10 +34,9 @@ survive_calibration_config survive_calibration_config_create_from_idx(size_t v); size_t survive_calibration_config_index(const survive_calibration_config *config); -void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, - FLT *out); -void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, - const SurvivePose *pose, FLT *point3d, +void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, FLT *out); + +void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, const SurvivePose *pose, FLT *point3d, FLT *out); // This is given a lighthouse -- in the same system as stored in BaseStationData, and @@ -44,9 +45,19 @@ void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, // While this is typically opposite of what we want to do -- we want to find the 3d // position from a 2D coordinate, this is helpful since the minimization of reprojection // error is a core mechanism to many types of solvers. -void survive_reproject_from_pose_with_config( - const SurviveContext *ctx, const survive_calibration_config *config, - int lighthouse, const SurvivePose *pose, const FLT *point3d, FLT *out); +void survive_reproject_from_pose_with_config(const SurviveContext *ctx, const survive_calibration_config *config, + int lighthouse, const SurvivePose *pose, const FLT *point3d, FLT *out); + +void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const survive_calibration_config *config, + const SurvivePose *pose, const FLT *point3d, FLT *out); + +void survive_reproject_with_config(const SurviveContext *ctx, const survive_calibration_config *config, int lighthouse, + const FLT *point3d, FLT *out); + +void survive_calibration_config_fprint(FILE *file, const survive_calibration_config *config); + +void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, enum SurviveCalFlag f, const FLT *in, FLT *out); +void survive_apply_bsd_calibration(SurviveContext *ctx, int lh, const FLT *in, FLT *out); #ifdef __cplusplus } diff --git a/src/poser_epnp.c b/src/poser_epnp.c index 7e86542..c05450a 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -6,6 +6,7 @@ #include #include +#include #include "epnp/epnp.h" #include "linmath.h" @@ -71,7 +72,9 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) for (size_t i = 0; i < so->sensor_ct; i++) { FLT *lengths = pdfs->lengths[i][lh]; - FLT *ang = pdfs->angles[i][lh]; + FLT *_ang = pdfs->angles[i][lh]; + FLT ang[2]; + survive_apply_bsd_calibration(so->ctx, lh, _ang, ang); if (lengths[0] < 0 || lengths[1] < 0) continue; @@ -103,7 +106,10 @@ static void add_correspondences(SurviveObject *so, epnp *pnp, SurviveSensorActiv for (size_t sensor_idx = 0; sensor_idx < so->sensor_ct; sensor_idx++) { if (SurviveSensorActivations_isPairValid(scene, SurviveSensorActivations_default_tolerance, timecode, sensor_idx, lh)) { - double *angles = scene->angles[sensor_idx][lh]; + FLT *_angles = scene->angles[sensor_idx][lh]; + FLT angles[2]; + survive_apply_bsd_calibration(so->ctx, lh, _angles, angles); + epnp_add_correspondence(pnp, so->sensor_locations[sensor_idx * 3 + 0], so->sensor_locations[sensor_idx * 3 + 1], so->sensor_locations[sensor_idx * 3 + 2], tan(angles[0]), tan(angles[1])); diff --git a/src/poser_sba.c b/src/poser_sba.c index df28a2d..4a4ed8f 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -40,6 +40,8 @@ typedef struct SBAData { int successes_to_reset; int successes_to_reset_cntr; + FLT max_error; + FLT sensor_variance; FLT sensor_variance_per_second; int sensor_time_window; @@ -71,7 +73,9 @@ static size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, continue; } - double *angles = pdfs->angles[sensor][lh]; + double *_angles = pdfs->angles[sensor][lh]; + double angles[2]; + survive_apply_bsd_calibration(so->ctx, lh, _angles, angles); vmask[sensor * NUM_LIGHTHOUSES + lh] = 1; meas[measCount++] = angles[0]; @@ -89,7 +93,9 @@ static size_t construct_input_from_scene(SBAData *d, PoserDataLight *pdl, Surviv for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) { for (size_t lh = 0; lh < 2; lh++) { if (SurviveSensorActivations_isPairValid(scene, d->sensor_time_window, pdl->timecode, sensor, lh)) { - double *a = scene->angles[sensor][lh]; + double *_a = scene->angles[sensor][lh]; + FLT a[2]; + survive_apply_bsd_calibration(so->ctx, lh, _a, a); vmask[sensor * NUM_LIGHTHOUSES + lh] = 1; if (cov) { @@ -273,7 +279,7 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o if (distance > 1.) status = -1; } - if (status > 0) { + if (status > 0 && (info[1] / meas_size * 2) < d->max_error) { d->failures_to_reset_cntr = d->failures_to_reset; quatnormalize(soLocation.Rot, soLocation.Rot); PoserData_poser_raw_pose_func(&pdl->hdr, so, 1, &soLocation); @@ -283,7 +289,7 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o 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 || meas_size < d->required_meas) { + if (cnt++ > 1000 || meas_size < d->required_meas || (info[1] / meas_size * 2) > d->max_error) { 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; @@ -389,11 +395,12 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { d->failures_to_reset_cntr = 0; d->failures_to_reset = survive_configi(ctx, "sba-failures-to-reset", SC_GET, 1); d->successes_to_reset_cntr = 0; - d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, 1); + d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, 100); d->required_meas = survive_configi(ctx, "sba-required-meas", SC_GET, 8); - - d->sensor_time_window = survive_configi(ctx, "sba-time-window", SC_GET, 1600000 * 4); + d->max_error = survive_configf(ctx, "sba-max-error", SC_GET, .0001); + d->sensor_time_window = + survive_configi(ctx, "sba-time-window", SC_GET, SurviveSensorActivations_default_tolerance * 2); d->sensor_variance_per_second = survive_configf(ctx, "sba-sensor-variance-per-sec", SC_GET, 0.001); d->sensor_variance = survive_configf(ctx, "sba-sensor-variance", SC_GET, 1.0); d->so = so; @@ -403,6 +410,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { SV_INFO("\tsba-sensor-variance: %f", d->sensor_variance); SV_INFO("\tsba-sensor-variance-per-sec: %f", d->sensor_variance_per_second); SV_INFO("\tsba-time-window: %d", d->sensor_time_window); + SV_INFO("\tsba-max-error: %f", d->max_error); } SBAData *d = so->PoserData; switch (pd->pt) { @@ -416,8 +424,8 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { // only process sweeps FLT error = -1; if (d->last_lh != lightData->lh || d->last_acode != lightData->acode) { - survive_calibration_config config = *survive_calibration_default_config(); - error = run_sba_find_3d_structure(d, config, lightData, scene, 50, .5); + survive_calibration_config config = *survive_calibration_default_config(ctx); + error = run_sba_find_3d_structure(d, config, lightData, scene, 100, .5); d->last_lh = lightData->lh; d->last_acode = lightData->acode; } @@ -435,9 +443,9 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { case POSERDATA_FULL_SCENE: { SurviveContext *ctx = so->ctx; PoserDataFullScene *pdfs = (PoserDataFullScene *)(pd); - survive_calibration_config config = *survive_calibration_default_config(); + survive_calibration_config config = *survive_calibration_default_config(ctx); SV_INFO("Running sba with %u", (int)survive_calibration_config_index(&config)); - double error = run_sba(config, pdfs, so, 50, .005); + double error = run_sba(config, pdfs, so, 100, .005); // std::cerr << "Average reproj error: " << error << std::endl; return 0; } diff --git a/src/survive.c b/src/survive.c index a15e0ed..63ad2ba 100644 --- a/src/survive.c +++ b/src/survive.c @@ -219,6 +219,8 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) { ctx->configfunction = survive_default_htc_config_process; ctx->rawposeproc = survive_default_raw_pose_process; + ctx->calibration_flag = (enum SurviveCalFlag)survive_configi(ctx, "bsd-cal", SC_GET, SVCal_All); + return ctx; } diff --git a/src/survive_cal.c b/src/survive_cal.c index 2fc1896..e094e7b 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -61,19 +61,20 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) //print_lighthouse_info_v6(&v6); b->BaseStationID = v6.id; - b->fcalphase[0] = v6.fcal_0_phase; - b->fcalphase[1] = v6.fcal_1_phase; - b->fcaltilt[0] = tan(v6.fcal_0_tilt); - b->fcaltilt[1] = tan(v6.fcal_1_tilt); //XXX??? Is this right? See https://github.com/cnlohr/libsurvive/issues/18 - b->fcalcurve[0] = v6.fcal_0_curve; - b->fcalcurve[1] = v6.fcal_1_curve; - b->fcalgibpha[0] = v6.fcal_0_gibphase; - b->fcalgibpha[1] = v6.fcal_1_gibphase; - b->fcalgibmag[0] = v6.fcal_0_gibmag; - b->fcalgibmag[1] = v6.fcal_1_gibmag; + b->fcal.phase[0] = v6.fcal_0_phase; + b->fcal.phase[1] = v6.fcal_1_phase; + b->fcal.tilt[0] = (v6.fcal_0_tilt); + b->fcal.tilt[1] = (v6.fcal_1_tilt); // XXX??? Is this right? See https://github.com/cnlohr/libsurvive/issues/18 + b->fcal.curve[0] = v6.fcal_0_curve; + b->fcal.curve[1] = v6.fcal_1_curve; + b->fcal.gibpha[0] = v6.fcal_0_gibphase; + b->fcal.gibpha[1] = v6.fcal_1_gibphase; + b->fcal.gibmag[0] = v6.fcal_0_gibmag; + b->fcal.gibmag[1] = v6.fcal_1_gibmag; b->accel[0] = v6.accel_dir_x; b->accel[1] = v6.accel_dir_y; b->accel[2] = v6.accel_dir_z; + b->mode = v6.mode_current; b->OOTXSet = 1; config_set_lighthouse(ctx->lh_config,b,id); @@ -617,8 +618,9 @@ static void handle_calibration( struct SurviveCalData *cd ) } fsd.lengths[i][j][0] = cd->avglens[dataindex+0]; fsd.lengths[i][j][1] = cd->avglens[dataindex+1]; - fsd.angles[i][j][0] = cd->avgsweeps[dataindex+0]; - fsd.angles[i][j][1] = cd->avgsweeps[dataindex+1]; + // fsd.angles[i][j][0] = cd->avgsweeps[dataindex+0]; + // fsd.angles[i][j][1] = cd->avgsweeps[dataindex+1]; + survive_apply_bsd_calibration(ctx, lh, &cd->avgsweeps[dataindex], fsd.angles[i][j]); fsd.synctimes[i][j] = temp_syncs[i][j]; } diff --git a/src/survive_config.c b/src/survive_config.c index 7b68ae4..d67cd8e 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -119,12 +119,13 @@ void config_read_lighthouse(config_group *lh_config, BaseStationData *bsd, uint8 FLT defaults[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; bsd->BaseStationID = config_read_uint32(cg, "id", 0); + bsd->mode = config_read_uint32(cg, "mode", 0); config_read_float_array(cg, "pose", &bsd->Pose.Pos[0], defaults, 7); - config_read_float_array(cg, "fcalphase", bsd->fcalphase, defaults, 2); - config_read_float_array(cg, "fcaltilt", bsd->fcaltilt, defaults, 2); - config_read_float_array(cg, "fcalcurve", bsd->fcalcurve, defaults, 2); - config_read_float_array(cg, "fcalgibpha", bsd->fcalgibpha, defaults, 2); - config_read_float_array(cg, "fcalgibmag", bsd->fcalgibmag, defaults, 2); + config_read_float_array(cg, "fcalphase", bsd->fcal.phase, defaults, 2); + config_read_float_array(cg, "fcaltilt", bsd->fcal.tilt, defaults, 2); + config_read_float_array(cg, "fcalcurve", bsd->fcal.curve, defaults, 2); + config_read_float_array(cg, "fcalgibpha", bsd->fcal.gibpha, defaults, 2); + config_read_float_array(cg, "fcalgibmag", bsd->fcal.gibmag, defaults, 2); bsd->PositionSet = config_read_uint32(cg, "PositionSet", 0); } @@ -132,12 +133,13 @@ void config_set_lighthouse(config_group *lh_config, BaseStationData *bsd, uint8_ config_group *cg = lh_config + idx; config_set_uint32(cg, "index", idx); config_set_uint32(cg, "id", bsd->BaseStationID); + config_set_uint32(cg, "mode", bsd->mode); config_set_float_a(cg, "pose", &bsd->Pose.Pos[0], 7); - config_set_float_a(cg, "fcalphase", bsd->fcalphase, 2); - config_set_float_a(cg, "fcaltilt", bsd->fcaltilt, 2); - config_set_float_a(cg, "fcalcurve", bsd->fcalcurve, 2); - config_set_float_a(cg, "fcalgibpha", bsd->fcalgibpha, 2); - config_set_float_a(cg, "fcalgibmag", bsd->fcalgibmag, 2); + config_set_float_a(cg, "fcalphase", bsd->fcal.phase, 2); + config_set_float_a(cg, "fcaltilt", bsd->fcal.tilt, 2); + config_set_float_a(cg, "fcalcurve", bsd->fcal.curve, 2); + config_set_float_a(cg, "fcalgibpha", bsd->fcal.gibpha, 2); + config_set_float_a(cg, "fcalgibmag", bsd->fcal.gibmag, 2); config_set_uint32(cg, "PositionSet", bsd->PositionSet); } diff --git a/src/survive_process.c b/src/survive_process.c index 97fbc46..62459f2 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -51,14 +51,17 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode static int use_bsd_cal = -1; if(use_bsd_cal == -1) { use_bsd_cal = survive_configi(ctx, "use-bsd-cal", SC_GET, 1); + if (use_bsd_cal == 0) { + SV_INFO("Not using BSD calibration values"); + } } if(use_bsd_cal) { BaseStationData * bsd = &ctx->bsd[base_station]; - //XXX TODO: This seriously needs to be worked on. See: https://github.com/cnlohr/libsurvive/issues/18 - angle += bsd->fcalphase[axis]; + // XXX TODO: This seriously needs to be worked on. See: https://github.com/cnlohr/libsurvive/issues/18 + // angle += (use_bsd_cal == 2 ? -1 : 1) * bsd->fcal.phase[axis]; // angle += bsd->fcaltilt[axis] * predicted_angle(axis1); - + //TODO!!! } diff --git a/src/survive_reproject.c b/src/survive_reproject.c index eabdb07..845f30a 100644 --- a/src/survive_reproject.c +++ b/src/survive_reproject.c @@ -1,6 +1,7 @@ #include "survive_reproject.h" #include <../redist/linmath.h> #include +#include #include static void survive_calibration_options_config_normalize( @@ -72,9 +73,8 @@ static FLT gibf(bool useSin, FLT v) { return cos(v); } -void survive_reproject_from_pose_with_config( - const SurviveContext *ctx, const survive_calibration_config *config, - int lighthouse, const SurvivePose *pose, const FLT *pt, FLT *out) { +void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const survive_calibration_config *config, + const SurvivePose *pose, const FLT *pt, FLT *out) { LinmathQuat invq; quatgetreciprocal(invq, pose->Rot); @@ -92,33 +92,36 @@ void survive_reproject_from_pose_with_config( double ang_x = atan(x); double ang_y = atan(y); - const BaseStationData *bsd = &ctx->bsd[lighthouse]; double phase[2]; - survive_calibration_options_config_apply(&config->phase, bsd->fcalphase, - phase); + survive_calibration_options_config_apply(&config->phase, bsd->fcal.phase, phase); double tilt[2]; - survive_calibration_options_config_apply(&config->tilt, bsd->fcaltilt, - tilt); + survive_calibration_options_config_apply(&config->tilt, bsd->fcal.tilt, tilt); double curve[2]; - survive_calibration_options_config_apply(&config->curve, bsd->fcalcurve, - curve); + survive_calibration_options_config_apply(&config->curve, bsd->fcal.curve, curve); double gibPhase[2]; - survive_calibration_options_config_apply(&config->gibPhase, bsd->fcalgibpha, - gibPhase); + survive_calibration_options_config_apply(&config->gibPhase, bsd->fcal.gibpha, gibPhase); double gibMag[2]; - survive_calibration_options_config_apply(&config->gibMag, bsd->fcalgibmag, - gibMag); + survive_calibration_options_config_apply(&config->gibMag, bsd->fcal.gibmag, gibMag); - out[0] = ang_x + phase[0] + tan(tilt[0]) * y + curve[0] * y * y + + out[0] = ang_x + phase[0] + (tilt[0]) * ang_y + curve[0] * ang_y * ang_y + gibf(config->gibUseSin, gibPhase[0] + ang_x) * gibMag[0]; - out[1] = ang_y + phase[1] + tan(tilt[1]) * x + curve[1] * x * x + + out[1] = ang_y + phase[1] + (tilt[1]) * ang_x + curve[1] * ang_x * ang_x + gibf(config->gibUseSin, gibPhase[1] + ang_y) * gibMag[1]; } +void survive_reproject_from_pose_with_config(const SurviveContext *ctx, const survive_calibration_config *config, + int lighthouse, const SurvivePose *pose, const FLT *pt, FLT *out) { + const BaseStationData *bsd = &ctx->bsd[lighthouse]; + survive_reproject_from_pose_with_bsd(bsd, config, pose, pt, out); +} + +void survive_reproject_with_config(const SurviveContext *ctx, const survive_calibration_config *config, int lighthouse, + const FLT *point3d, FLT *out) { + survive_reproject_from_pose_with_config(ctx, config, lighthouse, &ctx->bsd[lighthouse].Pose, point3d, out); +} void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, const SurvivePose *pose, FLT *pt, FLT *out) { - survive_reproject_from_pose_with_config( - ctx, survive_calibration_default_config(), lighthouse, pose, pt, out); + survive_reproject_from_pose_with_config(ctx, survive_calibration_default_config(ctx), lighthouse, pose, pt, out); } void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, @@ -127,14 +130,15 @@ void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, point3d, out); } -const survive_calibration_config *survive_calibration_default_config() { - static survive_calibration_config *def = 0; - if (def == 0) { - def = malloc(sizeof(survive_calibration_config)); - memset(def, 0, sizeof(survive_calibration_config)); - *def = survive_calibration_config_create_from_idx(0); +const survive_calibration_config *survive_calibration_default_config(const SurviveContext *_ctx) { + SurviveContext *ctx = (SurviveContext *)_ctx; + if (ctx->calibration_config == 0) { + size_t idx = survive_configi(ctx, "default-cal-conf", SC_GET, 0); + ctx->calibration_config = malloc(sizeof(survive_calibration_config)); + memset(ctx->calibration_config, 0, sizeof(survive_calibration_config)); + *ctx->calibration_config = survive_calibration_config_create_from_idx(idx); } - return def; + return ctx->calibration_config; } size_t survive_calibration_config_max_idx() { @@ -142,3 +146,78 @@ size_t survive_calibration_config_max_idx() { memset(&cfg, 0x1, sizeof(survive_calibration_config)); return survive_calibration_config_index(&cfg); } + +static void survive_calibration_options_config_fprint(FILE *file, const survive_calibration_options_config *self) { + fprintf(file, "\t"); + if (!self->enable[0] && !self->enable[1]) { + fprintf(file, "disabled"); + return; + } + + fprintf(file, "swap: %d\n", self->swap); + for (int i = 0; i < 2; i++) { + if (self->enable[i]) { + fprintf(file, "\tinvert[%d]: %d", i, self->invert[i]); + } else { + fprintf(file, "\t%d: disabled", i); + } + } +} + +void survive_calibration_config_fprint(FILE *file, const survive_calibration_config *self) { + fprintf(file, "Index: %ld\n", survive_calibration_config_index(self)); + + fprintf(file, "Phase: \n"); + survive_calibration_options_config_fprint(file, &self->phase); + fprintf(file, "\n"); + + fprintf(file, "Tilt: \n"); + survive_calibration_options_config_fprint(file, &self->tilt); + fprintf(file, "\n"); + + fprintf(file, "Curve: \n"); + survive_calibration_options_config_fprint(file, &self->curve); + fprintf(file, "\n"); + + fprintf(file, "gibPhase: \n"); + survive_calibration_options_config_fprint(file, &self->gibPhase); + fprintf(file, "\n"); + + fprintf(file, "gibMag: \n"); + survive_calibration_options_config_fprint(file, &self->gibMag); + fprintf(file, "\n"); + + fprintf(file, "gibUseSin: %d\n", self->gibUseSin); +} + +void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, enum SurviveCalFlag f, const FLT *in, + FLT *out) { + const BaseStationCal *cal = &ctx->bsd[lh].fcal; + out[0] = in[0] + cal->phase[0]; + out[1] = in[1] + cal->phase[1]; + + FLT tilt_scale = 10; + FLT curve_scale = 10000; + FLT gib_scale = 10; + const int iterations = 4; + for (int i = 0; i < iterations; i++) { + FLT last_out[2] = {out[0], out[1]}; + bool last_iteration = i == iterations - 1; + for (int j = 0; j < 2; j++) { + int oj = j == 0 ? 1 : 0; + out[j] = in[j]; + if (!last_iteration || (f & SVCal_Phase)) + out[j] += (cal->phase[j]); + if (!last_iteration || (f & SVCal_Tilt)) + out[j] += tan(cal->tilt[j] / tilt_scale) * last_out[oj]; + if (!last_iteration || (f & SVCal_Curve)) + out[j] += (cal->curve[j] / curve_scale) * last_out[oj] * last_out[oj]; + if (!last_iteration || (f & SVCal_Gib)) + out[j] += cos(cal->gibpha[j] + last_out[j]) * cal->gibmag[1] / gib_scale; + } + } +} + +void survive_apply_bsd_calibration(SurviveContext *ctx, int lh, const FLT *in, FLT *out) { + survive_apply_bsd_calibration_by_flag(ctx, lh, ctx->calibration_flag, in, out); +} diff --git a/src/survive_sensor_activations.c b/src/survive_sensor_activations.c index e42b50e..dc5c0d4 100644 --- a/src/survive_sensor_activations.c +++ b/src/survive_sensor_activations.c @@ -1,3 +1,4 @@ +#include #include bool SurviveSensorActivations_isPairValid(const SurviveSensorActivations *self, uint32_t tolerance, @@ -28,4 +29,4 @@ void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDa *length = lightData->length * 48000000; } -uint32_t SurviveSensorActivations_default_tolerance = (uint32_t)(48000000 /*mhz*/ * (16.7 * 2 /*ms*/) / 1000); +uint32_t SurviveSensorActivations_default_tolerance = (uint32_t)(48000000 /*mhz*/ * (16.7 * 2 /*ms*/) / 1000) + 5000; diff --git a/tools/findoptimalconfig/Makefile b/tools/findoptimalconfig/Makefile index 5e14814..f174812 100644 --- a/tools/findoptimalconfig/Makefile +++ b/tools/findoptimalconfig/Makefile @@ -7,7 +7,8 @@ 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) +findoptimalconfig : findoptimalconfig.cc $(LIBSURVIVE) + cd ../..;make g++ $(CFLAGS) -o $@ $^ $(LDFLAGS) clean : 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.3.1 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 --- include/libsurvive/survive.h | 10 +- include/libsurvive/survive_reproject.h | 37 +---- src/poser_epnp.c | 6 +- src/poser_sba.c | 41 +++-- src/survive.c | 3 +- src/survive_cal.c | 10 +- src/survive_reproject.c | 230 +++++++-------------------- tools/findoptimalconfig/findoptimalconfig.cc | 72 +-------- 8 files changed, 97 insertions(+), 312 deletions(-) (limited to 'tools') diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index e435752..669821d 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -185,6 +185,13 @@ enum SurviveCalFlag { SVCal_All = SVCal_Gib | SVCal_Curve | SVCal_Tilt | SVCal_Phase }; +typedef struct survive_calibration_config { + enum SurviveCalFlag use_flag; + FLT phase_scale, tilt_scale, curve_scale, gib_scale; +} survive_calibration_config; + +survive_calibration_config survive_calibration_config_ctor(); + struct SurviveContext { text_feedback_func faultfunction; text_feedback_func notefunction; @@ -222,8 +229,7 @@ struct SurviveContext { ButtonQueue buttonQueue; void *user_ptr; - enum SurviveCalFlag calibration_flag; - struct survive_calibration_config *calibration_config; + struct survive_calibration_config calibration_config; }; void survive_verify_FLT_size( diff --git a/include/libsurvive/survive_reproject.h b/include/libsurvive/survive_reproject.h index c90d39c..6546e66 100644 --- a/include/libsurvive/survive_reproject.h +++ b/include/libsurvive/survive_reproject.h @@ -9,35 +9,12 @@ extern "C" { #endif -typedef struct { - bool enable[2]; - bool invert[2]; - bool swap; -} survive_calibration_options_config; - -typedef struct survive_calibration_config { - - survive_calibration_options_config phase, tilt, curve, gibMag, gibPhase; - - bool gibUseSin; - -} survive_calibration_config; - -void survive_calibration_options_config_apply(const survive_calibration_options_config *option, const FLT *input, - FLT *output); - -const survive_calibration_config *survive_calibration_default_config(const SurviveContext *ctx); - -size_t survive_calibration_config_max_idx(); - -survive_calibration_config survive_calibration_config_create_from_idx(size_t v); - -size_t survive_calibration_config_index(const survive_calibration_config *config); - void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, FLT *out); void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, const SurvivePose *pose, FLT *point3d, FLT *out); +void survive_reproject_from_pose_with_config(const SurviveContext *ctx, struct survive_calibration_config *config, + int lighthouse, const SurvivePose *pose, FLT *point3d, FLT *out); // This is given a lighthouse -- in the same system as stored in BaseStationData, and // a 3d point and finds what the effective 'angle' value for a given lighthouse syste @@ -45,18 +22,12 @@ void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, cons // While this is typically opposite of what we want to do -- we want to find the 3d // position from a 2D coordinate, this is helpful since the minimization of reprojection // error is a core mechanism to many types of solvers. -void survive_reproject_from_pose_with_config(const SurviveContext *ctx, const survive_calibration_config *config, - int lighthouse, const SurvivePose *pose, const FLT *point3d, FLT *out); void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const survive_calibration_config *config, const SurvivePose *pose, const FLT *point3d, FLT *out); -void survive_reproject_with_config(const SurviveContext *ctx, const survive_calibration_config *config, int lighthouse, - const FLT *point3d, FLT *out); - -void survive_calibration_config_fprint(FILE *file, const survive_calibration_config *config); - -void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, enum SurviveCalFlag f, const FLT *in, FLT *out); +void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, struct survive_calibration_config *config, + const FLT *in, FLT *out); void survive_apply_bsd_calibration(SurviveContext *ctx, int lh, const FLT *in, FLT *out); #ifdef __cplusplus diff --git a/src/poser_epnp.c b/src/poser_epnp.c index c05450a..2cbd9c1 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -72,9 +72,9 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) for (size_t i = 0; i < so->sensor_ct; i++) { FLT *lengths = pdfs->lengths[i][lh]; - FLT *_ang = pdfs->angles[i][lh]; - FLT ang[2]; - survive_apply_bsd_calibration(so->ctx, lh, _ang, ang); + FLT *ang = pdfs->angles[i][lh]; + // FLT ang[2]; + // survive_apply_bsd_calibration(so->ctx, lh, _ang, ang); if (lengths[0] < 0 || lengths[1] < 0) continue; diff --git a/src/poser_sba.c b/src/poser_sba.c index 4a4ed8f..49854f2 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -18,7 +18,6 @@ #include "survive_reproject.h" typedef struct { - survive_calibration_config calibration_config; PoserData *pdfs; SurviveObject *so; SurvivePose obj_pose; @@ -58,8 +57,8 @@ static void metric_function(int j, int i, double *aj, double *xij, void *adata) SurvivePose obj2world = ctx->obj_pose; FLT sensorInWorld[3] = {0}; ApplyPoseToPoint(sensorInWorld, &obj2world, &so->sensor_locations[i * 3]); - survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, j, (SurvivePose *)aj, sensorInWorld, - xij); + survive_calibration_config cfg = so->ctx->calibration_config; + survive_reproject_from_pose_with_config(so->ctx, &cfg, j, (SurvivePose *)aj, sensorInWorld, xij); } static size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, char *vmask, double *meas) { @@ -73,9 +72,9 @@ static size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, continue; } - double *_angles = pdfs->angles[sensor][lh]; - double angles[2]; - survive_apply_bsd_calibration(so->ctx, lh, _angles, angles); + double *angles = pdfs->angles[sensor][lh]; + // double angles[2]; + // survive_apply_bsd_calibration(so->ctx, lh, _angles, angles); vmask[sensor * NUM_LIGHTHOUSES + lh] = 1; meas[measCount++] = angles[0]; @@ -93,9 +92,9 @@ static size_t construct_input_from_scene(SBAData *d, PoserDataLight *pdl, Surviv for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) { for (size_t lh = 0; lh < 2; lh++) { if (SurviveSensorActivations_isPairValid(scene, d->sensor_time_window, pdl->timecode, sensor, lh)) { - double *_a = scene->angles[sensor][lh]; - FLT a[2]; - survive_apply_bsd_calibration(so->ctx, lh, _a, a); + const double *a = scene->angles[sensor][lh]; + // FLT a[2]; + // survive_apply_bsd_calibration(so->ctx, lh, _a, a); vmask[sensor * NUM_LIGHTHOUSES + lh] = 1; if (cov) { @@ -160,7 +159,7 @@ static void str_metric_function_single_sweep(int j, int i, double *bi, double *x SurvivePose *camera = &so->ctx->bsd[lh].Pose; FLT out[2]; - survive_reproject_from_pose_with_config(so->ctx, &ctx->hdr.calibration_config, lh, camera, xyz, out); + survive_reproject_from_pose(so->ctx, lh, camera, xyz, out); *xij = out[acode]; } @@ -181,12 +180,11 @@ static void str_metric_function(int j, int i, double *bi, double *xij, void *ada // 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); + survive_reproject_from_pose(so->ctx, lh, camera, xyz, xij); } -static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config options, PoserDataLight *pdl, - SurviveSensorActivations *scene, int max_iterations /* = 50*/, - double max_reproj_error /* = 0.005*/) { +static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, SurviveSensorActivations *scene, + int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) { double *covx = 0; SurviveObject *so = d->so; @@ -246,7 +244,7 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o double opts[SBA_OPTSSZ] = {0}; double info[SBA_INFOSZ] = {0}; - sba_context ctx = {options, &pdl->hdr, so}; + sba_context ctx = {&pdl->hdr, so}; opts[0] = SBA_INIT_MU; opts[1] = SBA_STOP_THRESH; @@ -300,15 +298,15 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o } // Optimizes for LH position assuming object is posed at 0 -static double run_sba(survive_calibration_config options, PoserDataFullScene *pdfs, SurviveObject *so, - int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) { +static double run_sba(PoserDataFullScene *pdfs, SurviveObject *so, int max_iterations /* = 50*/, + double max_reproj_error /* = 0.005*/) { double *covx = 0; char *vmask = alloca(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES); double *meas = alloca(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES); size_t meas_size = construct_input(so, pdfs, vmask, meas); - sba_context sbactx = {options, &pdfs->hdr, so, .camera_params = {so->ctx->bsd[0].Pose, so->ctx->bsd[1].Pose}, + sba_context sbactx = {&pdfs->hdr, so, .camera_params = {so->ctx->bsd[0].Pose, so->ctx->bsd[1].Pose}, .obj_pose = so->OutPose}; { @@ -424,8 +422,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { // only process sweeps FLT error = -1; if (d->last_lh != lightData->lh || d->last_acode != lightData->acode) { - survive_calibration_config config = *survive_calibration_default_config(ctx); - error = run_sba_find_3d_structure(d, config, lightData, scene, 100, .5); + error = run_sba_find_3d_structure(d, lightData, scene, 100, .5); d->last_lh = lightData->lh; d->last_acode = lightData->acode; } @@ -443,9 +440,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { case POSERDATA_FULL_SCENE: { SurviveContext *ctx = so->ctx; PoserDataFullScene *pdfs = (PoserDataFullScene *)(pd); - survive_calibration_config config = *survive_calibration_default_config(ctx); - SV_INFO("Running sba with %u", (int)survive_calibration_config_index(&config)); - double error = run_sba(config, pdfs, so, 100, .005); + double error = run_sba(pdfs, so, 100, .005); // std::cerr << "Average reproj error: " << error << std::endl; return 0; } diff --git a/src/survive.c b/src/survive.c index 63ad2ba..2a7aad1 100644 --- a/src/survive.c +++ b/src/survive.c @@ -219,7 +219,8 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) { ctx->configfunction = survive_default_htc_config_process; ctx->rawposeproc = survive_default_raw_pose_process; - ctx->calibration_flag = (enum SurviveCalFlag)survive_configi(ctx, "bsd-cal", SC_GET, SVCal_All); + ctx->calibration_config = survive_calibration_config_ctor(); + ctx->calibration_config.use_flag = (enum SurviveCalFlag)survive_configi(ctx, "bsd-cal", SC_GET, SVCal_All); return ctx; } diff --git a/src/survive_cal.c b/src/survive_cal.c index e094e7b..3015b68 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -13,12 +13,13 @@ #include "survive_internal.h" #include "survive_reproject.h" +#include +#include #include +#include #include #include #include -#include -#include #include "survive_config.h" @@ -618,9 +619,8 @@ static void handle_calibration( struct SurviveCalData *cd ) } fsd.lengths[i][j][0] = cd->avglens[dataindex+0]; fsd.lengths[i][j][1] = cd->avglens[dataindex+1]; - // fsd.angles[i][j][0] = cd->avgsweeps[dataindex+0]; - // fsd.angles[i][j][1] = cd->avgsweeps[dataindex+1]; - survive_apply_bsd_calibration(ctx, lh, &cd->avgsweeps[dataindex], fsd.angles[i][j]); + fsd.angles[i][j][0] = cd->avgsweeps[dataindex + 0]; + fsd.angles[i][j][1] = cd->avgsweeps[dataindex + 1]; fsd.synctimes[i][j] = temp_syncs[i][j]; } diff --git a/src/survive_reproject.c b/src/survive_reproject.c index 845f30a..ee9704f 100644 --- a/src/survive_reproject.c +++ b/src/survive_reproject.c @@ -4,75 +4,6 @@ #include #include -static void survive_calibration_options_config_normalize( - survive_calibration_options_config *option) { - if (!option->enable[0]) - option->invert[0] = false; - if (!option->enable[1]) - option->invert[1] = false; - if (!option->enable[0] && !option->enable[1]) - option->swap = false; -} - -void survive_calibration_options_config_apply( - const survive_calibration_options_config *option, const FLT *input, - FLT *output) { - FLT tmp[2]; // In case they try to do in place - for (int i = 0; i < 2; i++) { - tmp[i] = option->enable[i] * (option->invert[i] ? -1 : 1) * - input[i ^ option->swap]; - } - for (int i = 0; i < 2; i++) { - output[i] = tmp[i]; - } -} - -survive_calibration_config -survive_calibration_config_create_from_idx(size_t v) { - survive_calibration_config config; - memset(&config, 0, sizeof(config)); - - bool *_this = (bool *)&config; - - for (size_t i = 0; i < sizeof(config); i++) { - _this[i] = (bool)(v & 1); - v = v >> 1; - } - - survive_calibration_options_config_normalize(&config.phase); - survive_calibration_options_config_normalize(&config.tilt); - survive_calibration_options_config_normalize(&config.curve); - survive_calibration_options_config_normalize(&config.gibMag); - - config.gibPhase.enable[0] = config.gibMag.enable[0]; - config.gibPhase.enable[1] = config.gibMag.enable[1]; - - survive_calibration_options_config_normalize(&config.gibPhase); - - if (!config.gibPhase.enable[0] && !config.gibPhase.enable[1]) - config.gibUseSin = false; - - return config; -} - -size_t -survive_calibration_config_index(const survive_calibration_config *config) { - bool *_this = (bool *)config; - size_t v = 0; - for (size_t i = 0; i < sizeof(*config); i++) { - v = (v | _this[sizeof(*config) - i - 1]); - v = v << 1; - } - v = v >> 1; - return v; -} - -static FLT gibf(bool useSin, FLT v) { - if (useSin) - return sin(v); - return cos(v); -} - void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const survive_calibration_config *config, const SurvivePose *pose, const FLT *pt, FLT *out) { LinmathQuat invq; @@ -89,116 +20,43 @@ void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const surv FLT x = -t_pt[0] / -t_pt[2]; FLT y = t_pt[1] / -t_pt[2]; - double ang_x = atan(x); - double ang_y = atan(y); - - double phase[2]; - survive_calibration_options_config_apply(&config->phase, bsd->fcal.phase, phase); - double tilt[2]; - survive_calibration_options_config_apply(&config->tilt, bsd->fcal.tilt, tilt); - double curve[2]; - survive_calibration_options_config_apply(&config->curve, bsd->fcal.curve, curve); - double gibPhase[2]; - survive_calibration_options_config_apply(&config->gibPhase, bsd->fcal.gibpha, gibPhase); - double gibMag[2]; - survive_calibration_options_config_apply(&config->gibMag, bsd->fcal.gibmag, gibMag); - - out[0] = ang_x + phase[0] + (tilt[0]) * ang_y + curve[0] * ang_y * ang_y + - gibf(config->gibUseSin, gibPhase[0] + ang_x) * gibMag[0]; - out[1] = ang_y + phase[1] + (tilt[1]) * ang_x + curve[1] * ang_x * ang_x + - gibf(config->gibUseSin, gibPhase[1] + ang_y) * gibMag[1]; -} -void survive_reproject_from_pose_with_config(const SurviveContext *ctx, const survive_calibration_config *config, - int lighthouse, const SurvivePose *pose, const FLT *pt, FLT *out) { - const BaseStationData *bsd = &ctx->bsd[lighthouse]; - survive_reproject_from_pose_with_bsd(bsd, config, pose, pt, out); -} - -void survive_reproject_with_config(const SurviveContext *ctx, const survive_calibration_config *config, int lighthouse, - const FLT *point3d, FLT *out) { - survive_reproject_from_pose_with_config(ctx, config, lighthouse, &ctx->bsd[lighthouse].Pose, point3d, out); -} - -void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, - const SurvivePose *pose, FLT *pt, FLT *out) { - survive_reproject_from_pose_with_config(ctx, survive_calibration_default_config(ctx), lighthouse, pose, pt, out); -} + double ang[] = {atan(x), atan(y)}; -void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, - FLT *out) { - survive_reproject_from_pose(ctx, lighthouse, &ctx->bsd[lighthouse].Pose, - point3d, out); -} + const FLT *phase = bsd->fcal.phase; + const FLT *curve = bsd->fcal.curve; + const FLT *tilt = bsd->fcal.tilt; + const FLT *gibPhase = bsd->fcal.gibpha; + const FLT *gibMag = bsd->fcal.gibmag; + enum SurviveCalFlag f = config->use_flag; -const survive_calibration_config *survive_calibration_default_config(const SurviveContext *_ctx) { - SurviveContext *ctx = (SurviveContext *)_ctx; - if (ctx->calibration_config == 0) { - size_t idx = survive_configi(ctx, "default-cal-conf", SC_GET, 0); - ctx->calibration_config = malloc(sizeof(survive_calibration_config)); - memset(ctx->calibration_config, 0, sizeof(survive_calibration_config)); - *ctx->calibration_config = survive_calibration_config_create_from_idx(idx); - } - return ctx->calibration_config; -} + for (int axis = 0; axis < 2; axis++) { + int opp_axis = axis == 0 ? 1 : 0; -size_t survive_calibration_config_max_idx() { - survive_calibration_config cfg; - memset(&cfg, 0x1, sizeof(survive_calibration_config)); - return survive_calibration_config_index(&cfg); -} + out[axis] = ang[axis]; -static void survive_calibration_options_config_fprint(FILE *file, const survive_calibration_options_config *self) { - fprintf(file, "\t"); - if (!self->enable[0] && !self->enable[1]) { - fprintf(file, "disabled"); - return; + if (f & SVCal_Phase) + out[axis] -= config->phase_scale * phase[axis]; + if (f & SVCal_Tilt) + out[axis] -= (config->tilt_scale * tilt[axis]) * ang[opp_axis]; + if (f & SVCal_Curve) + out[axis] -= config->curve_scale * curve[axis] * ang[opp_axis] * ang[opp_axis]; + if (f & SVCal_Gib) + out[axis] -= config->gib_scale * sin(gibPhase[axis] + ang[axis]) * gibMag[axis]; } - fprintf(file, "swap: %d\n", self->swap); - for (int i = 0; i < 2; i++) { - if (self->enable[i]) { - fprintf(file, "\tinvert[%d]: %d", i, self->invert[i]); - } else { - fprintf(file, "\t%d: disabled", i); - } - } } -void survive_calibration_config_fprint(FILE *file, const survive_calibration_config *self) { - fprintf(file, "Index: %ld\n", survive_calibration_config_index(self)); - - fprintf(file, "Phase: \n"); - survive_calibration_options_config_fprint(file, &self->phase); - fprintf(file, "\n"); - - fprintf(file, "Tilt: \n"); - survive_calibration_options_config_fprint(file, &self->tilt); - fprintf(file, "\n"); - - fprintf(file, "Curve: \n"); - survive_calibration_options_config_fprint(file, &self->curve); - fprintf(file, "\n"); - - fprintf(file, "gibPhase: \n"); - survive_calibration_options_config_fprint(file, &self->gibPhase); - fprintf(file, "\n"); - - fprintf(file, "gibMag: \n"); - survive_calibration_options_config_fprint(file, &self->gibMag); - fprintf(file, "\n"); - - fprintf(file, "gibUseSin: %d\n", self->gibUseSin); -} - -void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, enum SurviveCalFlag f, const FLT *in, - FLT *out) { +void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, struct survive_calibration_config *config, + const FLT *in, FLT *out) { const BaseStationCal *cal = &ctx->bsd[lh].fcal; - out[0] = in[0] + cal->phase[0]; - out[1] = in[1] + cal->phase[1]; - - FLT tilt_scale = 10; - FLT curve_scale = 10000; - FLT gib_scale = 10; + out[0] = in[0] + config->phase_scale * cal->phase[0]; + out[1] = in[1] + config->phase_scale * cal->phase[1]; + + enum SurviveCalFlag f = config->use_flag; + FLT phase_scale = config->phase_scale; + FLT tilt_scale = config->tilt_scale; + FLT curve_scale = config->curve_scale; + FLT gib_scale = config->gib_scale; const int iterations = 4; for (int i = 0; i < iterations; i++) { FLT last_out[2] = {out[0], out[1]}; @@ -207,17 +65,39 @@ void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, enum Sur int oj = j == 0 ? 1 : 0; out[j] = in[j]; if (!last_iteration || (f & SVCal_Phase)) - out[j] += (cal->phase[j]); + out[j] += phase_scale * cal->phase[j]; if (!last_iteration || (f & SVCal_Tilt)) - out[j] += tan(cal->tilt[j] / tilt_scale) * last_out[oj]; + out[j] += (tilt_scale * cal->tilt[j]) * last_out[oj]; if (!last_iteration || (f & SVCal_Curve)) - out[j] += (cal->curve[j] / curve_scale) * last_out[oj] * last_out[oj]; + out[j] += (cal->curve[j] * curve_scale) * last_out[oj] * last_out[oj]; if (!last_iteration || (f & SVCal_Gib)) - out[j] += cos(cal->gibpha[j] + last_out[j]) * cal->gibmag[1] / gib_scale; + out[j] += sin(cal->gibpha[j] + last_out[j]) * cal->gibmag[j] * gib_scale; } } } +void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, const SurvivePose *pose, FLT *pt, + FLT *out) { + survive_reproject_from_pose_with_bsd(&ctx->bsd[lighthouse], &ctx->calibration_config, pose, pt, out); +} + +void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, FLT *out) { + survive_reproject_from_pose(ctx, lighthouse, &ctx->bsd[lighthouse].Pose, point3d, out); +} + +survive_calibration_config survive_calibration_config_ctor() { + return (survive_calibration_config){.use_flag = SVCal_All, + .phase_scale = 1., + .tilt_scale = 1. / 10000., + .curve_scale = 1. / 1000., + .gib_scale = -1. / 10.}; +} + void survive_apply_bsd_calibration(SurviveContext *ctx, int lh, const FLT *in, FLT *out) { - survive_apply_bsd_calibration_by_flag(ctx, lh, ctx->calibration_flag, in, out); + survive_apply_bsd_calibration_by_flag(ctx, lh, &ctx->calibration_config, in, out); +} + +void survive_reproject_from_pose_with_config(const SurviveContext *ctx, struct survive_calibration_config *config, + int lighthouse, const SurvivePose *pose, FLT *point3d, FLT *out) { + return survive_reproject_from_pose_with_bsd(&ctx->bsd[lighthouse], config, pose, point3d, out); } 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.3.1 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 --- include/libsurvive/survive.h | 2 +- src/survive_reproject.c | 15 ++++---- tools/findoptimalconfig/findoptimalconfig.cc | 54 ++++++++++++++-------------- 3 files changed, 35 insertions(+), 36 deletions(-) (limited to 'tools') diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 669821d..7248b1c 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -186,8 +186,8 @@ enum SurviveCalFlag { }; typedef struct survive_calibration_config { - enum SurviveCalFlag use_flag; FLT phase_scale, tilt_scale, curve_scale, gib_scale; + enum SurviveCalFlag use_flag; } survive_calibration_config; survive_calibration_config survive_calibration_config_ctor(); diff --git a/src/survive_reproject.c b/src/survive_reproject.c index ee9704f..751abc0 100644 --- a/src/survive_reproject.c +++ b/src/survive_reproject.c @@ -19,7 +19,7 @@ void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const surv FLT x = -t_pt[0] / -t_pt[2]; FLT y = t_pt[1] / -t_pt[2]; - + double xy[] = {x, y}; double ang[] = {atan(x), atan(y)}; const FLT *phase = bsd->fcal.phase; @@ -37,9 +37,9 @@ void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const surv if (f & SVCal_Phase) out[axis] -= config->phase_scale * phase[axis]; if (f & SVCal_Tilt) - out[axis] -= (config->tilt_scale * tilt[axis]) * ang[opp_axis]; + out[axis] -= tan(config->tilt_scale * tilt[axis]) * xy[opp_axis]; if (f & SVCal_Curve) - out[axis] -= config->curve_scale * curve[axis] * ang[opp_axis] * ang[opp_axis]; + out[axis] -= config->curve_scale * curve[axis] * xy[opp_axis] * xy[opp_axis]; if (f & SVCal_Gib) out[axis] -= config->gib_scale * sin(gibPhase[axis] + ang[axis]) * gibMag[axis]; } @@ -60,6 +60,7 @@ void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, struct s const int iterations = 4; for (int i = 0; i < iterations; i++) { FLT last_out[2] = {out[0], out[1]}; + FLT tlast_out[2] = {tan(out[0]), tan(out[1])}; bool last_iteration = i == iterations - 1; for (int j = 0; j < 2; j++) { int oj = j == 0 ? 1 : 0; @@ -67,9 +68,9 @@ void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, struct s if (!last_iteration || (f & SVCal_Phase)) out[j] += phase_scale * cal->phase[j]; if (!last_iteration || (f & SVCal_Tilt)) - out[j] += (tilt_scale * cal->tilt[j]) * last_out[oj]; + out[j] += tan(tilt_scale * cal->tilt[j]) * tlast_out[oj]; if (!last_iteration || (f & SVCal_Curve)) - out[j] += (cal->curve[j] * curve_scale) * last_out[oj] * last_out[oj]; + out[j] += (cal->curve[j] * curve_scale) * tlast_out[oj] * tlast_out[oj]; if (!last_iteration || (f & SVCal_Gib)) out[j] += sin(cal->gibpha[j] + last_out[j]) * cal->gibmag[j] * gib_scale; } @@ -88,8 +89,8 @@ void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, survive_calibration_config survive_calibration_config_ctor() { return (survive_calibration_config){.use_flag = SVCal_All, .phase_scale = 1., - .tilt_scale = 1. / 10000., - .curve_scale = 1. / 1000., + .tilt_scale = 1. / 10., + .curve_scale = 1. / 10., .gib_scale = -1. / 10.}; } diff --git a/tools/findoptimalconfig/findoptimalconfig.cc b/tools/findoptimalconfig/findoptimalconfig.cc index 88a43eb..b94590f 100644 --- a/tools/findoptimalconfig/findoptimalconfig.cc +++ b/tools/findoptimalconfig/findoptimalconfig.cc @@ -200,35 +200,36 @@ double sba_opt(SurviveContext *ctx, const survive_calibration_config &config, Pl struct optimal_cal_ctx { std::vector 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.3.1 From 5384af65f9d63d095cb9987d36de57ccee323300 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 1 Apr 2018 15:47:07 -0600 Subject: Added reproject tool --- tools/showreproject/Makefile | 16 +++ tools/showreproject/showreproject.cc | 271 +++++++++++++++++++++++++++++++++++ 2 files changed, 287 insertions(+) create mode 100644 tools/showreproject/Makefile create mode 100644 tools/showreproject/showreproject.cc (limited to 'tools') 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..89d67c4 --- /dev/null +++ b/tools/showreproject/showreproject.cc @@ -0,0 +1,271 @@ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "opencv2/imgproc.hpp" +#include +#include + +uint32_t timestamp; + +cv::Mat_ img; +cv::Mat_ err[2]; + +cv::Vec3f flow2rgb(float x, float y, float scale = 1) { + cv::Mat_ 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_ 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_(); + + 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 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_ img = cv::Mat_(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_flag(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_(SIZE, numCtx * SIZE); + img.setTo(cv::Vec3b(0, 0, 0)); + } + for (int lh = 0; lh < 2; lh++) { + err[lh] = cv::Mat_(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; +} -- cgit v1.3.1 From 1724abef15a4090640bd82ba408681438316de7e Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 1 Apr 2018 16:04:05 -0600 Subject: Made calibration on other posers use calibration data --- .gitignore | 1 + include/libsurvive/survive_reproject.h | 14 +++++++++++--- src/poser_charlesslow.c | 28 +++++++++++++++++++--------- src/poser_daveortho.c | 17 ++++++++++------- src/poser_epnp.c | 6 +++--- src/poser_sba.c | 2 -- src/poser_turveytori.c | 18 ++++++++++++++---- src/survive_reproject.c | 6 +++--- tools/showreproject/showreproject.cc | 2 +- 9 files changed, 62 insertions(+), 32 deletions(-) (limited to 'tools') diff --git a/.gitignore b/.gitignore index 8cfd2be..f39d866 100644 --- a/.gitignore +++ b/.gitignore @@ -19,6 +19,7 @@ simple_pose_test *.csv calinfo/ *.log +*.png # Windows specific *.dll diff --git a/include/libsurvive/survive_reproject.h b/include/libsurvive/survive_reproject.h index 6546e66..e4f21d0 100644 --- a/include/libsurvive/survive_reproject.h +++ b/include/libsurvive/survive_reproject.h @@ -17,8 +17,9 @@ void survive_reproject_from_pose_with_config(const SurviveContext *ctx, struct s int lighthouse, const SurvivePose *pose, FLT *point3d, FLT *out); // This is given a lighthouse -- in the same system as stored in BaseStationData, and -// a 3d point and finds what the effective 'angle' value for a given lighthouse syste +// a 3d point and finds what the effective 'angle' value for a given lighthouse system // would be. +// // While this is typically opposite of what we want to do -- we want to find the 3d // position from a 2D coordinate, this is helpful since the minimization of reprojection // error is a core mechanism to many types of solvers. @@ -26,10 +27,17 @@ void survive_reproject_from_pose_with_config(const SurviveContext *ctx, struct s void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const survive_calibration_config *config, const SurvivePose *pose, const FLT *point3d, FLT *out); -void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, struct survive_calibration_config *config, - const FLT *in, FLT *out); +// This is given input from the light sensors and approximates the idealized version of them +// by incorporating the calibration data from the lighthouse. In theory, it's an approximation +// but in practice in converges pretty quickly and to a good degree of accuracy. +// That said, all things being equal, it is better to compare reprojection to raw incoming +// data if you are looking to minimize that error. void survive_apply_bsd_calibration(SurviveContext *ctx, int lh, const FLT *in, FLT *out); +// Same as above, but lets you specify the configuration. Used internally and also in some tools +void survive_apply_bsd_calibration_by_config(SurviveContext *ctx, int lh, struct survive_calibration_config *config, + const FLT *in, FLT *out); + #ifdef __cplusplus } #endif diff --git a/src/poser_charlesslow.c b/src/poser_charlesslow.c index bc6683a..b44225e 100644 --- a/src/poser_charlesslow.c +++ b/src/poser_charlesslow.c @@ -1,12 +1,13 @@ +#include "linmath.h" #include "survive_cal.h" +#include +#include #include -#include -#include "linmath.h" -#include #include #include -#include -#include +#include +#include +#include typedef struct { @@ -255,9 +256,13 @@ static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int pr int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2; if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue; + FLT out[2] = {}; + survive_apply_bsd_calibration(hmd->ctx, lh, fs->angles[p][lh], out); + //Find out where our ray shoots forth from. - FLT ax = fs->angles[p][lh][0]; - FLT ay = fs->angles[p][lh][1]; + FLT ax = out[0]; + FLT ay = out[1]; + //NOTE: Inputs may never be output with cross product. //Create a fictitious normalized ray. Imagine the lighthouse is pointed //straight in the +z direction, this is the lighthouse ray to the point. @@ -338,8 +343,13 @@ static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int pr if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue; //Find out where our ray shoots forth from. - FLT ax = fs->angles[p][lh][0]; - FLT ay = fs->angles[p][lh][1]; + FLT out[2] = {}; + survive_apply_bsd_calibration(hmd->ctx, lh, fs->angles[p][lh], out); + + // Find out where our ray shoots forth from. + FLT ax = out[0]; + FLT ay = out[1]; + FLT RayShootOut[3] = { sin(ax), sin(ay), 0 }; RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) ); diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index c47bceb..9cdab45 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -1,12 +1,13 @@ +#include "linmath.h" #include "survive_cal.h" +#include +#include #include -#include -#include "linmath.h" -#include #include #include -#include -#include +#include +#include +#include // Dave talks about this poser here: https://www.youtube.com/watch?v=nSbEltdH9vM&feature=youtu.be&t=2h29m47s @@ -139,8 +140,10 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) //Load all our valid points into something the LHFinder can use. if( fs->lengths[i][LH_ID][0] > 0 ) { - S_in[0][max_hits] = fs->angles[i][LH_ID][0]; - S_in[1][max_hits] = fs->angles[i][LH_ID][1]; + FLT out[2]; + survive_apply_bsd_calibration(ctx, LH_ID, fs->angles[i][LH_ID], out); + S_in[0][max_hits] = out[0]; + S_in[1][max_hits] = out[1]; X_in[0][max_hits] = so->sensor_locations[i*3+0]; X_in[1][max_hits] = so->sensor_locations[i*3+1]; X_in[2][max_hits] = so->sensor_locations[i*3+2]; diff --git a/src/poser_epnp.c b/src/poser_epnp.c index 2cbd9c1..c05450a 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -72,9 +72,9 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) for (size_t i = 0; i < so->sensor_ct; i++) { FLT *lengths = pdfs->lengths[i][lh]; - FLT *ang = pdfs->angles[i][lh]; - // FLT ang[2]; - // survive_apply_bsd_calibration(so->ctx, lh, _ang, ang); + FLT *_ang = pdfs->angles[i][lh]; + FLT ang[2]; + survive_apply_bsd_calibration(so->ctx, lh, _ang, ang); if (lengths[0] < 0 || lengths[1] < 0) continue; diff --git a/src/poser_sba.c b/src/poser_sba.c index 49854f2..bd7d520 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -73,8 +73,6 @@ static size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, } double *angles = pdfs->angles[sensor][lh]; - // double angles[2]; - // survive_apply_bsd_calibration(so->ctx, lh, _angles, angles); vmask[sensor * NUM_LIGHTHOUSES + lh] = 1; meas[measCount++] = angles[0]; diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 035fca7..4628207 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -13,6 +13,8 @@ #include #else #include //for alloca +#include + #endif @@ -1786,8 +1788,12 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) to->sensor[sensorCount].point.x = point[0]; to->sensor[sensorCount].point.y = point[1]; to->sensor[sensorCount].point.z = point[2]; - to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) - to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) + + FLT out[2]; + survive_apply_bsd_calibration(ctx, 0, fs->angles[i][0], out); + + to->sensor[sensorCount].theta = out[0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = out[1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) sensorCount++; } @@ -1822,8 +1828,12 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) to->sensor[sensorCount].point.x = point[0]; to->sensor[sensorCount].point.y = point[1]; to->sensor[sensorCount].point.z = point[2]; - to->sensor[sensorCount].theta = fs->angles[i][lh][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) - to->sensor[sensorCount].phi = fs->angles[i][lh][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + + FLT out[2]; + survive_apply_bsd_calibration(ctx, lh, fs->angles[i][lh], out); + + to->sensor[sensorCount].theta = out[0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = out[1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) sensorCount++; } } diff --git a/src/survive_reproject.c b/src/survive_reproject.c index 751abc0..0eaceb2 100644 --- a/src/survive_reproject.c +++ b/src/survive_reproject.c @@ -46,8 +46,8 @@ void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const surv } -void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, struct survive_calibration_config *config, - const FLT *in, FLT *out) { +void survive_apply_bsd_calibration_by_config(SurviveContext *ctx, int lh, struct survive_calibration_config *config, + const FLT *in, FLT *out) { const BaseStationCal *cal = &ctx->bsd[lh].fcal; out[0] = in[0] + config->phase_scale * cal->phase[0]; out[1] = in[1] + config->phase_scale * cal->phase[1]; @@ -95,7 +95,7 @@ survive_calibration_config survive_calibration_config_ctor() { } void survive_apply_bsd_calibration(SurviveContext *ctx, int lh, const FLT *in, FLT *out) { - survive_apply_bsd_calibration_by_flag(ctx, lh, &ctx->calibration_config, in, out); + survive_apply_bsd_calibration_by_config(ctx, lh, &ctx->calibration_config, in, out); } void survive_reproject_from_pose_with_config(const SurviveContext *ctx, struct survive_calibration_config *config, diff --git a/tools/showreproject/showreproject.cc b/tools/showreproject/showreproject.cc index 89d67c4..caa7a66 100644 --- a/tools/showreproject/showreproject.cc +++ b/tools/showreproject/showreproject.cc @@ -176,7 +176,7 @@ void drawbsds(SurviveContext *ctx) { FLT out[2]; auto config = survive_calibration_config_ctor(); config.use_flag = f; - survive_apply_bsd_calibration_by_flag(ctx, lh, &config, in, out); + 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) { -- cgit v1.3.1