Trajectory planning for monocular SLAM systems

This paper proposes a novel method of integrating planning with Monocular Simultaneous Localization and Mapping (SLAM) systems. Monocular SLAM, typically referred to as VSLAM systems in literature consists of recovering trajectory estimates of the camera and stationary world features from a single moving camera. Such VSLAM systems are significantly more difficult than SLAM performed with depth sensors, such as using an accurate Laser Range Finder (LRF). When the camera motion is subject to steep changes in orientation, tracked features over the previous instances are lost, making VSLAM estimates highly unreliable, erroneous that cannot be recovered. Most often a complete breakdown occurs, which entails a new sequence of images to be captured from a fresh camera trajectory. Herein we propose an optimization based path planning formulation for such VSLAM systems that reduces occurence of such errors through paths that are not subject to high orientation changes. Further we plan a velocity profile over the path that prevents features from getting significantly displaced over successive images, often considered a critical criteria for robust feature tracking. The velocity profile is computed using the novel concept of non linear time scaling proposed in our earlier work. The VSLAM system is also sufficiently innovated to provide for dense mapping over planar segments. The efficacy of the formulation is verified over real experiments on a camera mounted robot.