Implement rotation jacobian. Debugging relative pose in standalone cpp.
Continue testing relative pose.
Merge branch 'master' of github.com:alexlee-gk/trajopt