From 5b07e56094f2f765d679ac06bc07d4081338bb29 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 7 Apr 2018 09:53:26 -0600 Subject: Added statistics to sba --- tools/showreproject/showreproject.cc | 2 ++ 1 file changed, 2 insertions(+) (limited to 'tools') diff --git a/tools/showreproject/showreproject.cc b/tools/showreproject/showreproject.cc index 98dd5f0..755cb92 100644 --- a/tools/showreproject/showreproject.cc +++ b/tools/showreproject/showreproject.cc @@ -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'; -- cgit v1.2.3 From 347a479f84f124548e810d94d91175b6be43db1d Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 8 Apr 2018 15:52:22 -0600 Subject: modified showreproject tool to be less annoying --- tools/showreproject/showreproject.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'tools') diff --git a/tools/showreproject/showreproject.cc b/tools/showreproject/showreproject.cc index 755cb92..8bb91a2 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; -- cgit v1.2.3 From d46271513e6f789af0e82d4ed6628abe21e96a92 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 8 Apr 2018 15:54:07 -0600 Subject: Added jacobian to sba, ~2x speed improvement --- tools/generate_reprojection_functions/Makefile | 20 ++++ .../check_generated.c | 128 ++++++++++++++++++++ .../reprojection_functions.sage | 131 +++++++++++++++++++++ 3 files changed, 279 insertions(+) create mode 100644 tools/generate_reprojection_functions/Makefile create mode 100644 tools/generate_reprojection_functions/check_generated.c create mode 100644 tools/generate_reprojection_functions/reprojection_functions.sage (limited to 'tools') 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..a139651 --- /dev/null +++ b/tools/generate_reprojection_functions/check_generated.c @@ -0,0 +1,128 @@ +#include "survive_reproject.full.generated.h" +#include +#include +#include +#include + + +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 ") + +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))) -- cgit v1.2.3 From c5d07ca78a7d9a14c70b7bd6deaa7ccbbf48661c Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 10 Apr 2018 09:07:40 -0600 Subject: Updated tool to proper cb sig --- tools/findoptimalconfig/findoptimalconfig.cc | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) (limited to 'tools') 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 #include -#include 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); -- cgit v1.2.3