An improved optimization method for iSAM2

There is an issue called maximum likelihood estimation in SLAM that corresponds to a nonlinear least-square problem. It is expected to earn an accurate solution for large-scale environments with high speed of convergence. Although all the applied optimization methods might be accepted in terms of accuracy and speed of convergence for small datasets, their solutions for large-scale datasets are often far from the ground truth. In this paper, a double Dogleg trust region method is proposed and adjusted with iSAM2 to level up performance and accuracy of the algorithm especially in large-scale datasets. Since the trust region methods are sensitive to their own parameters, Gould parameters are chosen to obtain better performance. Simulations are done on some large-scale datasets and the results indicate that the proposed method is more efficient compared to the conventional iSAM2 algorithm.