A Closed-Form Constraint Networks Solver for Maximum Likelihood Mapping

Maximum likelihood mapping is one of the approaches applied to simultaneous localization and mapping problems. According to such formulation map estimation corresponds to the configuration that maximizes the likelihood associated to a constraint network representing the map. Several efficient iterative fixed-point techniques have been proposed to solve this optimization problem in practice, but with no regard to the structure of the solution. In this paper, we present the closed-form solution for a generic constraint network of planar poses. The fundamental assumption concerns the form of error function and the expression of the corresponding gradient. Through algebraic manipulation, the equations relating position variables to angular parameters are derived. Furthermore, the solution is expressed by an orthogonality condition between the vector of orientation parameters and an affine transformation of the same vector. The proposed algorithm has been implemented and applied to solve the position equations of a simple constraint network.