Combining Bundle Adjustment with a Digital Terrain Map: Soft and hard constraints

This paper presents an algorithm for computing a pose solution based on constraining the Bundle Adjustment algorithm with a Digital Terrain Map. The approach is motivated by the difficulties observed when using previous schemes for solving realistic problems. Some specific implementation aspects are reviewed, and in particular an algorithm for approximating the sampled DTM with a smooth surface is discussed. The paper contains a thorough simulation study and the results of an actual flight test. The implementation of the algorithm is shown to have good convergence properties, even under relatively hard scenarios, and a consistent behavior when the parameters characterizing the problem are varied.