Initialization of 3D pose graph optimization using Lagrangian duality

Abstract

Abstract-Pose Graph Optimization (PGO) is the de facto choice to solve the trajectory of an agent in Simultaneous Localization and Mapping (SLAM). The Maximum Likelihood Estimation (MLE) for PGO is a non-convex problem for which no known technique is able to guarantee a globally optimal solution under general conditions. In recent years, Lagrangian duality has proved suitable to provide good, frequently tight relaxations of the hard PGO problem through convex Semidefinite Programming (SDP). In this work, we build from the state-ofthe-art Lagrangian relaxation [1] and contribute a complete recovery procedure that, given the (tractable) optimal solution of the relaxation, provides either the optimal MLE solution if the relaxation is tight, or a remarkably good feasible guess if the relaxation is non-tight, which occurs in specially challenging PGO problems (very noisy observations, low graph connectivity, etc.). In the latter case, when used for initialization of local iterative methods, our approach outperforms other state-ofthe-art approaches converging to better solutions. We support our claims with extensive experiments.

Convex Global 3D Registration with Lagrangian Duality

“…This provides particularly good approximations for many problems that can be reformulated as a Quadratically Constrained Quadratic Program (QCQP), where the relaxed problem becomes a Semidefinite Program (SDP) [4,15]. Some problems involving rotations can be characterized as QCQPs, such as Pose Graph Optimization, for which recent literature applying the Lagrangian dual relaxation has shown impressive results finding globally optimal solutions based solely on convex relaxations [11,10,7,40,8].…”

“…Because of the quadratic nature of the cost (8) it is possible to accumulate the observations, compressing all the data into a single 13 × 13 matrix termM :…”

Try to Start It! The Challenge of Reusing Code in Robotics Research

“…It is well suited for mature software packages, but the learning curve is steep, possibly not worth for experimental code. Self-triggered-mechanism [1] Matlab PGO-Laginit [2] Matlab mMath, manopt Fast-SeqSLAM [3] Matlab, C++ EKF-SLAM-on-Manifold [4] Matlab LT-Algorithm [5] Matlab lwAnn [6] Java sun-bcnn [7] Python caffe, lmdb, cv2 delta-execution-models [8] Python, C++ Visual Studio, Unreal Engine bpvo [9] C++ 11 Eigen 3.2+, OpenCV 2.11 driving-in-the-matrix [10] Linux Python CUDA 8, nvidia docker learning-forces [11] Linux C++, Python Keras, SciPy, Theano, plotly, PyOpenGL PUMP [12] Unix CUDA C rrd slam [13] Ubuntu C++ OpenCV 2.4.8 MSGD [14] macOS Sierra C++ SSM linearArray [15] Ubuntu 14.04 C++ 11 Pangolin, OpenCV, Eigen3, BLAS, LAPACK crazyswarm [16] Ubuntu 16.04 Python, C, Matlab git, swig, numpy, yaml, matplotlib jps3d [17] ROS C, C++ Eigen3, yaml-cpp autonomy hri [18] ROS C++ skimap ros [19] ROS C++ OpenMP, Eigen3, OpenCV 2.4, Boost atom mapping [20] ROS C++ VI-MEAN [21] ROS C++ OpenCV, Eigen, Ceres, OpenChisel, camodocal gps [22] ROS Python 2.7 numpy, matplotlib, scipy, boost, protobuf team acrv 2016 [23] ROS Indigo C++ MoveIt!, PCL, Baxter SDK Incremental DuDe ROS [24] ROS Indigo C++ CGAL, Freeglut, MPFR, OpenCV superquadric-grasping [25] YARP C++ IPOPT, OpenCV costar stack [26] Ubuntu 14.04 ROS Indigo C++, Python Git, Catkin Build Tools, OpenCV 2.4 nonfree human robot collaboration [27] Ubuntu 14.04 ROS Indigo gcc-4.9 NLOPT segmap [28] Ubuntu 14.04 ROS Indigo C++ python-wstool, doxygen, autoconf bayesian-object-tracking [29] Ubuntu 14.04 ROS Indigo C++ 11 Eigen 3.2.1 smartwatches apps [30] Ubuntu 14.10 ROS Indigo Java, Python Some of the previous approaches use Docker, a wellestablished container technology that has proven useful for reproducible research in many scientific domains [40]. Integration of Docke...…”

“…Self-triggered-mechanism [1] No instructions Yes PGO-Laginit [2] No instructions No Fast-SeqSLAM [3] No instructions Yes EKF-SLAM-on-Manifold [4] Undefined function or variable 'so3_log' Yes LT-Algorithm [5] Undefined function or variable 'circ_dist'…”

“…The remaining repository [2] consists of the code of a function, without any demonstration script or any other information about how to run it.…”