Refactor ThinPlateSpline. Store N, z factorization. Move some util and move planning to lfd.
Remove duplicate tps code and fix hyperparameters in planning.py.