Estimating camera pose using Bundle Adjustment and Digital Terrain Model constraints

Bundle Adjustment is the current state of the art method for solving the simultaneous localization and mapping problem. This problem is important for the localization of robots, and most acute for flying robots that cannot rely on ground odometry. The solution requires additional information to resolve scale, and most implementations either assume that relatively accurate pose information exists, or utilize GPS and IMU sensors. This paper presents an alternative approach that incorporates Digital Terrain Model constraints into the Bundle Adjustment algorithm, enabling the correct resolution of scale even in the absence of additional information. It is shown, in multiple test scenarios, that this algorithm provides estimations that do not diverge with time and that exceed, in accuracy, the resolution of the underlying sampled terrain.