Initialization of 3D Pose Graph Optimization using Lagrangian duality

Initialization of 3D Pose Graph Optimization using Lagrangian duality Jesus Briales Javier Gonzalez-Jimenez 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. I. INTRODUCTION Pose Graph Optimization (PGO) is at present the most widespread formulation for Simultaneous Localization and Mapping (SLAM) in robotics, where it serves as back-end receiving the relative pose measurements produced by the front-end from the sensory data. PGO is also widely known as SE(d)-Synchronization, and it is a pervasive problem in many other fields, including computer vision and control, where closely related problems appear in the tasks of structure from motion [2], [3], extrinsic sensor calibration [4], [5], sensor network localization [6], etc. The Pose Graph Optimization problem consists in finding the trajectory that best explains all the observed relative poses. This is formulated as a high-dimensional non-convex optimization problem, which makes finding the optimal solution to a PGO instance a hard problem in general. On the other hand, local minima in the context of PGO must be avoided at any cost as these may lie arbitrarily far from the optimal solution, rendering the estimate completely useless (see Fig. 1(b)). When applied at the core of real autonomous applications, which may involve safety issues (e.g. transportation of passengers, medical interventions, etc.), failing to converge to the globally optimal solution can thus drive to unnacceptable catastrophic failures [7]. An important venue of research that has shown remarkably successful for dealing with hard optimization problem like MAPIR-UMA Group. Dept. Ingenieria de Sistemas y Automatica. Instituto de Investigacion Biomedica de Malaga (IBIMA). Universidad de Malaga, 29071 Malaga, Spain. E-mail: [email protected] This work has been supported by a University of Malaga travel grant, the Spanish grant program FPU14/06098 and the project PROMOVE (DPI201455826-R), funded by the Spanish Government and the "European Regional Development Fund". (a) groundtruth (b) chordal init. + GN (c) our init. + GN Fig. 1. Example of a challenging PGO problem with non-tight (best) Lagrangian relaxation (see [R?]). The groundtruth is corrupted with severe rotation noise. The state-of-the-art chordal initialization [8] drives to a local minimum, whereas the result from our initialization looks appealing when compared to the original groundtruth. The initialization was refined using Gauss-Newton (GN). This work exploits the Lagrangian relaxation of [1] to obtain better initializations for PGO under a larger range of conditions. PGO is that of relaxation techniques. Relaxing an optimization problem is a modeling strategy and kind of an art, although plenty of references and examples exist in the literature for many various fields. In the case of PGO, whose optimization domain is highly non-convex, the relaxation of the constraints in the original problem yields a new optimization problem that is tractable in the sense that its globally optimal solution can be attained. The most important aspect about a good relaxation is that, when properly exploited, it may provide a suboptimal yet remarkably good estimate that serves as initialization to local iterative methods (which are the standard tool in the case of PGO [9]–[11]). A good initialization not only decreases the odds of converging to local minima, but it usually enables faster convergence. Last but not least, under certain circumstances, a relaxation may even provide a certifiably globally optimal solution for the original hard problem [12]. Our present contribution is depicted in Fig. 2and adheres to this same line of research. The outcome of this work will be a well-founded recovery procedure that exploits a recently proposed Lagrangian relaxation [1] for the PGO problem and provides, depending on the case, either a very good initial guess or even the globally optimal solution for PGO. In both cases our approach outperforms the state-of-the-art alternatives, as supported by experiments in Section V. The following section provides a more exhaustive review of relaxations in the context of PGO and places our contribution therein. Several additional results and proofs are given as supplementary material (suppl. material) in [13]. Lastly, we make the code implementing the procedure available at https: //github.com/jbriales/PGO-LagInit. Non-convex constraints penalize constraints Param. by Lag. multipliers: Unconstrained Relaxed domain: Semidefinite Program solve SDP Present contribution: MLE recovery [Sec. IV] metric upgrade [Sec. IV.A]: choose most feasible relaxed solution projection [Sec. IV.B] Original domain: Lagrangian duality: Formulation [Sec. III] find best relaxation: solve best relaxation: ( equiv. solutions) ! PGO Initialization: Fig. 2. Overview of the complete pipeline in this work. Top row (from left to right, relaxation in [1]): The original hard PGO problem [P] is relaxed to the unconstrained Lagrangian relaxation [R]. The search for the best possible relaxation [R?] results in the dual problem [D]. Bottom row (from right to left, our recovery procedure): Once the dual optimal solution Λ?is known, we obtain a parameterized family of solutions ˇ Xfor the best Lagrangian relaxation. We apply a metric upgrade (Sec. IV-A) to obtain the “most feasible” solution ˇ X?and finally project (Sec. IV-B) this onto the original domain. II. RELATED WORK An important and well-known example of relaxation is the chordal relaxation in the related (and simpler) problem of rotation synchronization, also known as linear or spectral relaxation in other fields [2], [8], [14], [15]. This relaxation basically drops the constraints in the original problem yielding a simpler linear problem (or an eigenvalue problem, depending on the variant) which is easy to solve. The chordal relaxation is tight only for trivial cases1, but the suboptimal estimates obtained upon a simple recovery procedure are good enough to provide an initialization that works remarkably well for many cases in practice. The stateof-the-art initialization procedure for PGO [8] applies this relaxation on the rotational part of the problem to get a good initial estimate for the complete PGO problem. A quite recent milestone in the context of PGO has been the effective exploitation of the Lagrangian relaxation [1], [17], [18]. In the Lagrangian relaxation the constraints are substituted by penalization terms. In order to find the best Lagrangian relaxation the dual problem, which consists in a Semidefinite Program (SDP) [19], needs to be solved. This results in a remarkably improved relaxation that empirically shows to be tight for common PGO instances with intermediate noise level. In the 3D case, the Lagrangian relaxation was presented and used by Carlone et al. [18] both for performing global optimality verification and for recovering the globally optimal solution from the SDP solution, as long as the relaxation holds tight. In our previous work [1], the Lagrangian relaxation is exploited again using a different formulation of the PGO problem. The result was a much smaller dual (SDP) problem with the same tightness properties for the relaxation. This novel relaxation was used in [1] for optimality verification purposes only, providing an important gain in efficiency and speed w.r.t. the reference method in [18]. In the present work we present an appropriate recovery procedure that allows us to get a feasible estimate for the original problem from the solution of the dual (SDP) problem 1When the graph underlying the problem is balanced or a tree [16]. presented in [1]. As for [18], the obtained estimate is globally optimal if the relaxation is tight. More importantly, in the case the relaxation is not tight, our recovery procedure provides a good initialization even for very challenging problem instances (e.g. high rotational noise, low connectivity, etc.), surpassing in effectiveness the state-of-the-art chordal initialization [8]: our initialization tends to be much closer to the optimum rendering the convergence from our initial guess more robust and faster. Note that unlike the chordal initialization procedure [8], our initialization uses all the information available in the problem (both rotational and translational). The key for the success of our procedure is a sensible heuristic founded on the theory underlying the Lagrangian relaxation. In [16], a similar heuristic was exploited in the context of 2D PGO, outperforming all current 2D alternatives in convergence success. Our main contribution involves the non-trivial development of the necessary framework for exploiting this same heuristic in the case of 3D PGO. III. 3D PGO: MLE AND LAGRANGIAN RELAXATION This section provides a brief review of the Maximum Likelihood Estimation formulation for 3D PGO and the corresponding Lagrangian relaxation presented in [1]. A. Quadratic MLE formulation PGO is the problem of estimating a model consisting of nposes (Ri,ti)(the unknowns) from mrelative measurements (¯ Rij,¯ tij)(the data), both being entities in the special Euclidean group SE(3) ≡SO(3) n R3(for the 3D case). It is customary to associate this problem to a graph G(V, E), associating the unknowns to the nodes V={1, . . . , n}and the relative measurements to the edges (i, j)∈E. The best model is obtained by maximizing the consistency of the modelled poses (the nodes) with the relative pose observations (the edges). Assuming the same (well-founded) generative noise model argued by Carlone et al. in [18], namely an isotropic2Gaussian distribution for ¯ tij and an 2Isotropy is an essential condition in the present formulation, nonisotropic observations can be approximated in a fundamented way by isotropic distributions as in [20]. isotropic2Langevin distribution [21] for ¯ Rij, the Maximum Likelihood Estimate (MLE) problem takes the form f? ML = min {(Ri,ti)∈SE(3)}X (i,j)∈E ω2 ¯ tij ktj−ti−Ri¯ tijk2 2(1) +ω2 ¯ Rij kRj−Ri¯ Rijk2 F, where the objective function f(·)will be referred to as the MLE (or primal) objective. The weights ω2 ¯ tij and ω2 ¯ Rij are the scalar information and concentration parameter of the Gaussian and Langevin distribution, respectively. This MLE formulation is virtually equivalent to the more traditional formulations based on the absolute angular error (see [18] for details), but its quadratic objective makes the Lagrangian relaxation fairly simpler, yielding a Semidefinite Program (SDP) as the dual problem. B. Compact MLE formulation In our previous work [1] we obtained an equivalent compact matrix formulation for the MLE problem (1): Problem P(Primal problem: Matrix form of MLE): f? ML = min X 1 2tr(X>M X) + const.(2) s.t. X=R1,...,Rn,t1,...,tn>∈DP⊂R4n×3.(3) This problem will be also referred to as the primal problem. Both const.≥0and a 4n×4nsymmetric (positive semidefinite) matrix Mgather all the data (observations ¯ Rij and ¯ tij) in the PGO problem [1, Sec. II.A]. We will refer to the set of feasible Xpoints in the matrix reformulation (2) as the original or primal domain DP, that is, X∈DPonly if the corresponding rotation blocks X[Ri]fulfill X> [Ri]≡Ri∈SO(3) ⇐⇒ (R> iRi=I3, det(Ri) = +1.(4) No constraints apply on the translation blocks X[ti]of X, since X> [ti]≡ti∈R3. C. Lagrangian relaxation and dual problem The MLE (primal) problem [P] is hard only because of the non-convexity in the rotational constraints, rendering the Lagrangian relaxation approach a perfect candidate to obtain a good relaxation of the problem. The Lagrangian relaxation of Problem [P]3provides a whole family of relaxations parameterized by the different weightings (Lagrange multipliers Λi) employed. From here on, we use the notation Snto denote the set of n×nsymmetric matrices. Problem R(Lagrangian relaxation of [P]): d(Λ) = min ˘ X∈R4n×3 1 2tr( ˘ X> MΛ˘ X) + 1 2tr(Λ) + const.(5) where Λ= blkdiag({Λi}i∈V,0n),Λi∈S3are the Lagrange multipliers for each orthogonality constraint (4), and we define the penalized cost matrix MΛ:=M−Λ. We use 3After dropping the virtually inactive determinant constraints [22]. ˘ Xto refer to a point in the relaxed domain ˘ X∈R4n×3, in comparison to points in the original domain X∈DP. Proof: This result is obtained in [1, Sec. III]. The optimal objective d(Λ)of Problem [R] is, by definition, a lower bound on f? ML for Problem [P]: d(Λ)≤f? ML. The search for the best possible relaxation [R] is referred to as the dual problem or, more precisely in our case, the Lagrangian dual problem: Problem D(Lagrangian dual problem of [P]): d?=const.+ max Λ 1 2tr(Λ),s.t. M−Λ<0,(6) where <0denotes the matrix is Positive Semidefinite. Proof: This result is obtained in [1, Sec. III]. Upon close observation, this problem adopts the form of a (convex) Semidefinite Program (SDP) [19]. By definition of the dual problem it holds that d?≤d(Λ)≤f? ML. Note that the set of Lagrange multipliers Λi(also called dual variables) form the domain of the dual problem [D]. The best Lagrangian relaxation [R] is that obtained from the optimal solution Λ?of the dual problem [D]: Problem R?(Best Lagrangian relaxation of [P]): d?= min ˘ X∈R4n×3 1 2tr( ˘ X> M? Λ˘ X) + 1 2tr(Λ?) + const.(7) where the optimal penalized cost matrix is M? Λ=M−Λ?. Note that since d?=1 2tr(Λ?) + const.(from [D]), the optimal solution ˘ X?for [R?] fulfills tr(( ˘ X?)> M? Λ˘ X?)=0. M? Λ<0by the constraints of the dual problem [D] so tr(( ˘ X?)> M? Λ˘ X?)=0⇒M? Λ˘ X?=04n×3.(8) The relation (8) above states the equivalent condition that the optimal primal solution ˘ X?belongs to the null space or kernel N= ker(M? Λ)of the optimal penalized cost matrix M? Λ, which can be proved has rank(N)>3. An important property pointed in [1, Sec. IV] is: Proposition 1 (Nullspace condition): If d?=f? ML, that is, if the best Lagrangian relaxation [R?] is tight, then M? ΛX?=04n×3⇒X?∈ker(M−Λ?),(9) where X?and Λ?solve [P] and [D], respectively. Proof: From duality theory [23, Sec. 5], if d?=f? ML then X?, the optimal solution to [P], is also a solution to the best Lagrangian relaxation [R?]. The result (9) follows then from (8). The best Lagrangian relaxation [R?] has the remarkable property of turning tight (d?=f? ML) in a wide range of PGO problem instances, as empirically shown in [1], so the result (9) holds in many practical instances. This will be the fundamental relation that we exploit in the subsequent section for recovering a good (frequently globally optimal) guess X0∈DPfor the MLE problem [P] from the solution Λ?of the dual problem [D]. IV. RECOVERY: FROM DUAL SOLUTION TO MLE GUESS This section focuses on the development of a recovery procedure RECOVER(Λ?)7→ X0, that is, an approach that given the optimal solution Λ?of the dual problem [D], returns a good (feasible) guess X0for the original problem [P]. In fact, if the best Lagrangian relaxation [R?] is tight (d?=f? ML), the obtained guess will directly be the optimal solution for [P], that is, X?=X0. This is done exploiting the nullspace condition (9). In the general case where our relaxation is not tight (d?≤f? ML), the nullspace relation (9) does not hold and there is no way to recover the optimal solution of the original problem [P] in a certifiable way4. However, to recover a good initial guess X0∈DPfor [P] we apply a similar heuristic to Carlone et al. in [16, Sec. V.A.2] stemming from the nullspace condition (9): The optimal solution X? should be “close” to the nullspace of M? Λ. According to this heuristic, the RECOVER procedure should seek for feasible candidates X∈DPthat are close to the nullspace of M? Λ. We attain this in a two-step process: First, we look for the Lagrangian solution ˘ X?that is “most feasible” for the original problem. We will refer to this operation as a metric upgrade, for its resemblance with the metric upgrade proposed in [24]. Second, we project the chosen ˘ X?onto its “closest” feasible candidate X0∈DP. Both “close” or “most feasible” are notions that require some underlying metric to quantify the distance of a general matrix Xto the feasible domain DPof the original problem [P]. Feasibility metric: Given a point X∈DP, the constraints of the original problem apply only to the rotation blocks in X:X> [Ri]≡Ri∈SO(3). Note that R> iRi=I3⇐⇒ kR> iRi−I3k2 F= 0.(10) As a result a simple and convenient metric for the feasibility of a point Xis5 kXk2 P=1 2 n X i=1 kX[Ri]X> [Ri]−I3k2 F.(11) From here on, we will refer to k·kPas the feasibility metric. If this metric is zero, the point is feasible for [P]: kXkP= 0⇒X∈DP. Next, we briefly describe the two steps involved in the RECOVER procedure. A overview of the pipeline can be found in Algorithm 1. A. Metric upgrade of the nullspace From here on, when we refer to the nullspace Nof M? Λ, we will be considering any orthonormal basis V∈ R4n×kthat spans this nullspace6. From here on, we will use ˇ X≡˘ X?to refer to any solution of the best Lagrangian relaxation [R?]. According to (8), this family of solutions is 4For details on the optimality verification process, see [1], [18]. 5Note this metric does not account for the determinant constraint. 6A basis for ker(M? Λ)is formed by the eigenvectors corresponding to the ksmallest (zero) eigenvalues of the positive semidefinite matrix M? Λ. characterized by all possible 4n×3matrices lying inside the nullspace N= ker(M? Λ), which can be parameterized as ˇ X≡ˇ X(K) = V K ≡ { ˘ X?},K∈Rk×3.(12) Under this parameterization, the task of finding the “most feasible” point ˇ Xin the nullspace for the chosen feasibility metric (11) yields the metric upgrade problem [24]: ˇ X?= arg min ˇ X 1 2kˇ Xk2 P,s.t. ˇ X=V K.(13) The rotation blocks in ˇ Xcan be written as ˇ X[Ri]=V[Ri]K where V[Ri]stands for the i-th 3×kblock in V. For a given nullspace basis V, 1 2kˇ Xk2 P=1 2kV Kk2 P(14) =1 2 n X i=1 kV[Ri](KK>)V> [Ri]−I3k2 F≡g(KK> k×k ). The objective g(·)is referred to as the upgrade objective, and the equivalent metric upgrade problem we address is: Problem Upg (Metric upgrade of ˘ X?in [R?]): g?= min K∈Rk×3g(KK>).(15) This is an unconstrained problem, but the upgrade objective g(KK>)is a quartic polynomial7in the entries of K. Instead, we introduce the auxiliar lifted variable S=KK>∈Skand address the equivalent formulation: Problem UpgL (Lifting of metric upgrade problem [Upg]): g?= min S∈Skg(S),s.t. rank(S) = 3,S<0.(16) The equivalent lifted metric upgrade problem [UpgL] is still not trivial due to the constraints affecting S. However, its linear relaxation, g? R= min S∈Skg(S),(17) is straightforward to solve as the minimization of a quadratic function8. If the solution S? Rfor this linear relaxation (17) has 3<rank(S? R)≤k,S? Ris not feasible for the lifted metric upgrade problem [UpgL]. In this case, we compute the rank-3matrix S0closest to S? Rapplying the matrix approximation lemma9and use this as an initial estimate to perform a local iterative search on the constrained problem (16). This operation can be readily performed using optimization approaches tailored to this kind of problem, e.g. the Manopt toolbox [25]. Further details are given in the suppl. material [13]. Note that performing local optimization on the metric upgrade problem might lead to local (suboptimal) 7The minimization of a quartic polynomial like this is a non-convex problem whose resolution is not trivial. 8The details about this linear problem are given in suppl. material [13]. 9Find details about the low-rank approximation in suppl. material [13]. Algorithm 1: Recovery procedure Input: Dual solution Λ?of dual problem [6] Output: Feasible guess X0, optimality flag isOpt V←ker(M−Λ?);.Compute nullspace ˇ X?, g?←MetricUpgrade(V);.Sec. IV-A X0←Project( ˇ X?);.Sec. IV-B isTight ←(g?== 0);.Detect if optimal return X0,isOpt ←isTight solutions. Nevertheless, the empirical performance remains good in view of the experimental results. Once we attain the desired optimal rank-3estimate S?, we decompose it into S?=K?(K?)>using the SVD decomposition of S?. The product ˇ X?=V K?yields the seeked “most feasible” Lagrangian solution (13). Exact MLE recovery10:An important case is that when rank(S? R) = 3 and g? R= 0, since this evidences that the Lagrangian relaxation is tight, f? ML =d?. In this case, the upgrade output ˇ X?=V K?provides the globally optimal solution to the original problem [P]. B. Projection to the feasible set The “most feasible” candidate ˇ X?=V K?obtained through the metric upgrade is still not, in general, a feasible point for the original domain DP. To fix this, we look for the point X0∈DPclosest to the chosen ˇ X?following the same approach as in [14, Sec. III.A]. Namely, we substitute each rotation block in ˇ X?by its closest rotation matrix. To account for possible issues with reflections in the solution, we also check the projection of ˇ X?J, where J= diag(1,1,−1), and keep the best projection as the initial guess X0. V. EXPERIMENTS The objective of the experiments in this section is twofold: First, we show that for low and moderate rotation noise the Lagrangian relaxation in [1] is tight and the proposed recovery procedure is able to recover the optimal MLE solution. Second, for PGO instances which are nontight due to high rotation noise and other poor conditions of the problem we show that the initial estimate provided by our recovery procedure drives to better solutions (lower objective) than the state-of-the-art initialization in 3D PGO [8], especially in those problem regimes where the PGO problem becomes more challenging. These claims above are supported by extensive Monte Carlo analysis on simulated datasets with various topologies. The conclusions were consistent regardless of the topology so we show here the results for the grid topology only. The results for the rest of topologies can be found in the suppl. material [13]. The grid scenario is equivalent to the simulation setup employed by Carlone et al. in [18] (see Fig. 1), so we refer interested readers to that work or our suppl. material [13] for further details on this synthetic 10Find the proof for this result in suppl. material [13]. dataset generation. The list of parameters parameterizing the generated instances are the noise level both in rotation measurements (σR) and translation measurements (σt), the level of connectivity in the graph given as the probability or ratio of loop closure Pcand, finally, the number of nodes in the graph n. For each simulated PGO instance, the dual problem (6) was solved using CVX [26], providing the lower bound d?≤f? ML on the optimal MLE objective and the [D] solution Λ?. We used the latter in our recovery procedure (Alg. 1) to obtain a feasible point X0for the original problem [P]. When necessary (for non-tight relaxations) we performed Gauss-Newton (GN) refinement on this initial guess. This approach is tagged as A1. For comparison we used also the state-of-the-art chordal initialization [2], [8] followed by local iterative refinement with GN as well. We tag this approach as chordal. When refining with GN, we iterated until convergence or up to a maximum of 100 iterations. The statistics were computed over 50 runs. The performance of the methods is measured using the optimality gap ∆ = ˆ f−d?≥0, where ˆ fis the final objective value attained by the approach. For tight problems, f?=d? and ∆=0certifies a solution is optimal. Otherwise, there is no way to certify optimality but ∆a<∆bstill implies that “a” is a better estimate than “b”. The main results in Fig. 3shows the results as a boxplot superimposed with a sorted display of the true underlying data. A custom square-root-based scale [27] is used to improve the visibility of the results. The upper portion of the left Y axis above the dashed line displays extreme outliers, which are significative here as a proof of occasional cases with very bad performance. The percentage of non-tight cases11 is symbolized by H, in the right Y axis. Results: First we will focus on the results under increasing rotation noise. Previous works has extensively pointed [1], [8], [16], [18] that the tightness of the Lagrangian relaxation (6) is sensitive to rotation noise above certain threshold. This trend appears in Fig. 3(a): the relaxation is tight for low noise, then tightness quickly deteriorates after σR= 0.2rad and from there on all the problems are just non-tight. As for chordal, it reaches the optimal MLE solution for moderate noise levels, but as the problem becomes more difficult, A1 reaches significantly lower ∆than chordal, especially for σRbetween 0.3and 0.5rad. This suggests the heuristic at the core of A1 holds for a certain range of problems wich go from moderate to high rotation noise. For the rest of analyzed parameters, we fixed the rotation noise at σR= 0.3rad. At this critical point, the results show that all the parameters affect the tightness, and again the performance for A1 surpassed that of chordal. Comparison with SDP relaxation in [18]: The recovery procedure for the Lagrangian relaxation in [18] does not make any special consideration about the non-tight casuistic, often driving to poor initialization. For tight cases, both re11Optimality (in the tight case) is returned as a flag by Algorithm 1. (a) rotation noise σR(b) translation noise σt(c) connectivity Pc(d) graph size n Fig. 3. Optimality gap and average tightness vs. grid parameters. The default set of parameters is σR= 0.3rad, σt= 0.1m, Pc= 0.2, and n= 125. Left axis shows ∆after GN refinement. The points Hrefer to the percentage of non-tight cases (right axis). covery procedures provide the same optimal result. However, as shown in [1, Fig. 3], our dual problem [D] is much faster to solve by conventional interior point methods. Scalability issues: At the time of writing of this work, the solution of the dual problem [D] by interior point solvers was the main computational burden in the pipeline. These present serious scalability issues, and thus prevented the application of our Algorithm 1on large-scale datasets [8]. Luckily, very fast state-of-the-art solvers for the dual problem [D] exploiting the low-rank structure for our problem have appeared since the original submission of this work that unties the potential of the proposed procedures for application in virtually any PGO instance regardless of its size [28]. Future work will include further evaluation on large-scale problem exploring the use of these solvers. VI. CONCLUSIONS This paper proposes an effective recovery procedure that exploits the state-of-the-art formulation of the Lagrangian relaxation for PGO [1] to return the globally optimal MLE solution if the relaxation is tight, or a remarkably good initialialization for non-tight relaxations. In both cases our results match or surpass those of state-of-the-art alternatives, specially in problem instances with severe noise regimes. The performed experiments give some interesting hints about the relation between the PGO problem and different features of the graph data. We leave as future work exploring this aspect and exploiting better suited SDP solvers to apply these techniques in difficult large-scale real problems too. REFERENCES [1] J. Briales and J. González-Jiménez, “Fast Global Optimality Verification in 3D SLAM,” in Int. Conf. Intell. Robot. Syst., IEEE/RSJ, 2016. [2] D. Martinec and T. Pajdla, “Robust rotation and translation estimation in multiview reconstruction,” in Comput. Vis. Pattern Recognition, 2007. CVPR’07. IEEE Conf., pp. 1–8, IEEE, 2007. [3] M. Arie-Nachimson, S. Z. Kovalsky, I. Kemelmacher-Shlizerman, A. Singer, and R. Basri, “Global motion estimation from point matches,” in 3D Imaging, Model. Process. Vis. Transm. (3DIMPVT), 2012 Second Int. Conf., pp. 81–88, IEEE, 2012. [4] R. Gomez-Ojeda, J. Briales, E. Fernandez-Moral, and J. GonzalezJimenez, “Extrinsic calibration of a 2d laser-rangefinder and a camera based on scene corners,” in Proc. - IEEE Int. Conf. Robot. Autom., pp. 3611–3616, 2015. [5] J. Briales and J. Gonzalez-Jimenez, “A minimal solution for the calibration of a 2D laser-rangefinder and a camera based on scene corners,” in Intell. Robot. Syst. (IROS), 2015 IEEE/RSJ Int. Conf., pp. 1891–1896, sep 2015. [6] G. Piovan, I. Shames, B. Fidan, F. Bullo, and B. D. O. Anderson, “On frame and orientation localization for relative sensing networks,” Automatica, vol. 49, no. 1, pp. 206–213, 2013. [7] B. Vlasic and N. E. Boudette, “As U.S. investigates fatal Tesla crash, company defends Autopilot system.” [8] L. Carlone, R. Tron, K. Daniilidis, and F. Dellaert, “Initialization techniques for 3D SLAM: a survey on rotation estimation and its use in pose graph optimization,” in Robot. Autom. (ICRA), 2015 IEEE Int. Conf., pp. 4597–4604, IEEE, 2015. [9] R. Kummerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard, “G2o: A general framework for graph optimization,” 2011 IEEE Int. Conf. Robot. Autom., pp. 3607–3613, may 2011. [10] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. J. Leonard, and F. Dellaert, “iSAM2: Incremental smoothing and mapping using the Bayes tree,” Int. J. Rob. Res., 2011. [11] D. M. Rosen, M. Kaess, and J. J. Leonard, “RISE: An incremental trust-region method for robust online sparse least-squares estimation,” Robot. IEEE Trans., vol. 30, no. 5, pp. 1091–1108, 2014. [12] A. Bandeira, “A note on probably certifiably correct algorithms,” Comptes Rendus Math., 2016. [13] J. Briales and J. Gonzalez, “Initialization of 3D Pose Graph Optimization using Lagrangian duality - Supplementary Material.” Online. Available: mapir.isa.uma.es/jbriales/ICRA17-suppl.pdf. [14] N. Boumal, A. Singer, and P.-A. Absil, “Robust estimation of rotations from relative measurements by maximum likelihood,” in Decis. Control (CDC), 2013 IEEE 52nd Annu. Conf., pp. 1156–1161, 2013. [15] O. Ozyesil, N. Sharon, and A. Singer, “Synchronization over Cartan motion groups via contraction,” arXiv:1612.00059, nov 2016. [16] L. Carlone, G. C. Calafiore, C. Tommolillo, and F. Dellaert, “Planar Pose Graph Optimization: Duality, Optimal Solutions, and Verification,” IEEE Trans. Robot., vol. 32, no. 3, pp. 545–565, 2016. [17] L. Carlone and F. Dellaert, “Duality-based verification techniques for 2D SLAM,” in Intl. Conf. Robot. Autom. (ICRA), Accept., 2015. [18] L. Carlone, D. M. Rosen, G. Calafiore, J. J. Leonard, and F. Dellaert, “Lagrangian duality in 3D SLAM: Verification techniques and optimal solutions,” in Intell. Robot. Syst. (IROS), 2015 IEEE/RSJ Int. Conf., pp. 125–132, sep 2015. [19] L. Vandenberghe and S. Boyd, “Semidefinite programming,” SIAM Rev., vol. 38, no. 1, pp. 49–95, 1996. [20] D. M. Rosen, C. DuHadway, and J. J. Leonard, “A convex relaxation for approximate global optimization in simultaneous localization and mapping,” in Robot. Autom. (ICRA), 2015 IEEE Int. Conf., pp. 5822– 5829, IEEE, 2015. [21] A. Chiuso, G. Picci, S. Soatto, and Others, “Wide-sense estimation on the special orthogonal group,” Commun. Inf. Syst., vol. 8, no. 3, pp. 185–200, 2008. [22] R. Tron, D. M. Rosen, and L. Carlone, “On the Inclusion of Determinant Constraints in Lagrangian Duality for 3D SLAM,” in Robot. Sci. Syst. Work. “The Probl. Mob. Sensors”, 2015. [23] S. Boyd and L. Vandenberghe, Convex optimization. Cambridge University Press, 2004. [24] R. Tron, X. Zhou, and K. Daniilidis, “A Survey on Rotation Optimization in Structure From Motion,” in Proc. IEEE Conf. Comput. Vis. Pattern Recognit. Work., pp. 77–85, 2016. [25] N. Boumal, B. Mishra, P.-A. Absil, and R. Sepulchre, “Manopt, a Matlab toolbox for optimization on manifolds,” J. Mach. Learn. Res., vol. 15, no. 1, pp. 1455–1459, 2014. [26] M. Grant and S. Boyd, “{CVX}: Matlab Software for Disciplined Convex Programming, version 2.1.” \url{http://cvxr.com/cvx}, 2014. [27] C. Kleiber and A. Zeileis, “Visualizing Count Data Regressions Using Rootograms,” may 2016. [28] D. M. Rosen, L. Carlone, A. S. Bandeira, and J. J. Leonard, “A Certifiably Correct Algorithm for Synchronization over the Special Euclidean Group,” Int. Work. Algorithmic Found. Robot., nov 2016.