Goal is to find a set of poses (node values) that best solve all of the constraints. We set up a non-linear least squares problem (which can be solved with something like Gauss-Newton Method).

worldModeling