Added jacobian to sba, ~2x speed improvement

3 files changed, 310 insertions, 14 deletions

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;
+}
+