Elmnatng Condtonally Independent Sets n Factor Graphs: A Unfyng Perspectve ased on Smart Factors Luca Carlone, Zsolt Kra, Chrs Beall, Vadm Indelman, and Frank Dellaert Astract Factor graphs are a general estmaton framework that has een wdely used n computer vson and rootcs. In several classes of prolems a natural partton arses among varales nvolved n the estmaton. A suset of the varales are actually of nterest for the user: we call those target varales. The remanng varales are essental for the formulaton of the optmzaton prolem underlyng maxmum a posteror (MAP) estmaton; however these varales, that we call support varales, are not strctly requred as output of the estmaton prolem. In ths paper, we propose a systematc way to astract support varales, defnng optmzaton prolems that are only defned over the set of target varales. Ths astracton naturally leads to the defnton of smart factors, whch correspond to constrants among target varales. We show that ths perspectve unfes the treatment of heterogeneous prolems, rangng from structureless undle adjustment to roust estmaton n SLAM. Moreover, t enales to explot the underlyng structure of the optmzaton prolem and the treatment of degenerate nstances, enhancng oth computatonal effcency and roustness. I. INTRODUCTION Future generatons of roots wll e requred to operate fully autonomously for extended perods of tme over largescale envronments. Ths goal stresses the mportance of scalalty for the estmaton algorthms supportng root navgaton. State-of-the-art technques for localzaton and mappng (SLAM) have reached a maturty that enales fast soluton of medum-szed scenaros [], [], [3], [4]. These technques are ased on a maxmum a posteror estmaton paradgm that computes the optmal state estmate y solvng a nonlnear optmzaton prolem. Whle n specfc cases t s possle to explot prolem structure and devse closed-form solutons (or approxmatons) [5], [6], general technques are ased on teratve nonlnear optmzaton. The optmal soluton of the orgnal nonlnear optmzaton prolem s computed y solvng a sequence of lnear systems (normal equatons). In real-world applcatons the state may nvolve root poses, locaton of external landmarks, and other auxlary varales (e.g., sensor ases, sensor calraton). Therefore, the optmzaton prolem to e solved s very large. Moreover, the sze of the state to e estmated grows over tme, and ths prevents long-term operaton. Ths work was partally funded y the Natonal Scence Foundaton Award 5678 RI: Small: Ultra-Sparsfers for Fast and Scalale Mappng and 3D Reconstructon on Mole Roots. L. Carlone, C. Beall, V. Indelman, and F. Dellaert are wth the College of Computng, Georga Insttute of Technology, Atlanta, GA 3033, USA, {luca.carlone,ceall3}@gatech.edu, {frank,ndelman}@cc.gatech.edu. Z. Kra s wth the Georga Tech Research Insttute, Atlanta, GA 3033, USA, zkra@gatech.edu. The ssue of scalalty recently attracted the nterest of the research communty for ts practcal relevance. In [4], Polok et al. propose ad-hoc technques for speedng-up soluton of normal equatons, explotng prolem structure. In [7], Ila et al. consder the prolem of pose graph optmzaton and propose nformaton-theoretc measures to select only the most nformatve edges, so to prevent uncontrolled growth of the graph. In [8], Stachnss and Kretzschmar propose a graph compresson technque for the specfc case of laserased SLAM. In [9], Huang et al. focus on consstency ssues and propose a node margnalzaton scheme, ased on Schur complement; moreover, they ntroduce a technque for edge sparsfcaton, ased on l -regularzed optmzaton. In [0], Carlevars-Banco and Eustce propose a sparsfcaton technque ased on margnalzaton for pose graphs. Scalalty ssues also emerge when dealng wth outler rejecton n SLAM. Outler rejecton can e addressed n the front-end (.e., efore performng nference) [], [] or the choce of removng an outlyng measurement can e part of the nference process [3]. The latter approaches are ecomng very popular as they allow the optmzaton process to dynamcally decde the est set of measurements; these approaches have een shown to e reslent to extreme percentages of outlyng measurements. Sünderhauf and Protzel [3] propose to model the choce of selectng or dscardng a measurement usng nary varales that are added to the optmzaton prolem; relaxaton s then used to solve the otherwse ntractale mxed-nteger optmzaton prolem. In ths case, the prce of roustness s the ncrease n the computatonal cost of optmzng over a large numer of latent varales (the nary varales). Olson and Agarwal [4] proposes a max-mxture model, to avod ntroducton of nary varales. Agarwal et al. [5] propose to fx the value of the relaxed nary varales, n order to crcumvent the soluton of a large optmzaton prolem. Whle recent lterature offers effectve solutons for specfc prolem nstances (e.g., pose-only graphs, sensorspecfc applcatons, outler rejecton), n ths context we are nterested n developng a general proalstc framework for varale elmnaton. The asc oservaton s that n several computer vson and rootcs prolems a natural partton arses n the set of varales. In partcular, t s possle to splt the set of varales nto two sets: a frst set, that we call target varales, contans varales that are actually requred as output of the estmaton prolem. The remanng varales, whch we call support varales, are only functonal to the estmaton prolem although they are often not requred as output of the estmator. Furthermore,
the set of support varales can often e parttoned nto susets that are condtonally ndependent gven the set of target varales. Several motvatng examples n whch ths pattern emerges are reported n Secton III. Startng from ths oservaton we model the prolem usng factor graph formalsm. We then show how to reduce the orgnal optmzaton prolem to a smaller prolem, whch only nvolves the (usually small) set of target varales, astractng support varales (mplct parameterzaton). Moreover, we show that elmnaton of the condtonally ndependent susets of support varales naturally leads to the defnton of new factors, that we call smart factors, whch susttute a (usually large) set of factors that were prevously connected to the support varales. The new factors are smart for dfferent reasons: frst of all, thanks to the mplct parametrzaton, the optmzaton prolem to e solved ecomes smaller (small numer of varales and small numer of factors) hence the ntroducton of these factors leads to a computatonal advantage; moreover, these factors can easly nclude domanspecfc knowledge, allowng the management of degenerate prolem nstances (e.g., under-constraned prolems) wthout compromsng the soluton of the overall prolem. Fnally, a smart factor contans the ntellgence to retreve a suset of the support varales, f needed, explotng the condtonal ndependence property that s satsfed y these varales. The contruton of the paper s threefold: () a tutoral contruton, as we propose a unfyng perspectve that draws connectons among technques appled to dfferent prolems and n dfferent research communtes (e.g., rootcs and computer vson); () a practcal contruton, as we show that the defnton of smart factors leads to mprovements n the estmaton process n terms of computatonal effort and roustness; () a techncal contruton, as we dscuss several elmnaton technques and we show nsghts on ther formulaton and on the underlyng approxmatons; for nstance we show the equvalence of the Schur complement (wdely used n computer vson) and the null space trck whch has een proposed n the context of EKF-ased navgaton. Moreover, we show that n rotaton graphs (graphs n whch only a rotaton matrx s assgned to each node) elmnaton can e performed nonlnearly, whle the same approach consttutes an approxmaton for pose graphs. The source code of several smart factors s avalale onlne at https://org.cc.gatech.edu/. The paper s organzed as follows. Secton II provdes prelmnares on factor graphs. Secton III descres some motvatng examples. Secton IV ntroduces the proalstc framework, the concept of mplct parametrzaton, and shows how the elmnaton of support varales leads to the defnton of smart factors. Secton V provdes vale technques for elmnaton, for each of the motvatng examples. Conclusons are drawn n Secton VI. II. FACTOR GRAPHS Factor graphs are a general representaton of estmaton prolems n terms of graphcal models [6]. A factor graph s a partte graph G = {F, Θ, E}, where F s the set of factor nodes, Θ s the set of varale nodes, and E are edges n the graph. A (gven) measurement Z s assocated to the factor node F. An (unknown) varale s assocated to each varale node n Θ. Fg. (a) provdes a pctoral representaton of a factor graph. The graph s partte, n the sense that edges may only connect a node n F to a node n Θ. A factor graph essentally encodes a densty over the varales n Θ: F P (Θ Z) = P (Θ Z ) = (Θ ) () F where each factor (Θ ) = P (Θ Z ) nvolves a suset of varales Θ Θ. Note that Bayes nets and Markov random felds can e modeled as factor graphs; moreover, factor graphs also fnd applcatons outsde the estmaton doman (e.g., constrant satsfacton prolems). In ths context we focus on those factor graphs where (Θ ) can e normalzed to a proalty dstruton,.e., (Θ )dθ = ; clearly ths can easly accommodate dscrete varales and the mxed contnuous-dscrete case. Gven a factor graph G = {F, Θ, E}, the maxmum a posteror estmate of varales n Θ s defned as: Θ = arg max Θ F (Θ ) = arg mn Θ F log (Θ ) () where (Θ ) s usually approxmated as a Gaussan densty and log (Θ ) ecomes a squared resdual error. F 3 4 5 ε A (a) x x x3 x4 B Fg.. (a) Factor graph wth 5 factors F = {,..., 5 } and 6 varale nodes Θ = {x,..., x 4,, }. In ths paper we dscuss how to elmnate a suset of the orgnal varales. The elmnaton leads to the creaton of a new graph (), whch contans a smaller numer of varales and a small numer of smart factors (factors wth the red crcle n ()). () III. MOTIVATING EXAMPLES In several computer vson and rootcs prolems a natural partton arses n the set of varale nodes Θ. In partcular, t s possle to wrte Θ = {A, B}, where A s the set of target nodes, that are actually requred as output of the estmaton prolem. The remanng nodes B are only functonal to the estmaton prolem although they are often not requred as output of the estmator; we call B the support nodes. Furthermore, n many practcal applcatons the set B naturally splts n (small) condtonally ndependent sets. In the followng we dscuss many practcal examples n rootcs and computer vson n whch one can oserve ths property. A x x x3 x4
Long-term navgaton. Consder the pose graph n Fg. (a). The root starts at pose P o and acqures odometrc measurements whch are modeled as factors connectng consecutve poses. Moreover, the root can occasonally acqure loop closng constrants (dashed lue lnes n the fgure), usng exteroceptve sensors (e.g., camera, GPS). Odometrc measurements are acqured at hgh rate (e.g., 00Hz, from an Inertal Measurement Unt). However, one s usually nterested n havng estmates at a lower frequency (say, Hz). For nstance, n the example n Fg. (a) we may only want to estmate few poses, say P 0, P, P, P 3, P 4. In our framework we treat A = {P 0, P, P, P 3, P 4 } as target varales and astract the remanng poses as support varales nto smart factors. The resultng graph s the one n Fg. (), where the smart factors are shown as red dotted lnks. We note that the choce of the target varales mples the followng condtonal ndependence relaton: P (Θ) = P (A) P ( A) (3) B where the set B (support varales) ncludes all poses that are not n A, and each suset contans the nodes along the path connectng two target varales; the susets are condtonally ndependent gven the separator nodes A. P0 P0 P3 P3 P0 P (a) P P3 P P4 P P P Fg.. Long-term navgaton example. (a) Sensor measurements are acqured at hgh rate and estmaton over the entre graph rapdly ecomes prohtve. In ths paper we defne a set of target varales A = {P 0, P, P, P 3, P 4 }, and elmnate the remanng poses (support varales) leadng to the smaller graph n (). Landmark-ased SLAM. Consder the range-ased SLAM prolem n Fg. 3. A mole root takes range measurements wth respect to external landmarks and has to solve SLAM usng these measurements and measurements of the ego-moton from odometry. In order to perform estmaton of root moton one usually solves a large optmzaton prolem nvolvng oth root poses (target varales) and the poston of external landmarks (support varales). Ths prolem can e not well-ehaved, as, for nstance, the poston of a landmark s amguous for m < 3 range measurements. If we call R the set of root poses and L = {l,..., l m } the set of landmarks postons, we have the followng condtonal ndependence relaton P (R, L) = P (R) P (l R) P0 l L P3 () P P4 P r0 r l r Fg. 3. Landmark-ased SLAM. A mole root takes range measurements wth respect to external landmarks and has to solve SLAM usng these measurements and measurements of the ego-moton from odometry. Note that n some cases the landmark poston remans amguous, e.g., from two range measurements taken from pose r 3 and r 4 the root cannot determne f the landmark l s n the poston denoted wth a red dot, or the one denoted wth the red cross. n whch each landmark s ndependent on all the other landmarks gven root poses. Exactly the same pattern emerges for the case n whch the root oserves landmarks from a vson sensor (undle adjustment n computer vson). Landmarks are only used to properly model each measurement n terms of projectve geometry, although they may not e useful for the fnal user (nstead of a sparse map of features the user may e nterested n dense map representatons whch can e easly ult from root poses). In our framework we elmnate landmarks, generatng smart factors connectng root poses. Structure from moton. In ths case the user s nterested n the structure, hence n landmark postons rather than oservaton poses. A typcal example of ths prolem s photo toursm [7], n whch one wants to reconstruct the 3D geometry of a place from a collecton of photos. In ths case the camera calraton has to e estmated, therefore we have three sets of varales: the camera poses C, the calraton matrces K, and the landmark postons L. In SFM we can oserve condtonal ndependence relatons etween dfferent sets. For example we can factorze the jont densty as: P (C, K, L) = P (C, K) l L P (l C, K) = = P (C)P (K C) l L P (l C, K) Note that the other way, elmnatng K frst, create correlaton etween all landmarks and cameras, destroyng condtonal ndependence. Outler rejecton n SLAM. Let us consder a setup n whch a mole root has to perform SLAM n presence of outlers [3], [4], [5]. In ths case, some of the measurements are dstruted accordng to a gven measurement model (say, a zero-mean Gaussan nose), whle other measurements are outlers. Recent approaches propose to ntroduce a nary varale for each measurement, wth the conventon that f the nary varale s equal to the measurement s an nler, or t s zero for outlers. Also n ths case the nares are condtonally ndependent gven graph confguraton. Moreover, the user may e not nterested n the nares, ut the estmaton prolem needs to e solved over these varales as well, as a roust soluton requres dstngushng nlers from outlers. In our framework the nary varales do not appear n the optmzaton and they are mplctly modeled nsde smart factors. r3 l x r4
IV. ELIMINATING CONDITIONALLY INDEPENDENT SETS Suppose we can dvde Θ nto A and B such that P (Θ) = P (A) P ( A) (4) B.e., the support set B can e parttoned nto susets that are condtonally ndependent gven the set A (we omt the dependence on the measurements for smplcty); we oserved n Secton III that ths pattern occurs n several practcal examples. Note that we can calculate P ( A) = ( A ) (5) I where I are the ndces of factors connected to varales n the -th support set, and A are the separators. Lkewse, P (A) = (A ) (A ) (6) I A B where I A are the ndces of the factors that do not nvolve any varale n B and each factor (A ) on the separators s formed y the sum-product : ˆ (A ) = k (A k, )d (7) k I Note that the factors (A ) only nvolve a suset of target varales A A, whch are those varales that shared some factor wth the support varales n the orgnal graph. Wth ths we get the margnal densty on A, from whch we can compute the MAP estmate of the varales n A as: A = arg max (A ) (A ) (8) A I A B Computng A nvolves solvng an optmzaton prolem over some of the orgnal factors ( (A ), wth I A ) and other new factors (A ), that we call smart factors. Prolem (8) s now defned over the set of varales of nterest, A, whle the orgnal prolem () was defned over Θ = {A, B}; ths s mportant snce typcally B A. Moreover, the numer of smart factors s equal to the numer of susets B, and ths s usually much smaller numer than the orgnal numer of factors (see Fg. ). Later n ths secton we stress that ths perspectve has advantages n terms of graph compresson (reducng the numer of varales) and also provdes tools to enhance roustness. Note that n several applcatons we are not nterested n computng the margnal proalty (6), ut we rather want to compute ts maxmum A. Ths suggests a second optmzaton-ased elmnaton approach, whch s often a etter opton wth respect to the ntegraton n (7): A = arg max I A (A ) I B (A, ) = arg max A A,B I A (A ) ( max B I B (A, ) ) = I A (A ) B (A ) (9) wth (A ) = max k I k (A k, ). In (9) we frst splt the factors ncludng support varales (factors n the set I B ) from remanng factors (set I A ); then we oserved that we can solve ndependently each su-prolem max k I k (A k, ) nvolvng a sngle set of support varales ; each optmzaton su-prolem returns a functon of A,.e., (A ) = max k I k (A k, ). Wth slght ause of notaton we use the same symol (A ) for oth elmnaton technques (sum-product and optmzatonased). Both elmnaton technques allows transformng a collecton of factors nto a small set of smart factors (A ) whose numer s equal to the numer of condtonally ndependent susets n B. In Secton V we talor these elmnaton technques to practcal examples. In partcular, we dscuss cases n whch one can perform elmnaton lnearly, nonlnearly, or va upper-ound approxmatons. Note that after computng A we can easly compute (f needed) the MAP estmate for varales n : = arg max k (A k, ) (0) k I Smart factors. We used the name smart factors for the factors (A ) arsng from the elmnaton of each suset B. The new factors are smart for dfferent reasons: frst of all, thanks to the mplct parametrzaton, the optmzaton prolem to e solved ecomes smaller (small numer of varales and small numer of factors) hence leadng to a computatonal advantage. Moreover, the elmnaton process to e carred out nsde the factor, as per (8) or (9), always has the same structure, therefore the correspondng code can e hghly optmzed and t s sutale for cache-frendly mplementatons. Furthermore, snce the ntegral (or the maxmzaton) to e solved nsde each smart factor s ndependent on the other factors n the graph, the code can therefore e hghly parallelzed. A second reason s that these factors can easly nclude doman-specfc knowledge, allowng management of degenerate prolem nstances (e.g., under-constraned prolems) wthout compromsng the overall soluton. For nstance, n vson-ased applcatons, the margnalzaton (8) has to follow a dfferent route for degenerate root moton (e.g., pure rotaton). Ths awareness, whch s hard to mantan at the factor graph level, ecomes trval f ncluded n a factor that decdes the margnalzaton approach dynamcally. Fnally, smart factors contan the ntellgence to retreve a suset of the support varales, f needed, explotng the condtonal ndependence property that s satsfed y these varales, once the target varales have een computed. In specfc applcatons, the maxmzaton (0) can e computed or approxmated n closed form, hence enalng quck retreval of the complete set of varales Θ = {A, B }. V. LINEAR AND NONLINEAR ELIMINATION: THEORY AND EXAMPLES In practce, the ntegral (or the maxmzaton) underlyng the defnton of a smart factor does not have a closed-form soluton, ut there are lucky cases n whch ths s possle. In general, t s possle to approxmate the expresson of the factor locally, or usng sutale upper ounds. We wll
dscuss several examples n whch we can explot closedform solutons, approxmatons, or resort to upper ounds. A. Elmnaton n Lnear(zed) factor graphs Let us consder the case n whch we have lnear Gaussan factors connected to the set of support varales. Then we want to compute the expresson of the smart factor (A ) = max k I k (A k, ). Accordng to standard procedures, we are rather nterested n the (negatve) logarthm of the proalty densty (A ) (see also eq. ()): log (A ) = mn log( k (A k, )) () k I For the case n whch the factors k (A k, ) are lnear Gaussan factors, we can rewrte () explctly as: mn Ha k x a +H k x r k =mn H a x a +H x r () x x k I where we stack all the susets of varales n A k, wth k I, nto a sngle column vector x a (for nstance n landmarkased SLAM ths vector may nclude all root poses from whch the same landmark s oserved); smlarly, we stack n a vector x the support varales n the set (n our landmark-ased SLAM example x may descre the poston of a sngle landmark ); fnally, H a, H are matrces descrng a lnear(zed) verson of the measurement model and r s the vector of resdual errors. From () we can easly compute the mnmum wth respect to x, as x = ( H H T H T ) (r H a x a ). Pluggng x ack nto () we get the expresson of our smart factor: log (A (I )= H ( H T H ) H ) T (H a x a r) (3) In our framework ths su-prolem s solved y the -th smart factor. We do not clam the novelty of ths dervaton: the expert reader wll notce that the elmnaton procedure appled here corresponds to the Schur complement, whch allows the solvng of lnear system n a suset of varales. Smlar results can e otaned y applyng Householder transformatons when elmnatng n a lnear factor graph. In the lnear case, the elmnaton (7) leads to the same result. Less trval s the fact that one can drectly otan the smart factor y multplyng the terms nsde the squared cost () y a matrx U, whch s untary (.e., U T U = I), and defnes a ass for the left nullspace of H : log (A )= U T (H a x a +H x r) = U T (H a x a r) (4) Whle we omt the proof here, t can e shown that (3) and (4) are equvalent. Technques ased on (4) have een used n EKF-ased navgaton, e.g., [8], [9]. When the constrants () are lnearzed nonlnear constrants, the smart factors (3) (or the equvalent verson (4)) have to e recomputed at each teraton of the optmzaton algorthm. In our smart factor perspectve we only use lnearzaton ponts for the varales n A, whle we compute the correspondng lnearzaton ponts for varales n B Orgnal Smart factors #Varales 388K 454 #Factors.65M 389K Tme 673.3s 79.3s Error 0.4% 0.7% Fg. 4. Applcaton of smart projecton factors on the Ktt dataset. The orgnal dataset has 454 poses and 389008 landmarks. The fgure shows the estmated trajectory wth standard projecton factors (sold red lne) versus the one estmated wth smart projecton factors (sold lue lne), wth translaton error reported as a percentage of dstance traveled. va (0), where we use the current lnearzaton pont nstead of A k. Ths approach, whch may seem to requre extra computaton, has een shown to mprove convergence speed [0] and avods commttng to a specfc parametrzaton of the support varales (our smart factors can dynamcally change the structure of the optmzaton prolem (0), enalng correct management of degenerate prolem nstances). Example - Smart Vson Factors: We apply the lnearzed elmnaton approach to a standard vson-ased SLAM prolem. The data s from the Ktt dataset []: a vehcle trajectory s estmated usng monocular vson. A standard approach would proceed as follows: one creates a factor graph ncludng oth vehcle poses and landmark postons. Then, each monocular oservaton s modeled as a projecton factor. The correspondng statstcs are shows n Fg. 4 (Column: Orgnal ). In our approach, each smart factor ndependently performs elmnaton of a landmark va (4); then, we only optmze a smaller graph whch nvolves vehcle poses and smart factors, see Fg. 4 (Column: Smart factors ). The same statstcs reveal a consstent computatonal advantage n usng the proposed approach. We remark that Schur complement s a common tool n computer vson [], [0]. However, smart factors do more than Schur complement: they are ale to manage degenerate prolems, explotng doman-specfc knowledge. In monocular SLAM one can have the followng degeneraces: () sngle oservaton of a landmark, () degenerate moton (e.g., pure rotaton, moton n the drecton of a landmark). In oth cases one s not ale to determne the dstance from a landmark; ths causes numercal ssues n factor graphs wth standard projecton factors snce a varale (3D poston of the landmark) s underconstraned. In our formulaton the smart factor can detect degenerate nstances when solvng the suprolem (0) to compute the lnearzaton pont for the landmarks. After the degeneracy s detected the smart factor only has to use a dfferent expresson of Jacoans n eq. (3), whle the overall elmnaton scheme remans the same. It s easy to show that n degenerate nstances, the Jacoans wll lead to rotaton-only constrants among poses.
Example - Smart Range Factors: We now resume the range-slam example of Secton III. We consder a toy example to remark that smart factors allow a etter management of degenerate prolem nstances. The example s the one n Fg. 5(a): the root starts from pose r and traverses the scenaro, reachng pose r 7. At each pose, the root takes range measurements of the poston of unknown landmarks n the scenaro (l, l, l 3 ). Moreover, t takes nosy measurement of the ego-moton (odometry). (a) Fg. 5. (a) Toy example of range SLAM. Root poses: r to r 7 ; landmark postons l to l 3. () Ground truth (sold lack lne) versus () trajectory estmated usng standard range factors (dashed red lne), and () trajectory estmated usng smart range factors (sold lue lne). In our toy example, after the frst two tme steps (r and r ) the root only has two range measurements of l, therefore the poston of the landmark s amguous (n Fg. 5(a) we draw as a red cross the other admssle poston, whle the actual one s the red dot). In factor graphs wth standard range factors, one has to e very careful to the lnearzaton pont of the landmark. If at the second tme step the landmark s ntalzed ncorrectly (to the red cross, nstead of the red dot) the trajectory estmate converges to a local mnmum (dashed red lne n Fg. 5()) and t s not ale to recover even f the remanng landmarks (l and l 3 ) are ntalzed at the correct poston. Conversely, when usng smart factors, the factor can detect degeneracy and mpose dfferent constrants among poses. The estmate usng a asc verson of the smart range factors s shown as a sold lue lne n Fg. 5(). B. Nonlnear Elmnaton If the lnear factor graphs of the prevous secton are otaned y lnearzaton, the elmnaton nsde the smart factor has to e repeated whenever the lnearzaton pont for the varales n A changes. In few lucky examples the elmnaton can e done nonlnearly, whch mples that the smart factor has to e computed only once. Example 3 - Smart Rotaton Factors: We now show that the elmnaton framework that we appled n the prevous secton n a lnear factor graph, can e appled nonlnearly, n some specfc prolem nstances. We consder a rotaton graph, whch s a factor graph n whch each varale s a rotaton matrx (n SO() or SO(3)), and the factor nodes correspond to relatve rotaton measurements among nodes pars. Fg. s an example of ths graph for the case n whch we dsregard the poston nformaton and we only consder the orentaton of each node. In the example of Fg. we defned a set of target varales, whle the support varales () corresponds to nodes along the path connectng a par of target nodes. In ths secton we want to show that the nodes n each ranch can e elmnated nto a sngle smart factor; for ths purpose, consder the ranch etween an artrary par of target nodes u and v (Fg. 6). Ru R, R,3 Rs,s+ R R3 Rs Rv Fg. 6. A ranch of a rotaton graph; target varales are n lack, support varales n red, and relatve rotaton measurements are denoted wth ar. The orgnal factor graph contans s factors along the ranch from u to v. The rotatons R u and R v are target varales. Rotatons R,..., R s are support varales and wll e elmnated. As n Secton V-A we start from the negatve logarthm of the orgnal factors, whch n the rotaton case assumes a more complex (nonlnear) form: log (A )= mn ( Log RT,+ R T R +) R,,...,R s,s+ s σ,+ wth R = R u and R s+ = R v (5) where R s the (unknown) rotaton of node =,..., s+, R,+ s the measured rotaton etween R and R + (whch s assumed to e affected y sotropc nose wth varance σ,+ ), and Log(R) denotes the logarthmc map whch returns an element of the Le algera whose exponental s R; wth slght ause of notaton Log( ) returns a rotaton vector nstead of a skew symmetrc matrx. In Appendx we prove that nonlnear elmnaton of R, wth =,..., s leads to a sngle factor on the target varales R u and R v : log (A ) = Log ( RT,s+ R T u R v ) s σ,+ (6) where R,s+ = R,... R s,s+ s the composton of the rotaton measurements n the factors along the ranch. The result may appear trval: the smart factor s nothng else than the composton of the ntermedate rotatons and the nose varance s the sum of the varances of the ntermedate measurements. However, recall that we are n a nonlnear doman, and ntutons that are true for the lnear case often fal: for nstance, the same elmnaton procedure cannot e repeated n the case wth non-sotropc nose. Also, n a pose graph, a smlar elmnaton procedure only results n an approxmaton, as we refly show n the next secton. We conclude ths secton y notng that the rotaton example s not just a toy prolem: from [5] we know that we can use rotatons to ootstrap pose estmaton and enhance convergence speed and roustness. In Fg. 7 we report an example of use of the smart rotaton factors n a sphercal scenaro, where the orgn at the ottom s fxed. The orgnal graph has 66 nodes and factors are connectng neary nodes along each merdan (Fg. 7(a)); we can convert each ranch connectng the node at the ottom wth the node at the top nto a smart factor, otanng a graph wth only nodes Fg. 7(). In the tale (Column: SO(3) ) we report the msmatch etween the rotaton estmate at the top node for the complete graph of Fg. 7(a), versus the correspondng estmate n Fg.
3 7 6 5 4 Nose free Pose Intalzaton ths case our smart factor has to compute ˆ ˇ (A ) (A ) = k (A k, )d (7) k I 3 0 3 0 3 0 3 Rotaton Nose (deg) SE(3) SO(3) 0 0 0 5 0.08 < E-7 0 0. < E-7 5 0.3 < E-7 Fg. 7. Applcaton of smart rotaton factors. (a) Orgnal graph. () Compressed graph wth smart rotaton factors. The tale reports the rotaton msmatch (norm of the rotaton error) of the top-most node n the orgnal versus the correspondng estmate n the compressed graph, for dfferent values of rotaton nose. Whle the estmates concde for rotaton graphs (column: SO(3) ), they are dfferent for pose graphs (column: SE(3) ). 7(). The msmatch s neglgle and ndependent on the rotaton nose, confrmng the dervaton n ths secton. If we repeat the same experment ncludng the full poses for each node and we consder relatve pose measurements, the estmates wll no longer match, as dscussed n the followng. Example 4 - Smart Pose Factors: Wthout gong n the mathematcal detals, t s pretty easy to see that, n a pose graph, smple pose composton s not equvalent to nonlnear elmnaton, see also [0]. For ths purpose consder Fg. 6 and magne that a pose s attached to each node, and that we want to smply compose the ntermedate poses to otan a relatve pose measurement etween node u and v. Applyng pose composton and a frst-order propagaton of the correspondng covarance matrx one may easly realze that oth the measurement etween u and v (and the correspondng covarance) wll depend on the lnearzaton pont of all the ntermedate nodes. Ths mples that, as for the lnearzed case, at every change of the lnearzaton pont, the smart factor has to e recomputed, hence the elmnaton can only e performed on a lnearzed verson of the factors. Nevertheless one may e satsfed wth an approxmaton and not recompute the smart pose factor (for small changes of the lnearzaton pont). Ths dea appeared several tmes n lterature [3], [4], [5], (whle to the est of our knowledge the correspondng elmnaton n rotaton graphs has not een nvestgated). In partcular, n [5] ntermedate pose relatons (computed from an IMU sensor) are summarzed or prentegrated nto a sngle constrant. Our C++ mplementaton of the prentegrated IMU factors, and a Matla example on the Ktt dataset [] are avalale on the weste https://org.cc.gatech.edu/. C. Upper ounds Approxmatons In several applcatons, t s dffcult to solve the optmzaton prolem (9) or to compute the ntegral (8). Therefore, one rather looks for a lower ound for (8), and teratvely optmzes ths lower ound. Ths dea s an old one [6]. In It s shown n [6] (and the reference theren) that takng the expectaton of the logarthm of k I k (A k, )d wth respect to yelds a lower ound for (A ). Therefore the followng upper ound on the negatve logarthm follows: log (A ) log ˇ (A ) = E log k (A k, )d k I (8) Ths s partcularly convenent for the case n whch the support varales are dscrete, as the expectaton can e computed as a weghted sum. Ths approach leads to standard expectaton-maxmzaton (EM) algorthms, see [6]. Example 5 - Roust SLAM: Many recent roust SLAM approaches follows the phlosophy descred n Secton III (paragraph: Outler rejecton n SLAM). An excellent example s [3]: nary varales are used to decde whether to accept or dscard a measurement; n [3] nary varales are relaxed to contnuous varales n the nterval [0, ]. Although the approach [3] greatly enhance roustness to outlers t pays the prce of optmzng over a large numer of latent varales. If the nares are relaxed to contnuous varale the elmnaton approach that we descred n Secton V- A s drectly amenale to transform the orgnal prolem nto one that only ncludes the target varales. However, n ths example, we avod relaxaton and mantan dscrete values for the support varales. Therefore, we use the elmnaton technque dscussed n Secton V-C. (a) () Fg. 8. (a) Ground truth trajectores of 3 roots movng n the same scenaro. () Tentatve correspondences etween root oservatons (lack lnes). (c) Trajectores estmated wth the elmnaton approach of Secton V-C; after optmzaton we can dstngush nlers (lack sold lnes) from outlers (cyan dashed lnes). We consder a mult root prolem n whch 3 roots traverse the same scenaro (Fg. 8(a)), wthout pror knowledge of ther ntal pose and wthout eng ale to drectly measure relatve poses durng rendezvous. Durng operaton, the roots try to estalsh possle correspondences etween oserved places, leadng to several possle relatve pose measurements (tentatve correspondences are shown n Fg. 8() as lack lnes); most of these measurements wll e outlers and correspond to false matches. We apply the elmnaton approach of Secton V-C to roustly solve the prolem, (c)
wthout explctly ntroducng nary varales. Each smart factor n ths case only computes the expectaton (8). The correspondng estmate s shown n Fg. 8(c), where after the optmzaton we can dstngush measurements classfed as nlers (lack lnes) from outlers (cyan dashed lnes). VI. CONCLUSION A natural structure emerges n several applcatons n computer vson and rootcs: the varales nvolved n the prolem can e parttoned n a set of target varales, whch are of nterest for the user, and a set of support varales that are only auxlary. Moreover, the set of support varale can e parttoned n condtonally ndependent susets. We explot ths structure to elmnate each suset of support varales. Ths elmnaton naturally leads to the defnton of new factors, that we call smart factors. Ths perspectve not only allows dramatc reducton n the sze of the prolem to e solved, ut provdes natural tools to enhance roustness when performng nference over factor graphs. APPENDIX We prove that nonlnear elmnaton of R,..., R s n (5) leads to the expresson (6). We reparametrze (5) n terms of the relatve rotatons R,+ = R T R +, wth =,..., s: log (A ) = mn Log ( ) RT,+ R,+ σ,+ R,,...,R s,s+ s suject to R,... R s,s+ = R T u R v (9) As n the lnear case, to get our smart factor, we solve for the support varales n functon of the target varales. Therefore, we compute {R,+ } from (9) and acksusttute the optmal value n the expresson of the factor. Ths prolem has een explored wth a dfferent applcaton n [6] and [7]. Therefore we know that the optmal soluton can e computed for fxed (unknown) R u and R v as: R,+ = R,+ R> Exp ( ω,+ log ( RT,s+ R T u R v )) RT > where we denote wth R > the product R+,+... R s,s+ (or the dentty matrx for = s), R,s+ = R,... R s,s+, and ω,+ = σ,+ s. As n the lnear case, we otan our σ,+ smart factor y pluggng ack the optmal soluton {R,+ } (whch s functon of R u to R v ) nto (9): s ( Log R> Exp ( ω,+ log ( RT,s+ Ru T )) ) R v RT > σ,+ where {R,+ } were computed so to satsfy the constrant R,... R s,s+ = Ru T R v [6]. Notcng that () for rotaton matrces S and S t holds Log ( ) S S S T = S Log(S ), that () the norm s nvarant to rotaton, () that the Log s the nverse of the exponental map, we prove the clam: log (A )= s ( s σ,+ σ,+) log ( RT,s+ R T u R v ) = = log ( RT,s+ R T u R v ) s σ,+ REFERENCES [] M. Kaess, H. Johannsson, R. Roerts, V. Ila, J. Leonard, and F. Dellaert, SAM: Incremental smoothng and mappng usng the Bayes tree, Intl. J. of Rootcs Research, vol. 3, pp. 7 36, Fe 0. [] K. Konolge, G. Grsett, R. Kuemmerle, W. Burgard, L. Benson, and R. Vncent, Effcent sparse pose adjustment for D mappng, n IEEE/RSJ Intl. Conf. on Intellgent Roots and Systems (IROS), Tape, Tawan, Oct 00, pp. 9. [3] R. Kümmerle, G. Grsett, H. Strasdat, K. Konolge, and W. Burgard, go: A general framework for graph optmzaton, n Proc. of the IEEE Int. Conf. on Rootcs and Automaton (ICRA), Shangha, Chna, May 0. [4] L. Polok, V. Ila, M. Solony, P. Smrz, and P. Zemck, Incremental lock cholesky factorzaton for nonlnear least squares n rootcs, n Rootcs: Scence and Systems (RSS), 03. [5] L. Carlone, R. Aragues, J. Castellanos, and B. Bona, A lnear approxmaton for graph-ased smultaneous localzaton and mappng, n Rootcs: Scence and Systems (RSS), 0. [6] G. Duelman, I. Estean, and K. Schutte, Effcent trajectory endng wth applcatons to loop closure, n IEEE/RSJ Intl. Conf. on Intellgent Roots and Systems (IROS), 00, pp. 7. [7] V. Ila, J. M. Porta, and J. Andrade-Cetto, Informaton-ased compact Pose SLAM, IEEE Trans. Rootcs, vol. 6, no., 00, In press. [Onlne]. Avalale: http://dx.do.org/0.09/tro.009.034435 [8] H. Kretzschmar and C. Stachnss, Informaton-theoretc compresson of pose graphs for laser-ased slam, Intl. J. of Rootcs Research, vol. 3, no., pp. 9 30, 0. [9] G. Huang, M. Kaess, and J. Leonard, Consstent sparsfcaton for graph optmzaton, n Proc. of the European Conference on Mole Roots (ECMR), 0. [0] N. Carlevars-Banco and R. M. Eustce, Generc factor-ased node margnalzaton and edge sparsfcaton for pose-graph SLAM, n IEEE Intl. Conf. on Rootcs and Automaton (ICRA), 03, pp. 578 5735, name n key s spelled wrong. [] J. Nera and J. Tardos, Data assocaton n stochastc mappng usng the jont compatlty test, IEEE Trans. Root. Automat., vol. 7, no. 6, pp. 890 897, Decemer 00. [] E. Olson, Recognzng places usng spectrally clustered local matches, Rootcs and Autonomous Systems, vol. 57, no., pp. 57 7, 009. [3] N. Sünderhauf and P. Protzel, Swtchale constrants for roust pose graph SLAM, n IEEE/RSJ Intl. Conf. on Intellgent Roots and Systems (IROS), 0. [4] E. Olson and P. Agarwal, Inference on networks of mxtures for roust root mappng, Intl. J. of Rootcs Research, vol. 3, no. 7, pp. 86 840, 03. [5] L. S. C. S. P. Agarwal, G.D. Tpald and W. Burgard, Roust map optmzaton usng dynamc covarance scalng, n IEEE Intl. Conf. on Rootcs and Automaton (ICRA), 03. [6] F. Kschschang, B. Frey, and H.-A. Loelger, Factor graphs and the sum-product algorthm, IEEE Trans. Inform. Theory, vol. 47, no., Feruary 00. [7] N. Snavely, S. M. Setz, and R. Szelsk, Modelng the world from Internet photo collectons, Intl. J. of Computer Vson, vol. 80, no., pp. 89 0, Novemer 008. [Onlne]. Avalale: http://phototour.cs.washngton.edu/ [8] A. Mourks and S. Roumelots, A mult-state constrant Kalman flter for vson-aded nertal navgaton, n IEEE Intl. Conf. on Rootcs and Automaton (ICRA), Aprl 007, pp. 3565 357. [9] M. L and A. Mourks, Hgh-precson, consstent EKF-ased vsualnertal odometry, Intl. J. of Rootcs Research, vol. 3, no. 6, pp. 690 7, 03. [0] Y. Jeong, D. Nster, D. Steedly, R. Szelsk, and I. Kweon, Pushng the envelope of modern methods for undle adjustment, IEEE Trans. Pattern Anal. Machne Intell., vol. 34, no. 8, pp. 605 67, 0. [] A. Geger, P. Lenz, and R. Urtasun, Are we ready for autonomous drvng? the KITTI vson enchmark sute, n IEEE Conf. on Computer Vson and Pattern Recognton (CVPR), Provdence, USA, June 0, pp. 3354 336. [] K. Konolge, Sparse sparse undle adjustment, n Brtsh Machne Vson Conf. (BMVC), Septemer 00. [3], Large-scale map-makng, n Proc. th AAAI Natonal Conference on AI, San Jose, CA, 004. [4] J. Folkesson and H. I. Chrstensen, Roust SLAM, n IAV-004, Lsoa, PT, July 5-7 004. [5] T. Lupton and S. Sukkareh, Vsual-nertal-aded navgaton for hghdynamc moton n ult envronments wthout ntal condtons, IEEE Trans. Rootcs, vol. 8, no., pp. 6 76, Fe 0. [6] T. Mnka, Expectaton-Maxmzaton as lower ound maxmzaton, Novemer 998. [7] G. Sharp, S. Lee, and D. Wehe, Multvew regstraton of 3D scenes y mnmzng error etween coordnate frames, IEEE Trans. Pattern Anal. Machne Intell., vol. 6, no. 8, pp. 037 050, 004.