A method of performing an iterative bundle adjustment for an imaging device is described. The method comprising implementing a plurality of functions in performing a bundle adjustment. Predetermined functions of the plurality of functions may be started using a processor for a second iteration in parallel with a first iteration of the plurality of functions. A result of the predetermined functions started during the first iteration may be used in a second iteration. An output of the bundle adjustment may then be generated for successive iterations.