Objective Function Formulation

Jacobian w.r.t Pose Part 1

To use L-M or G-N method, need to linearize objective function

Jacobian w.r.t Pose Part 2

Jacobian w.r.t Point Part 1