SAMLOS: a 2D simultaneous localization and mapping algorithm based on lines of sight

Applications that require the use of softball-sized or smaller robots impose size and power constraints that prohibit the use of active sensors such as ladar or sonar for mapping and localization. The small size of such robots also makes passive stereo vision impractical due to the limited baseline. As a result, map building capabilities for such robots will need to be based on structure from motion using monocular sequences of images. This paper presents a novel algorithm for estimation of 2-D hallway structure and robot motion given a set of feature observations from multiple images. The 2-D structure from motion algorithm is posed in a way that is linear in Cartesian coordinates given a set of camera rotations. Given an observation of a feature from a camera position, for a specified rotation of the camera´s coordinate system relative to the world there is a linear constraint that the camera´s location and the feature´s location should both lie along the line of sight between the camera and the feature. Starting with an initial set of camera orientations, the algorithm iteratively switches between refining the estimated camera and feature positions and refining the estimated camera rotations. The performance of the structure from motion algorithm is demonstrated by comparison of the algorithm results on a sequence of images to the manually measured true structure of a typical hallway.