This is really interesting, thanks!
If you decide to try to make this faster, check out ceres[0] a non-linear least squares optimisation framework that does automatic differentiation using a clever C++ template hack.
I've used it a few times to solve these kind of problems and found it to be very good!