Use ceres solver for extrinsics calibration
Using auto differentiation to solve for camera mount angle and imu bias.
Change-Id: I434f5bc7ac97acb5d18f09ec9174d79e6f5bbb06
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/runge_kutta.h b/frc971/control_loops/runge_kutta.h
index 61d674e..963f463 100644
--- a/frc971/control_loops/runge_kutta.h
+++ b/frc971/control_loops/runge_kutta.h
@@ -19,6 +19,18 @@
return X + dt / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}
+// Implements Runge Kutta integration (4th order) split up into steps steps. fn
+// is the function to integrate. It must take 1 argument of type T. The
+// integration starts at an initial value X, and integrates for dt.
+template <typename F, typename T>
+T RungeKuttaSteps(const F &fn, T X, double dt, int steps) {
+ dt = dt / steps;
+ for (int i = 0; i < steps; ++i) {
+ X = RungeKutta(fn, X, dt);
+ }
+ return X;
+}
+
// Implements Runge Kutta integration (4th order). This integrates dy/dt = fn(t,
// y). It must have the call signature of fn(double t, T y). The
// integration starts at an initial value y, and integrates for dt.