aboutsummaryrefslogtreecommitdiff
path: root/tools
diff options
context:
space:
mode:
authorJustin Berger <j.david.berger@gmail.com>2018-04-10 23:47:11 -0600
committerJustin Berger <j.david.berger@gmail.com>2018-04-10 23:47:11 -0600
commite6b487cacfd57792b9cdcde30489faf19f2a4a77 (patch)
tree2435f0302849919aa5001961a95daff693a16a12 /tools
parent30cfb87ec0d95e0cb8a671cf8f2327b4204927ed (diff)
parentd7c88592a5450a65f5359e23d87122a04d019503 (diff)
downloadlibsurvive-e6b487cacfd57792b9cdcde30489faf19f2a4a77.tar.gz
libsurvive-e6b487cacfd57792b9cdcde30489faf19f2a4a77.tar.bz2
Merge branch 'master' into simple_api
Diffstat (limited to 'tools')
-rw-r--r--tools/findoptimalconfig/findoptimalconfig.cc7
-rw-r--r--tools/generate_reprojection_functions/Makefile20
-rw-r--r--tools/generate_reprojection_functions/check_generated.c127
-rw-r--r--tools/generate_reprojection_functions/reprojection_functions.sage131
-rw-r--r--tools/showreproject/showreproject.cc6
5 files changed, 285 insertions, 6 deletions
diff --git a/tools/findoptimalconfig/findoptimalconfig.cc b/tools/findoptimalconfig/findoptimalconfig.cc
index b94590f..265fd94 100644
--- a/tools/findoptimalconfig/findoptimalconfig.cc
+++ b/tools/findoptimalconfig/findoptimalconfig.cc
@@ -9,7 +9,6 @@
#include <vector>
#include <sba/sba.h>
-#include <survive_reproject.h>
struct SBAData {
int last_acode = -1;
@@ -93,8 +92,8 @@ void light_process(SurviveObject *so, int sensor_id, int acode, int timeinsweep,
}
SurvivePose lastPose = {};
-void raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) {
- survive_default_raw_pose_process(so, lighthouse, pose);
+void pose_process(SurviveObject *so, uint32_t timecode, SurvivePose *pose) {
+ survive_default_raw_pose_process(so, timecode, pose);
PlaybackData *d = (PlaybackData *)so->ctx->user_ptr;
d->so = so;
d->inputs.emplace_back(so, *pose);
@@ -364,7 +363,7 @@ int main(int argc, char **argv) {
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_pose_fn(ctx, pose_process);
survive_install_lighthouse_pose_fn(ctx, lighthouse_process);
survive_install_light_fn(ctx, light_process);
diff --git a/tools/generate_reprojection_functions/Makefile b/tools/generate_reprojection_functions/Makefile
new file mode 100644
index 0000000..79d05cb
--- /dev/null
+++ b/tools/generate_reprojection_functions/Makefile
@@ -0,0 +1,20 @@
+all : check_generated
+
+SRT:=../..
+
+LIBSURVIVE:=$(SRT)/lib/libsurvive.so
+
+CFLAGS:=-I$(SRT)/redist -I$(SRT)/include -O3 -g -DFLT=double -DUSE_DOUBLE # -fsanitize=address -fsanitize=undefined
+
+check_generated: check_generated.c ../../src/survive_reproject.generated.h survive_reproject.full.generated.h $(LIBSURVIVE)
+ cd ../..;make
+ gcc $(CFLAGS) -o $@ $^ $(LDFLAGS) -lm -lc -lgcc
+
+clean :
+ rm -rf check_generated
+
+../../src/survive_reproject.generated.h: reprojection_functions.sage
+ sage reprojection_functions.sage > ../../src/survive_reproject.generated.h
+
+survive_reproject.full.generated.h: reprojection_functions.sage
+ sage reprojection_functions.sage --full > survive_reproject.full.generated.h
diff --git a/tools/generate_reprojection_functions/check_generated.c b/tools/generate_reprojection_functions/check_generated.c
new file mode 100644
index 0000000..446e94f
--- /dev/null
+++ b/tools/generate_reprojection_functions/check_generated.c
@@ -0,0 +1,127 @@
+#include "survive_reproject.full.generated.h"
+#include <libsurvive/survive.h>
+#include <libsurvive/survive_reproject.h>
+#include <math.h>
+#include <os_generic.h>
+
+void gen_survive_reproject_full(FLT *out, const SurvivePose *obj_pose, const LinmathVec3d obj_pt,
+ const SurvivePose *lh2world, const BaseStationData *bsd,
+ const survive_calibration_config *config) {
+ FLT phase_scale = config->use_flag & SVCal_Phase ? config->phase_scale : 0.;
+ FLT phase_0 = bsd->fcal.phase[0];
+ FLT phase_1 = bsd->fcal.phase[1];
+
+ FLT tilt_scale = config->use_flag & SVCal_Tilt ? config->tilt_scale : 0.;
+ FLT tilt_0 = bsd->fcal.tilt[0];
+ FLT tilt_1 = bsd->fcal.tilt[1];
+
+ FLT curve_scale = config->use_flag & SVCal_Curve ? config->curve_scale : 0.;
+ FLT curve_0 = bsd->fcal.curve[0];
+ FLT curve_1 = bsd->fcal.curve[1];
+
+ FLT gib_scale = config->use_flag & SVCal_Gib ? config->gib_scale : 0;
+ FLT gibPhase_0 = bsd->fcal.gibpha[0];
+ FLT gibPhase_1 = bsd->fcal.gibpha[1];
+ FLT gibMag_0 = bsd->fcal.gibmag[0];
+ FLT gibMag_1 = bsd->fcal.gibmag[1];
+
+ gen_reproject(out, obj_pose->Pos, obj_pt, lh2world->Pos, phase_scale, phase_0, phase_1, tilt_scale, tilt_0, tilt_1,
+ curve_scale, curve_0, curve_1, gib_scale, gibPhase_0, gibPhase_1, gibMag_0, gibMag_1);
+}
+
+double next_rand(double mx) { return (float)rand() / (float)(RAND_MAX / mx) - mx / 2.; }
+
+SurvivePose random_pose() {
+ SurvivePose rtn = {.Pos = {next_rand(10), next_rand(10), next_rand(10)},
+ .Rot = {next_rand(1), next_rand(1), next_rand(1), next_rand(1)}};
+
+ quatnormalize(rtn.Rot, rtn.Rot);
+ return rtn;
+}
+
+void random_point(FLT *out) {
+ out[0] = next_rand(10);
+ out[1] = next_rand(10);
+ out[2] = next_rand(10);
+}
+
+void print_pose(const SurvivePose *pose) {
+ printf("[%f %f %f] [%f %f %f %f]\n", pose->Pos[0], pose->Pos[1], pose->Pos[2], pose->Rot[0], pose->Rot[1],
+ pose->Rot[2], pose->Rot[3]);
+}
+
+void check_rotate_vector() {
+ SurvivePose obj = random_pose();
+ FLT pt[3];
+ random_point(pt);
+
+ int cycles = 1000000000;
+ FLT gen_out[3], out[3];
+ double start, stop;
+ start = OGGetAbsoluteTime();
+ for (int i = 0; i < cycles; i++) {
+ gen_quat_rotate_vector(gen_out, obj.Rot, pt);
+ }
+ stop = OGGetAbsoluteTime();
+ printf("gen: %f %f %f (%f)\n", gen_out[0], gen_out[1], gen_out[2], stop - start);
+
+ start = OGGetAbsoluteTime();
+ for (int i = 0; i < cycles; i++) {
+ quatrotatevector(out, obj.Rot, pt);
+ }
+ stop = OGGetAbsoluteTime();
+
+ printf("%f %f %f (%f)\n", out[0], out[1], out[2], stop - start);
+}
+
+void check_invert() {
+ SurvivePose obj = random_pose();
+ SurvivePose gen_inv, inv;
+ gen_invert_pose(gen_inv.Pos, obj.Pos);
+ InvertPose(&inv, &obj);
+
+ print_pose(&gen_inv);
+ print_pose(&inv);
+}
+
+void check_reproject() {
+ SurvivePose obj = random_pose();
+ LinmathVec3d pt;
+ random_point(pt);
+ SurvivePose lh = random_pose();
+
+ survive_calibration_config config;
+ BaseStationData bsd;
+ for (int i = 0; i < 10; i++)
+ *((FLT *)&bsd.fcal.phase[0] + i) = next_rand(1);
+
+ for (int i = 0; i < 4; i++)
+ *((FLT *)&config.phase_scale + i) = next_rand(1);
+
+ config.use_flag = (enum SurviveCalFlag)0xff;
+ FLT out_pt[2] = {0};
+ int cycles = 10000000;
+
+ double start_gen = OGGetAbsoluteTime();
+ for (int i = 0; i < cycles; i++) {
+ gen_survive_reproject_full(out_pt, &obj, pt, &lh, &bsd, &config);
+ }
+ double stop_gen = OGGetAbsoluteTime();
+ printf("gen: %f %f (%f)\n", out_pt[0], out_pt[1], stop_gen - start_gen);
+
+ double start_reproject = OGGetAbsoluteTime();
+ for (int i = 0; i < cycles; i++)
+ survive_reproject_full(out_pt, &obj, pt, &lh, &bsd, &config);
+ double stop_reproject = OGGetAbsoluteTime();
+
+ printf("%f %f (%f)\n", out_pt[0], out_pt[1], stop_reproject - start_reproject);
+ out_pt[0] = out_pt[1] = 0;
+}
+
+int main() {
+ check_rotate_vector();
+ check_invert();
+ check_reproject();
+
+ return 0;
+}
diff --git a/tools/generate_reprojection_functions/reprojection_functions.sage b/tools/generate_reprojection_functions/reprojection_functions.sage
new file mode 100644
index 0000000..1ff5c25
--- /dev/null
+++ b/tools/generate_reprojection_functions/reprojection_functions.sage
@@ -0,0 +1,131 @@
+# -*- python -*-
+from sympy.utilities.codegen import codegen
+from sympy.printing import print_ccode
+from sympy import cse, sqrt, sin, pprint, ccode
+import types
+import sys
+
+obj_qw,obj_qi,obj_qj,obj_qk=var('obj_qw,obj_qi,obj_qj,obj_qk')
+obj_px,obj_py,obj_pz=var('obj_px,obj_py,obj_pz')
+
+lh_qw,lh_qi,lh_qj,lh_qk=var('lh_qw,lh_qi,lh_qj,lh_qk')
+lh_px,lh_py,lh_pz=var('lh_px,lh_py,lh_pz')
+
+sensor_x,sensor_y,sensor_z=var('sensor_x,sensor_y,sensor_z')
+
+phase_scale=var('phase_scale')
+tilt_scale=var('tilt_scale')
+curve_scale=var('curve_scale')
+gib_scale=var('gib_scale')
+
+phase_0,phase_1=var('phase_0, phase_1')
+tilt_0,tilt_1=var('tilt_0, tilt_1')
+curve_0,curve_1=var('curve_0, curve_1')
+gibPhase_0,gibPhase_1=var('gibPhase_0, gibPhase_1')
+gibMag_0,gibMag_1=var('gibMag_0, gibMag_1')
+
+def quatmagnitude(q):
+ qw,qi,qj,qk = q
+ return sqrt(qw*qw+qi*qi+qj*qj+qk*qk)
+
+def quatrotationmatrix(q):
+ qw,qi,qj,qk = q
+ s = quatmagnitude(q)
+ return matrix(SR,
+ [ [ 1 - 2 * s * (qj*qj + qk*qk), 2 * s*(qi*qj - qk*qw), 2*s*(qi*qk + qj*qw)],
+ [ 2*s*(qi*qj + qk*qw), 1 - 2*s*(qi*qi+qk*qk), 2*s*(qj*qk-qi*qw)],
+ [ 2*s*(qi*qk-qj*qw), 2*s*(qj*qk+qi*qw), 1-2*s*(qi*qi+qj*qj)]
+ ])
+
+def quatrotatevector(q, pt):
+ qw,qi,qj,qk = q
+ x,y,z = pt
+ return quatrotationmatrix(q) * vector((x,y,z))
+
+def quatgetreciprocal(q):
+ return [ q[0], -q[1], -q[2], -q[3] ]
+
+def apply_pose_to_pt(p, pt):
+ px,py,pz = p[0]
+ return quatrotatevector(p[1], pt) + vector((px,py,pz))
+
+def invert_pose(p):
+ r = quatgetreciprocal(p[1])
+ return ( -1 * quatrotatevector(r, p[0]), r)
+
+def reproject(p, pt,
+ lh_p,
+ phase_scale, phase_0, phase_1,
+ tilt_scale, tilt_0, tilt_1,
+ curve_scale, curve_0, curve_1,
+ gib_scale, gibPhase_0, gibPhase_1, gibMag_0, gibMag_1):
+ pt_in_world = apply_pose_to_pt( p, pt )
+ pt_in_lh = apply_pose_to_pt( invert_pose(lh_p), pt_in_world)
+ xy = vector((pt_in_lh[0] / pt_in_lh[2], pt_in_lh[1] / -pt_in_lh[2]))
+ ang = vector((atan(xy[0]), atan(xy[1])))
+
+ return vector((
+ ang[0] - phase_scale * phase_0 - tan(tilt_scale * tilt_0) * xy[1] - curve_scale * curve_0 * xy[1] * xy[1] - gib_scale * sin(gibPhase_0 + ang[0]) * gibMag_0,
+ ang[1] - phase_scale * phase_1 - tan(tilt_scale * tilt_1) * xy[0] - curve_scale * curve_1 * xy[0] * xy[0] - gib_scale * sin(gibPhase_1 + ang[1]) * gibMag_1
+ ))
+
+obj_rot = (obj_qw,obj_qi,obj_qj,obj_qk)
+obj_p = ((obj_px, obj_py, obj_pz), (obj_qw,obj_qi,obj_qj,obj_qk))
+
+lh_p = ((lh_px, lh_py, lh_pz), (lh_qw,lh_qi,lh_qj,lh_qk))
+sensor_pt = (sensor_x,sensor_y,sensor_z)
+#print( quatrotationmatrix(obj_rot) )
+
+reproject_params = (obj_p, sensor_pt, lh_p, phase_scale, phase_0, phase_1,
+ tilt_scale, tilt_0, tilt_1,
+ curve_scale, curve_0, curve_1,
+ gib_scale, gibPhase_0, gibPhase_1, gibMag_0, gibMag_1)
+
+def flatten_args(bla):
+ output = []
+ for item in bla:
+ output += flatten_args(item) if hasattr (item, "__iter__") else [item]
+ return output
+
+def generate_ccode(name, args, expressions):
+ flatten = []
+ if isinstance(expressions, types.FunctionType):
+ expressions = expressions(*args)
+
+ for col in expressions:
+ if hasattr(col, '_sympy_'):
+ flatten.append(col._sympy_())
+ else:
+ for cell in col:
+ flatten.append(cell._sympy_())
+
+ cse_output = cse( flatten )
+ cnt = 0
+ arg_str = lambda (idx, a): ("const FLT *%s" % str(flatten_args(a)[0]).split('_', 1)[0] ) if isinstance(a, tuple) else ("FLT " + str(a))
+ print("static inline void gen_%s(FLT* out, %s) {" % (name, ", ".join( map(arg_str, enumerate(args)) )))
+
+ for idx, a in enumerate(args):
+ if isinstance(a, tuple):
+ name = str(flatten_args(a)[0]).split('_', 1)[0]
+ for v in flatten_args(a):
+ print("\tFLT %s = *(%s++);" % (str(v), name))
+
+ for item in cse_output[0]:
+ if isinstance(item, tuple):
+ print("\tFLT %s = %s;" % (ccode(item[0]), ccode(item[1])))
+ for item in cse_output[1]:
+ print("\t*(out++) = %s;" % ccode(item))
+ print "}"
+ print ""
+
+#print(min_form)
+
+print(" // NOTE: Auto-generated code; see tools/generate_reprojection_functions ")
+print("#include <math.h>")
+
+if len(sys.argv) > 1 and sys.argv[1] == "--full":
+ generate_ccode("quat_rotate_vector", [obj_rot, sensor_pt], quatrotatevector)
+ generate_ccode("invert_pose", [obj_p], invert_pose)
+ generate_ccode("reproject", reproject_params, reproject)
+
+generate_ccode("reproject_jac_obj_p", reproject_params, jacobian(reproject(*reproject_params), (obj_px, obj_py, obj_pz, obj_qw,obj_qi,obj_qj,obj_qk)))
diff --git a/tools/showreproject/showreproject.cc b/tools/showreproject/showreproject.cc
index 98dd5f0..3eea83d 100644
--- a/tools/showreproject/showreproject.cc
+++ b/tools/showreproject/showreproject.cc
@@ -223,8 +223,8 @@ int main(int argc, char **argv) {
}
size_t showui = survive_configi(ctx1, "show-ui", SC_GET, 0);
-
- drawbsds(ctx1);
+ if (showui)
+ drawbsds(ctx1);
int waitUpdate = 100;
int SIZE = 1000;
@@ -263,6 +263,8 @@ int main(int argc, char **argv) {
cv::imshow(name, err[i]);
}
+ survive_close(ctx1);
+
std::cerr << "Error: " << error / error_count << std::endl;
int c = '\0';