We propose a robust framework for the planar pose graph optimization contaminated by loop closure outliers. Our framework rejects outliers by first decoupling the robust PGO problem wrapped by a Truncated Least Squares kernel into two subproblems. Then, the framework introduces a linear angle representation to rewrite the first subproblem that is originally formulated with rotation matrices. The framework is configured with the Graduated Non-Convexity (GNC) algorithm to solve the two non-convex subproblems in succession without initial guesses. Thanks to the linearity properties of both the subproblems, our framework requires only linear solvers to optimally solve the optimization problems encountered in GNC. We extensively validate the proposed framework, named DEGNC-LAF (DEcoupled Graduated Non-Convexity with Linear Angle Formulation) in planar PGO benchmarks. It turns out that it runs significantly (sometimes up to over 30 times) faster than the standard and general-purpose GNC while resulting in high-quality estimates.
翻译:本文提出了一种鲁棒的框架,用于处理被环路闭合异常值污染的平面位姿图优化问题。该框架通过将被截断的最小二乘核封装的强鲁棒PGO问题分解成两个子问题来拒绝异常值。然后,该框架引入了线性角表示来重写原本用旋转矩阵表示的第一个子问题。该框架采用Graduated Non-Convexity (GNC)算法配置,连续解决两个非凸子问题而无需初始猜测。由于两个子问题都具有线性特性,因此我们的框架只需要线性求解器来最优地解决GNC所遇到的优化问题。我们在平面位姿图基准测试中广泛验证了所提出的框架,该框架被命名为DEGNC-LAF(Decoupled Graduated Non-Convexity with Linear Angle Formulation)。结果表明,相较于标准的、通用的GNC,它运行速度显著提高(有时多达30倍),同时能够产生高质量的估计结果。