Closed gchenfc closed 2 years ago
After https://github.com/borglab/gtsam/pull/885 merges, then this will use, e.g.
Pose3 T = joint->parentTchild(q, T_H_q); Vector6 twist_p = T.Adjoint(twist_c, H_T, H_twist_c); H_q = H_T * T_H_q;
instead of:
Vector6 twist_p = T.Adjoint() * twist_c; H_q = AdjointMapJacobianQ(joint, q) * twist_c;
This is just much more sane and generalizeable and uses more GTSAM code instead of custom code.
@gchenfc I am undoing some duplicate changes from #290 since those changes are moot given the current state of master. I'm waiting for folks on Office Hours so going to use this time to land this. :)
After https://github.com/borglab/gtsam/pull/885 merges, then this will use, e.g.
instead of:
This is just much more sane and generalizeable and uses more GTSAM code instead of custom code.