Note on nonlinear optimization

Nonlinear optimization is susceptible to becoming trapped in local minima. In order to guarantee convergence to the global minimum, the iterations must begin in some basin of attraction of the global minimum, essentially close to the correct solution.

The heuristic followed here is that by incrementally building the model and optimizing at each step, the estimate is always near the global minimum. If the model is already optimized for k robot poses and we add one more robot pose after that, initialized based on some odometry information which is at least somewhat accurate, then the new robot pose has to be close enough to the global optimum to allow the optimization to converge to the right solution.

The only part that this doesn't apply to is the very first estimate, which is made using only a few images so that the odometry can again be taken to be correct enough that the estimate of robot trajectory from odometry alone is close enough to the true trajectory that the optimization will converge.

A similar argument holds for new landmarks that are being initialized. I initialize the point by placing it at unit distance from the nominal pose of the robot when the landmark was first observed, in the direction in which it was observed. Note that this initial point is somewhat arbitrary but doesn't really make a difference to the optimization. With only one bearing measurement the error surface for the new landmark is relatively benign, consisting of an infinitely long wedge shaped valley which tapers to a point at the robot pose. As long as the point lies within the valley it is a reasonable initial guess. Once a second measurement is made, the error surface really only has one minimum point and it lies along the original valley floor (this is assuming that the second bearing measurement was not made from the exact same location, in which case it provides no new information and the error surface is still an infinitely long wedge). The initial guess as chosen here works fine in practice.


Matthew C Deans
Last modified: Fri Dec 3 14:57:51 EST 1999