Add the joint tps and relative points error calculator, its jacobian and its plotter.
Add/modify costs needed for the jointopt paper. Add jacobian orthogonality cost, simplify pose cost, add tps relative pts cost, etc.