diff options
| -rw-r--r-- | .gitignore | 1 | ||||
| -rw-r--r-- | include/libsurvive/survive_reproject.h | 9 | ||||
| -rw-r--r-- | src/poser_sba.c | 41 | ||||
| -rw-r--r-- | src/survive_reproject.c | 70 | ||||
| -rw-r--r-- | src/survive_reproject.generated.h | 213 | ||||
| -rw-r--r-- | tools/generate_reprojection_functions/Makefile | 20 | ||||
| -rw-r--r-- | tools/generate_reprojection_functions/check_generated.c | 128 | ||||
| -rw-r--r-- | tools/generate_reprojection_functions/reprojection_functions.sage | 131 |
8 files changed, 599 insertions, 14 deletions
@@ -20,6 +20,7 @@ simple_pose_test calinfo/ *.log *.png +*.sage.py # Windows specific *.dll diff --git a/include/libsurvive/survive_reproject.h b/include/libsurvive/survive_reproject.h index e4f21d0..e7c1745 100644 --- a/include/libsurvive/survive_reproject.h +++ b/include/libsurvive/survive_reproject.h @@ -16,6 +16,15 @@ void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, cons void survive_reproject_from_pose_with_config(const SurviveContext *ctx, struct survive_calibration_config *config, int lighthouse, const SurvivePose *pose, FLT *point3d, FLT *out); +void survive_reproject_full_jac_obj_pose(FLT *out, const SurvivePose *obj_pose, const LinmathVec3d obj_pt, + const SurvivePose *lh2world, const BaseStationData *bsd, + const survive_calibration_config *config); + +void survive_reproject_full(FLT *out, const SurvivePose *obj_pose, const LinmathVec3d obj_pt, + const SurvivePose *lh2world, const BaseStationData *bsd, + const survive_calibration_config *config); + + // 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 system // would be. diff --git a/src/poser_sba.c b/src/poser_sba.c index 1eefc80..f101018 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -45,7 +45,7 @@ typedef struct SBAData { FLT sensor_variance; FLT sensor_variance_per_second; int sensor_time_window; - + int use_jacobian_function; int required_meas; SurviveIMUTracker tracker; @@ -183,13 +183,30 @@ static void str_metric_function(int j, int i, double *bi, double *xij, void *ada 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]); + // quatnormalize(obj.Rot, obj.Rot); // std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl; SurvivePose *camera = &so->ctx->bsd[lh].Pose; - survive_reproject_from_pose(so->ctx, lh, camera, xyz, xij); + survive_reproject_full(xij, &obj, &so->sensor_locations[sensor_idx * 3], camera, &so->ctx->bsd[lh], + &so->ctx->calibration_config); +} + +static void str_metric_function_jac(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); + + SurvivePose *camera = &so->ctx->bsd[lh].Pose; + survive_reproject_full_jac_obj_pose(xij, &obj, &so->sensor_locations[sensor_idx * 3], camera, &so->ctx->bsd[lh], + &so->ctx->calibration_config); } static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, SurviveSensorActivations *scene, @@ -277,12 +294,12 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive cov, // cov data 2, // mnp -- 2 points per image str_metric_function, - 0, // jacobia of metric_func - &ctx, // user data - max_iterations, // Max iterations - 0, // verbosity - opts, // options - info); // info + d->use_jacobian_function ? str_metric_function_jac : 0, // jacobia of metric_func + &ctx, // user data + max_iterations, // Max iterations + 0, // verbosity + opts, // options + info); // info if (currentPositionValid) { // FLT distp[3]; @@ -421,6 +438,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { 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, 10.0); d->sensor_variance = survive_configf(ctx, "sba-sensor-variance", SC_GET, 1.0); + d->use_jacobian_function = survive_configi(ctx, "sba-use-jacobian-function", SC_GET, 1.0); d->so = so; SV_INFO("Initializing SBA:"); @@ -431,6 +449,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { SV_INFO("\tsba-max-error: %f", d->max_error); SV_INFO("\tsba-successes-to-reset: %d", d->successes_to_reset); SV_INFO("\tsba-use-imu: %d", d->useIMU); + SV_INFO("\tsba-use-jacobian-function: %d", d->use_jacobian_function); } SBAData *d = so->PoserData; switch (pd->pt) { diff --git a/src/survive_reproject.c b/src/survive_reproject.c index d6ba70b..921386b 100644 --- a/src/survive_reproject.c +++ b/src/survive_reproject.c @@ -1,8 +1,72 @@ #include "survive_reproject.h" -#include <../redist/linmath.h> +#include "survive_reproject.generated.h" #include <math.h> -#include <stdio.h> -#include <string.h> + +void survive_reproject_full_jac_obj_pose(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_jac_obj_p(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); +} + +void survive_reproject_full(FLT *out, const SurvivePose *obj_pose, const LinmathVec3d obj_pt, + const SurvivePose *lh2world, const BaseStationData *bsd, + const survive_calibration_config *config) { + LinmathVec3d world_pt; + ApplyPoseToPoint(world_pt, obj_pose, obj_pt); + + SurvivePose world2lh; + InvertPose(&world2lh, lh2world); + + LinmathPoint3d t_pt; + ApplyPoseToPoint(t_pt, &world2lh, world_pt); + + 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; + 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; + + for (int axis = 0; axis < 2; axis++) { + int opp_axis = axis == 0 ? 1 : 0; + + out[axis] = ang[axis]; + + if (f & SVCal_Phase) + out[axis] -= config->phase_scale * phase[axis]; + if (f & SVCal_Tilt) + out[axis] -= tan(config->tilt_scale * tilt[axis]) * xy[opp_axis]; + if (f & SVCal_Curve) + 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]; + } +} void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const survive_calibration_config *config, const SurvivePose *pose, const FLT *pt, FLT *out) { diff --git a/src/survive_reproject.generated.h b/src/survive_reproject.generated.h new file mode 100644 index 0000000..f4b499a --- /dev/null +++ b/src/survive_reproject.generated.h @@ -0,0 +1,213 @@ + // NOTE: Auto-generated code; see tools/generate_reprojection_functions +#include <math.h> +static inline void gen_reproject_jac_obj_p(FLT* out, const FLT *obj, const FLT *sensor, const FLT *lh, FLT phase_scale, FLT phase_0, FLT phase_1, FLT tilt_scale, FLT tilt_0, FLT tilt_1, FLT curve_scale, FLT curve_0, FLT curve_1, FLT gib_scale, FLT gibPhase_0, FLT gibPhase_1, FLT gibMag_0, FLT gibMag_1) { + FLT obj_px = *(obj++); + FLT obj_py = *(obj++); + FLT obj_pz = *(obj++); + FLT obj_qw = *(obj++); + FLT obj_qi = *(obj++); + FLT obj_qj = *(obj++); + FLT obj_qk = *(obj++); + FLT sensor_x = *(sensor++); + FLT sensor_y = *(sensor++); + FLT sensor_z = *(sensor++); + FLT lh_px = *(lh++); + FLT lh_py = *(lh++); + FLT lh_pz = *(lh++); + FLT lh_qw = *(lh++); + FLT lh_qi = *(lh++); + FLT lh_qj = *(lh++); + FLT lh_qk = *(lh++); + FLT x0 = tan(tilt_0*tilt_scale); + FLT x1 = lh_qi*lh_qj; + FLT x2 = lh_qk*lh_qw; + FLT x3 = x1 - x2; + FLT x4 = pow(lh_qi, 2); + FLT x5 = pow(lh_qj, 2); + FLT x6 = x4 + x5; + FLT x7 = pow(lh_qk, 2); + FLT x8 = sqrt(pow(lh_qw, 2) + x6 + x7); + FLT x9 = lh_qi*lh_qk; + FLT x10 = lh_qj*lh_qw; + FLT x11 = x10 + x9; + FLT x12 = 2*lh_px*x8; + FLT x13 = lh_qj*lh_qk; + FLT x14 = lh_qi*lh_qw; + FLT x15 = x13 - x14; + FLT x16 = 2*lh_py*x8; + FLT x17 = 2*x8; + FLT x18 = x17*x6 - 1; + FLT x19 = obj_qi*obj_qk; + FLT x20 = obj_qj*obj_qw; + FLT x21 = sensor_z*(x19 + x20); + FLT x22 = pow(obj_qi, 2); + FLT x23 = pow(obj_qj, 2); + FLT x24 = x22 + x23; + FLT x25 = pow(obj_qk, 2); + FLT x26 = sqrt(pow(obj_qw, 2) + x24 + x25); + FLT x27 = 2*x26; + FLT x28 = obj_qi*obj_qj; + FLT x29 = obj_qk*obj_qw; + FLT x30 = sensor_y*(x28 - x29); + FLT x31 = x23 + x25; + FLT x32 = obj_px - sensor_x*(x27*x31 - 1) + x21*x27 + x27*x30; + FLT x33 = 2*x32*x8; + FLT x34 = sensor_x*(x28 + x29); + FLT x35 = obj_qj*obj_qk; + FLT x36 = obj_qi*obj_qw; + FLT x37 = sensor_z*(x35 - x36); + FLT x38 = x22 + x25; + FLT x39 = obj_py - sensor_y*(x27*x38 - 1) + x27*x34 + x27*x37; + FLT x40 = 2*x39*x8; + FLT x41 = sensor_y*(x35 + x36); + FLT x42 = sensor_x*(x19 - x20); + FLT x43 = obj_pz - sensor_z*(x24*x27 - 1) + x27*x41 + x27*x42; + FLT x44 = lh_pz*x18 - x11*x12 + x11*x33 - x15*x16 + x15*x40 - x18*x43; + FLT x45 = 1.0/x44; + FLT x46 = 2*x45*x8; + FLT x47 = x13 + x14; + FLT x48 = 2*lh_pz*x8; + FLT x49 = x17*(x4 + x7) - 1; + FLT x50 = 2*x43*x8; + FLT x51 = lh_py*x49 - x12*x3 + x3*x33 - x39*x49 - x47*x48 + x47*x50; + FLT x52 = pow(x44, -2); + FLT x53 = 4*curve_0*curve_scale*x51*x52*x8; + FLT x54 = x11*x52; + FLT x55 = pow(x51, 2); + FLT x56 = curve_0*x55; + FLT x57 = pow(x44, -3); + FLT x58 = 4*curve_scale*x11*x57*x8; + FLT x59 = x1 + x2; + FLT x60 = -x10 + x9; + FLT x61 = x17*(x5 + x7) - 1; + FLT x62 = lh_px*x61 - x16*x59 - x32*x61 + x40*x59 - x48*x60 + x50*x60; + FLT x63 = pow(x62, 2); + FLT x64 = 1.0/(x52*x63 + 1); + FLT x65 = x45*x61; + FLT x66 = 2*x54*x62*x8; + FLT x67 = x64*(x65 + x66); + FLT x68 = gibMag_0*gib_scale*cos(gibPhase_0 + atan(x45*x62)); + FLT x69 = x45*x49; + FLT x70 = 2*x15*x8; + FLT x71 = x51*x52; + FLT x72 = x70*x71; + FLT x73 = 4*curve_scale*x15*x57*x8; + FLT x74 = 2*x64; + FLT x75 = x45*x8; + FLT x76 = x74*(-x15*x52*x62*x8 + x59*x75); + FLT x77 = x46*x47; + FLT x78 = x18*x71; + FLT x79 = 2*curve_scale*x18*x57; + FLT x80 = x46*x60; + FLT x81 = x52*x62; + FLT x82 = x18*x81; + FLT x83 = x64*(x80 + x82); + FLT x84 = 2*x0; + FLT x85 = obj_qj*x26; + FLT x86 = sensor_x*x85; + FLT x87 = obj_qi*x26; + FLT x88 = sensor_y*x87; + FLT x89 = sensor_z*x24; + FLT x90 = 1.0/x26; + FLT x91 = obj_qw*x90; + FLT x92 = -x41*x91 - x42*x91 + x86 - x88 + x89*x91; + FLT x93 = 2*x8*x92; + FLT x94 = obj_qk*x26; + FLT x95 = sensor_y*x94; + FLT x96 = sensor_z*x85; + FLT x97 = sensor_x*x31; + FLT x98 = -x21*x91 - x30*x91 + x91*x97 + x95 - x96; + FLT x99 = 2*x8*x98; + FLT x100 = sensor_x*x94; + FLT x101 = sensor_z*x87; + FLT x102 = sensor_y*x38; + FLT x103 = x100 - x101 - x102*x91 + x34*x91 + x37*x91; + FLT x104 = x103*x49 + x3*x99 + x47*x93; + FLT x105 = x104*x45; + FLT x106 = 4*curve_0*curve_scale*x51*x52; + FLT x107 = -x103*x70 + x11*x99 - x18*x92; + FLT x108 = x107*x71; + FLT x109 = 4*curve_scale*x107*x57; + FLT x110 = 2*x59*x8; + FLT x111 = x103*x110 - x60*x93 + x61*x98; + FLT x112 = x74*(x107*x81 + x111*x45); + FLT x113 = sensor_y*x85; + FLT x114 = sensor_z*x94; + FLT x115 = obj_qi*x90; + FLT x116 = -x113 - x114 - x115*x21 - x115*x30 + x115*x97; + FLT x117 = 2*x116*x8; + FLT x118 = obj_qw*x26; + FLT x119 = sensor_y*x118; + FLT x120 = obj_qi*x27; + FLT x121 = -sensor_z*(x115*x24 + x120) + x100 + x115*x41 + x115*x42 + x119; + FLT x122 = 2*x121*x8; + FLT x123 = sensor_z*x118; + FLT x124 = -sensor_y*(x115*x38 + x120) + x115*x34 + x115*x37 - x123 + x86; + FLT x125 = x117*x3 - x122*x47 + x124*x49; + FLT x126 = x125*x45; + FLT x127 = x11*x117 + x121*x18 - x124*x70; + FLT x128 = x127*x71; + FLT x129 = 4*curve_0*curve_scale*x55*x57; + FLT x130 = x110*x124 + x116*x61 + x122*x60; + FLT x131 = x74*(x127*x81 + x130*x45); + FLT x132 = sensor_x*x87; + FLT x133 = obj_qj*x90; + FLT x134 = -x102*x133 + x114 + x132 + x133*x34 + x133*x37; + FLT x135 = obj_qj*x27; + FLT x136 = -sensor_x*(x133*x31 + x135) + x123 + x133*x21 + x133*x30 + x88; + FLT x137 = 2*x136*x8; + FLT x138 = sensor_x*x118; + FLT x139 = -sensor_z*(x133*x24 + x135) + x133*x41 + x133*x42 - x138 + x95; + FLT x140 = 2*x139*x8; + FLT x141 = -x134*x49 + x137*x3 + x140*x47; + FLT x142 = x141*x45; + FLT x143 = x11*x137 + x134*x70 - x139*x18; + FLT x144 = x143*x71; + FLT x145 = x110*x134 - x136*x61 + x140*x60; + FLT x146 = x74*(-x143*x81 + x145*x45); + FLT x147 = obj_qk*x90; + FLT x148 = x113 + x132 + x147*x41 + x147*x42 - x147*x89; + FLT x149 = 2*x148*x8; + FLT x150 = obj_qk*x27; + FLT x151 = -sensor_x*(x147*x31 + x150) + x101 - x119 + x147*x21 + x147*x30; + FLT x152 = 2*x151*x8; + FLT x153 = -sensor_y*(x147*x38 + x150) + x138 + x147*x34 + x147*x37 + x96; + FLT x154 = x149*x47 + x152*x3 - x153*x49; + FLT x155 = x154*x45; + FLT x156 = x11*x152 - x148*x18 + x153*x70; + FLT x157 = x156*x71; + FLT x158 = x110*x153 + x149*x60 - x151*x61; + FLT x159 = x74*(-x156*x81 + x158*x45); + FLT x160 = tan(tilt_1*tilt_scale); + FLT x161 = curve_1*x63; + FLT x162 = 1.0/(x52*x55 + 1); + FLT x163 = 2*x162; + FLT x164 = x163*(x3*x75 - x51*x54*x8); + FLT x165 = gibMag_1*gib_scale*cos(gibPhase_1 - atan(x45*x51)); + FLT x166 = 4*curve_1*curve_scale*x52*x62*x8; + FLT x167 = x162*(x69 + x72); + FLT x168 = x162*(x77 + x78); + FLT x169 = 2*x160*x45; + FLT x170 = 4*curve_1*curve_scale*x52*x62; + FLT x171 = 2*x160*x52*x62; + FLT x172 = x163*(-x105 + x108); + FLT x173 = 4*curve_1*curve_scale*x57*x63; + FLT x174 = x163*(-x126 + x128); + FLT x175 = x163*(x142 - x144); + FLT x176 = x163*(x155 - x157); + *(out++) = x0*x3*x46 - 2*x0*x51*x54*x8 - x3*x53 + x56*x58 + x67*x68 - x67; + *(out++) = 2*curve_0*curve_scale*x49*x51*x52 - x0*x69 - x0*x72 + x56*x73 - x68*x76 + x76; + *(out++) = x0*x77 + x0*x78 - x47*x53 - x56*x79 - x68*x83 + x83; + *(out++) = x104*x106 - x105*x84 + x108*x84 - x109*x56 - x112*x68 + x112; + *(out++) = x106*x125 - x126*x84 - x127*x129 + x128*x84 - x131*x68 + x131; + *(out++) = -x106*x141 + x129*x143 + x142*x84 - x144*x84 - x146*x68 + x146; + *(out++) = -x106*x154 + x129*x156 + x155*x84 - x157*x84 - x159*x68 + x159; + *(out++) = 2*curve_1*curve_scale*x52*x61*x62 + x160*x65 + x160*x66 + x161*x58 + x164*x165 - x164; + *(out++) = -x110*x160*x45 + x160*x52*x62*x70 + x161*x73 - x165*x167 - x166*x59 + x167; + *(out++) = -x160*x80 - x160*x82 - x161*x79 + x165*x168 - x166*x60 - x168; + *(out++) = -x107*x171 - x109*x161 - x111*x169 - x111*x170 + x165*x172 - x172; + *(out++) = -x127*x171 - x127*x173 - x130*x169 - x130*x170 + x165*x174 - x174; + *(out++) = x143*x171 + x143*x173 - x145*x169 - x145*x170 + x165*x175 - x175; + *(out++) = x156*x171 + x156*x173 - x158*x169 - x158*x170 + x165*x176 - x176; +} + 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 <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))) |