Efficient Range-Constraint Manifold Optimization with Application to Cooperative Navigation

We present a manifold optimization approach to solve inference and planning problems with range constraints. The core of our approach is the definition of a manifold that represents points or poses with range constraints. We discover that the manifold of range-constrained points is homogeneous under the rigid transformation group action, and utilize the group action to derive the tangent space, retraction and topology of the manifold. We evaluate the performance of manifold optimization approach on solving range-constrained inference problems over state-of-the-art constrained optimization methods. The results show that manifold optimization with the range-constraint manifold achieves both faster speed and better constraint satisfaction. We further study the conditions of inference problems that we can treat range measurements as constraints in practice.