Weak constraints network optimiser

We present a general framework to estimate the parameters of both a robot and landmarks in 3D. It relies on the use of a stochastic gradient descent method for the optimisation of the nodes in a graph of weak constraints where the landmarks and robot poses are the nodes. Then a belief propagation method combined with covariance intersection is used to estimate the uncertainties of the nodes. The first part of the article describes what is needed to define a constraint and a node models, how those models are used to update the parameters and the uncertainties of the nodes. The second part present the models used for robot poses and interest points, as well as simulation results.