LEO: Learning Energy-based Models in Factor Graph Optimization
Authors: Paloma Sodhi
Eric Dexheimer
Mustafa Mukadam
Stuart Anderson
Michael Kaess
Venue: CoRL2021 Poster (2021)
Keywords: factor graphs
energy-based learning
observation models

Abstract: We address the problem of learning observation models end-to-end for estimation. Robots operating in partially observable environments must infer latent states from multiple sensory inputs using observation models that capture the joint distribution between latent states and observations. This inference problem can be formulated as an objective over a graph that optimizes for the most likely sequence of states using all previous measurements. Prior work uses observation models that are either known a-priori or trained on surrogate losses independent of the graph optimizer. In this paper, we propose a method to directly optimize end-to-end tracking performance by learning observation models with the graph optimizer in the loop. This direct approach may appear, however, to require the inference algorithm to be fully differentiable, which many state-of-the-art graph optimizers are not. Our key insight is to instead formulate the problem as that of energy-based learning. We propose a novel approach, LEO, for learning observation models end-to-end with graph optimizers that may be non-differentiable. LEO alternates between sampling trajectories from the graph posterior and updating the model to match these samples to ground truth trajectories. We propose a way to generate such samples efficiently using incremental Gauss-Newton solvers. We compare LEO against baselines on datasets drawn from two distinct tasks: navigation and real-world planar pushing. We show that LEO is able to learn complex observation models with lower errors and fewer samples.

LEO: Learning Energy-based Models in Factor Graph Optimization
Paloma Sodhi!!, Eric Dexheimer', Mustafa Mukadam', Stuart Anderson", Michael Kaess! 'Carnegie Mellon University, Facebook AI Research
Abstract:
We address the problem of learning observation models end-to-end for estima- tion. Robots operating in partially observable environments must infer latent states from multiple sensory inputs using observation models that capture the joint distri- bution between latent states and observations. This inference problem can be for- mulated as an objective over graph that optimizes for the most likely sequence of states using all previous measurements. Prior work uses observation models that are either known a-priori or trained on surrogate losses independent of the graph optimizer. In this paper, we propose a method to directly optimize end-to-end tracking performance by learning observation models with the graph optimizer in the loop. This direct approach may appear, however, to require the inference algo- rithm to be fully differentiable, which many -tate-of-the-eee art graph optimizers are not. Our key insight is to instead formulate the problem as that of energy- based learning. We propose a novel approach, LEO, for learning observation models end-to-end with graph optimizers that may be non-differentiable. LEO alternates between sampling trajectories from the graph posterior and updating the model to match these samples to ground truth trajectories. We propose way to generate such samples efficiently using incremental Gauss-Newton solvers. We compare LEO against baselines on datasets drawn from two distinct tasks: navigation and real-world planar pushing. We show that LEO is able to learn complex observation models with lower errors and fewer samples.
Keywords: factor graphs, energy-based learning, observation models
1 Introduction
We focus on the problem of learning observation models end-to-end for estimation. Consider a robot hand manipulating an object with only tactile feedback. It must reason over sequence of touch observations over time to collapse uncertainty about the latent object pose. A common way to solve this is as an inference over a factor graph which relies on observation models that can map between states and observations [1, 2. 3]. However, in many domains, sensors that produce observations are complex and difficult to model. Can we instead learn observation models from data?
Given a batch of ground truth trajectories and corresponding measurements, how should we learn observations models? One approach would be to learn direct mapping from measurement state, for example as a regression or classification problem [4, 5, 6, 7]. However, while easy to optimize given direct supervised loss, this only minimizes surrogate loss independent of the graph opti- mizer and is not guaranteed to minimize the final tracking errors that we care about. The other option would be directly minimize final tracking errors between optimized and ground truth trajectories [8, 9]. However, many state-of-the-art factor graph optimizers, e.g. iSAM2 [10], are not natively differentiable due to operations such as dynamic re-lincarizations In such cases, we are limited to black box search for learning parameters which is very sample inefficient [11, 12, 13].
Instead of diffcrentiating through the optimization process, note that what we ultimately care about is the final solution from the optimizer, which depends only on the shape of the optimized cost function. We would like cost function that has low cost around the observed ground truth trajectories and high cost elsewhere. This is precisely what energy-basee models aim to do by
Code and supplementary material can be found on attps:://ssodee github. 1o/ leo
5th Conference on Robot Learning (CoRI. 2021), London, UK.

---

Inference (optimize X)
Learning (update )
States Factor Graph Observations
21
2/
push dona
Et 2::)
Agt
argmin R(B, 2)
E a::)
TT
Xget
Figure We show that learning observation models can be viewed as shaping energy functions that graph optimizers, even non differentiable optimize. Inference solves for most likely given model and input measurements Learning updates observation model parameters from training data.
shaping an "energy" function to be low around observed data and high elsewhere [14, 15, 16). We cast problem of learning observation models for estimation energy based learning.
We propose a novel approach, Learning Energy-based Models in Factor Graph Optimization (LEO), for learning observation models end-to-end with graph optimizers that may be non-differentiable LEO alternates between sampling trajectories from the graph posterior and updating the model to match the distribution of samples to ground truth trajectories. We show that samples efficiently using incremental Gauss-Newton graph solvers that create a Gaussian approxi- mation for the Boltzmann distribution defined over continuous space of trajectories. To the best of our knowledge, this is the first paper to show that even non- ni-differreniable factor graph models can be learned efficiently. Our main contributions are:
generate such
novel formulation of learning observation models in estimation as energy-based learning.
2. An algorithm, LEO, to learn observation models that minimize end-to-end tracking errors in factor graph optimizers that may be con-differentiable.
3. Empirical evaluation on two distinct tasks: synthetic navigation and real-world planar pushing.
Related Work
Filtering and smoothing based estimation. Observation models represent joint or conditional distributions between states and measurements. Early estimation methods utilized these models within filtering contexts such as Kalman filters and EKFs/UKFs [17, 18]. Recent methods have also looked at making these filters differentiable for end-to-end learning [19, 20, 21, 22, 23]. However, filtering can be inconsistent for nonlinear estimation problems due to lincarization based on past, marginalized states that cannot be undone [24]. Instead, a wide consensus amongst the simultancous localization and mapping (SLAM) community is to solve these problems as smoothing or nonlinear optimization objectives [3, 2]. Unlike filtering, smoothing allows dynamic relinearization of past states as well as exploits the inherent sparsity resulting in efficient and more accurate solutions. Factor graphs are an increasingly popular way for solving such smoothing problems [1, 2, 25, 4, 5, 26, 27, 28, 29], offering flexible modeling framework while being efficient to optimize. We use the factor graph based smoothing framework in this paper.
Learning for smoothing. Typically, observation models used in factor graphs are analytic models defined a-priori [30, 31, 32]. Recent work has looked at using learned models measurement representations [26, 33] either to be used independently or to be plugged into graph optimizer. However, these model parameters are learned on surrogate losses independent of the graph optimizer, and hence do not directly attempt to minimize final tracking errors.
6] or learned
An alternative approach is view observation model parameters as hyper-parameters of the graph and perform a hyper-parameter search. This is usually done as sampling and derivative-free ap-
'ee Appendix A for discussion on applying LEO differentiable optimizers.

---

proaches using black-box solvers [11, 12. 13). However, the high computational burden associated with sampling and evaluation can be prohibitive for large parameter spaces.
Differentiable optimization. Instead of a search, one may learn these model parameters via a con- tinuous gradient-based optimizer matching graph optimizer output against a desired solution. Min- imizing this loss would, however, require differentiating through the argmin optimization process. One way to do this is via unrolling where the optimization is represented as series of differentiable, gradient based updates [16, 34, 35]. Since smoothing is a nonlinear least-squares optimization, it similar fashion via differentiable Levenberg-Marquarde updates [9, 36, 37]
too can be unrolled or differentiable Geuss-Newton updates [8, 38, 39]. However, vanilla unrolling suffers from some drawbacks, notably the learned cost function can be sensitive the specific optimization procedure, e.g. number of unrolling steps [40]. Moreover, this approach cannot be readily applied to com- monly used optimizers relying on non-differentiable heuristics such as those in GTSAM, SNOPT [41, 42]. An alternate to unrolling is to employ the implicit function theorem which states that an op- timal energy function must satisfy first-order necessary conditions [43, 44]. While this promising for convex problems, for non-convex problems, one may recover an energy function with multiple minima such that the optimizer at test time may find itself in a different basin from ground truth.
Energy-based learning, Instead of differentiating through the optimization process, we note what we ultimately care about the final solution from the optimizer, which depends only on the shape of the cost function. This is what energy-based models aim to do by shaping an "energy" func- family
tion to be low around observed data and high elsewhere [14]. Energy-based modeling of unsupervised learning methods popular in many structured prediction tasks such as inverse rein- forcement learning [45, 46] and computer vision [47, 48). Recently, a number of approaches have looked at training deep energy-based models [49, 50, 16]. The challenge with such methods is gen- crating samples from the energy based model. Methods either rely on MCMC sampling [51, 52, 53] which does not scale well with dimension resort to learning separate generator [54, 55, 15J. We propose a way to generate such samples efficiently using incremental Gauss-Newtor solvers.
3 Problem Formulation
We formalize our problem of learning observation models under the general theoretical framework of energy based models. We begin by describing the observation model, how it is optimized at inference time and finally how its parameters are learned from training data (Fig. 1).
3.1 Model
We define an energy- energy-based model that captures dependencies between variables in graph by associ- ating a scalar energy FO()) to each configuration of the graph. Our goal to model the likelihood of a sequence of latent states x given sequence of measurements z. We adopt factor graph framework (Fig. 1(a)) for expressing this likelihood as?;
Po(a|2) 2(0;2) exp 1-0 2;2) 2(0;2) exp
- Σ k
(1 zZi 2W1
(I)
where, x := (x1 }T} is the sequence of latent states, and = {21... 2T) is the sequence of measurements. [[f m/: 12 are the factor costs with f(0, xx: 2x) being the local observation model mapping subser of states S x and measurements S to cost, 9 are the learnable model parameters, and 2(0; 2) J_
E(e,, .x; d) the normalization or partition function.
Note that Eq. 1 is special case of the Boltzmann distribution where the energy E(-) is sum-of- squares. Notably, when f(0, σx; 24) is linearized, E(-) is quadratic in x and subsequently P=(2)2) becomes Gaussian distribution. This is the structure that Gauss-Newton solvers employ, and we will leverage this fact to efficiently sample trajectories as described later in Section 4.3.
3.2 Inference
At inference time, given sequence of measurements 2, we wish to solve for the most likely quence of latent states r. We formulate this as maximizing the log-likelihood of the model,
ã = argmax log (x(22
(2)
We use the word likelihood as in typical learning formulations. Specifically. it refers to the posterior of states given measurements. Model parameters 0 include covariances Ee in the norm || II,,, well.
3

---

Substituting the model expression from Eq. 1, and dropping constants 2(0; and 1/2, results in nonlinear least-squares minimization of the form,
argmax log
2 exp
Σ Il
=argmnn Σ 211
We efficient online Newstn graph solver for this objective. This enables both real-time inference as well as efficient sampling for learning model parameters during training (Section 4.3).
3.3 Learning
At train time, our goal is to learn model that explains training data of pairs 2). We express this as minimizing a loss (() over the training data
(())
Σ (EED
C(0: x(0
(4)
where, (#)e:2 3 D is a training dataset of ground truth trajectories and measurements.
There are various choices of losses C(-) for learning model parameters 0,
Loss 1: Minimize final tracking loss. straightforward choice for C(-) is to directly minimize final tracking errors between optimized & and ground truth ziee trajectories, i.e. [(0; Zigt, 2) = 1/D1 E; =ggll.. While this loss directly minimizes final tracking errors, may be differ- entiable since the graph inference path from @ to å is not differentiable. In such cases, we are limited black-box hyperparameter search which is very sample inefficient.
Loss 2: Minimize surrogate loss. An alternate choice for the loss L(-) is to directly map measure-
ground truth states, for example a regression or classification problem. Such a surrogate loss is independent of graph inference, and unlike Loss 1, differentiable and easy to optimize. However, this does not minimize the final tracking errors that we about.
Loss 3: Minimize energy-based loss. Finally, we consider a loss that a function of the energy, Le. L(E(O, +); g z²). Intuitively, this assigns low loss to well-behaved energy functions, i.c. functions that give the lowest energy to training data of ground truth trajectories (correct answers) and higher energy unseen data (incorrect answers).
This energy-based based loss, unlike Loss 2, highly correlated with the final tracking loss as it shapes the energy (or cost) landscape ground truth. Moreover, unlike Loss 1, this loss is only a function of energies which differentiable irrespective of inference. Hence, we use Loss 3 and detail our approach in the next section.
make the inference step return trajectories closer to the
Approach
We instantiate the broad framework of energy- learning within the context of factor graph es- timation in 4.1 and 4.2. 4.3 we show how incremental Gauss Newton approaches enable efficient sampling from these energy based models.
4.1 Normalized Negative Log-Likelihood Loss
We define the overall loss as the negative log-like liglliklihood (NLL) of the data under the energy model?:
1
((0) =
log Po P0(2)112))
D
(5)
(as DDD
Substituting Pa as the Boltzman distribution expression from Eq. I we have,
E(0; + log / exxp(=E(0; 2')
(()) DED
Σ
(6)
'The motivation for NLL loss comes from probabilistic modeling, particularly maximum entropy moment matching. We provide more detailed derivation along with its Gaussian approximation in Appendix C.

---

Graph optimizer
Sampler
Match trajectories
2') ge
V
gi
2t, E
f
opr
DDI
opr
Training Dataset (parallel runs)
0+ - 0
opt
(Eq. 7)
Figure 2: LEO algorithm: For each training examplc, the zoomed in panel shows the three main stages: (a) solve graph optimization. (b) sample trajectories from graph posterior, and (c) compute gradients. Gradients are then pooled logether to updale parameters
Algorithm LEO algorithm
Input Initial Training data {u Fi-1 while <I convergad> do training example
p., E argmin (E(8,
Solve graph optimizer objective
8' (R', E
Sample S trajectorics from graph posterior
end
0+ + 6
Vo
V.E(0:2
b. Update model puramneters (Eq.
end while
The expression above has an intuitive explanation. The first term penalizes high energy over ground truth training samples. The second term penalizes low energy everywhere else, thus acting "contrastive" term. This has desirable effect of globally shaping the energy function so that the minima lies the training samples. The contrastive
prevents the energy surface from becom- ing flat and thus degenerate [14]. Unfortunately, we cannot directly plug in Eq. 6 as a standard machine learning loss (e.g. in Py'Torch) to optimize. This is because the second log partition term is over continuous space of trajectories which renders exact computation intractable for arbitrary nonlinear cost functions [56].
Instead, let us take the gradient of this loss, and substitute Pa(x|2²) expression from Eq. 1,
VoL(O)
Σ
E VoE(0;2,z'))
D
2
(o) JED
Po (at learned distribution samples
ground truth samples
We see the log partition term transforms into an expectation over the learned distribution. The gradient expression above has similar interpretation the loss. The first term pushes down the energy of ground truth training samples. Interestingly, the second term now pulls up the energy of samples drawn from the learned Boltzmann distribution. If one is able to generate such samples, computing the gradient is straightforward.
Generally, sampling from the continuous Boltzmann distribution is intractable, relying typically on MCMC methods. But as we will detail in Section 4.3, we can indeed generate such samples effi ciently since our graph optimizer maintains a Gaussian approximation of this distribution. Addition- ally in practice, we found that introducing temperature term T inside the Boltzmann distribution, ic. Po (2]2) exp(-1/r
2)), was critical for controlling the variance of the samples. Set- ting = corresponds to only using the mode of the distribution which in this case is the mean trajectory. This corresponds minimizing generalized perceptron loss [14].
4.2 LEO Algorithm
Algorithm summarizes the LEO algorithm, also illustrated in Fig. 2. takes as inputs the training data containing ground truth state and measurement pairs, i.e. {x/ = along with initial values for the learnable observation model parameters 0. The goal is to converge to 0 that minimizes the log-likelihood loss in Eq. 5 over the given training set i 1 I...
5

---

LEO invokes a black-box, non non-differentiable graph optimizer iSAM2 in the loop to generate sam- ples. Within each LEO iteration, for every training example we invoke the graph optimizer to obtain a Gaussian approximation Ni², ) the original Boltzmann distribution (a(x)(2) about its mode. We then sample trajectories &² M(a", Et) from this distribution.
The next step is to compute gradients of the energy, i.e. Vo =*) and Vo 1², =²), at the ground truth zizt and the samples H', respectively. Note that the gradients here only w.r.t 8, and not ø, and hence do not require the graph optimizer to be differentiable. Once we have the two gradients, we the difference between them update the observation model parameters 0. These updated parameters are then used in the next LEO iteration.
4.3 Efficient Inference via Incremental Gauss-Newton
The only thing that remains to be specified in Algorithm 1 is How do we generate samples efficiently from the graph inference? In Section 3.2 we noted that our inference algorithm must satisfy two requirements (a) real- time inference and (b) sampling from the Boltzmann distribution. We achieve this using via incremental Gauss-Newton to solve the nonlinear least squares in Eq. 3.
Gauss-Newton proceeds by linearizing observation model functions f()) about point x2.
[(0, (00,x:220 2x) = [(0, ox + 80gg zx) [0, .p: zx) + Fgò
(8)
where, Jgg 0 is the linearization point, Fk = 0f(0
is the measurement Jacobian, and 6x =
Ik - = the state update vector. Measurements & and learnable parameters 0 are treated as constant within the graph optimization step. Substituting the Taylor expansion in Eq. 8 back into Eq. 3,
6x = argmin Σ E11 - (-f(0, 2)) 112 argmin ||A6x - b112
Az
8x
where, A, b are constructed by concatenating all Fs. f(0, ~Q: =x) together into single matrix and vector respectively. We then iterate x. + x²' 6xx until convergence.
We can solve for the objective in Eq. 9 efficiently by exploiting two properties of the problem: sparsity and online measurements. For SLAM problems, both A and A A are large but sparse matrices, and can hence be factorized efficiently using sparse QR or Cholesky factorization respec- tively [57]. However, this still does not guarantee real-time inference during test time since A grows with measurements over time, making the method increasingly slower. Instead, subsequent incre- mental solvers [10, 58, 59] reuse matrix factorizations from previous time steps. In this work, we use iSAM2 [10], incremental Gauss-Newtor graph solver whose resulting MAP solution can be expressed as a Gaussian probability distribution with mean and covariance,
argmin Σ (1,
Σ (AT
-=
(10)
This is the Gaussian approximation of the original Boltzmann distribution Po(x)=") which we can now sample from efficiently. The graph posterior covariance E for such a Gauss-Newton solver is
obtained using just Jacobians A without needing to compute a Hessian [2]. Note that sampling å (ń, Σ) done at the tangent space of the linearized graph, i.e. à + f @ 8x where 8x (O E) method for inference is an efficient technique for
and ④ is a retraction [60]. The Gauss-Newan sampling from intractable models. recent overview of the Laplace approximation with Gauss- Newton method can be found in [61]. Here, we build a specific Laplace approximation using the incremental solver iSAM2 that scales to sampling efficiently real-time from very large graphs,*
5 Results and Evaluation
We evaluate LEO on synthetic navigation and real-world planar pushing. We compare against base- lines on metrics like final tracking eттor and sample efficiency. We implement LEO in Py Torch [62] and interface it with the GTSAM C++ library [41], which contains several non-differentiable factor graph optimizers. We use the iSAM2 [10] optimizer for real-time, online optimization.
'See Appendix B for time comparison other samplers.

---

Planar Pushing Setup
Tactile Factor Network
Factor Graph
21
Leatted Tactile
Figure 3: (a) Experimental setup for real-world planar pashing. (b) Learned tactile factor (red) penalizes deviations between predicted relative from Lactile factor network and estimated relative using variable estimates in the graph.
5.1 Overview
Experimental setup: For synthetic navigation (Datasets NI-N4), the goal is to estimate latent robot poses T2 (2) from relative odometry and absolute GPS measurements. For real-world planar pushing (Datasets P1-P3), the goal is to estimate latent object poses Og E (2) from tactile image measurements using DIGIT tactile sensor [63] shown in the setup in Fig. 3. Both these problems are modeled as factor graphs with poses as variable nodes and measurements as factor nodes.
Observation model parameters: Recall factor costs defined by the local observation model f()) and learned model parameters 6. Since we use Gaussian factors, we will learn their mean and covariance parameters, 0 = {u,E), E). For the navigation task,
learn (a) fixed co- variances := { (Eodom: Egps } (N1. N2), and (b) covariances as function of measurements 0 {Indom (2). E [2)} (N3, N4). For the planar pushing task, learn (a) fixed covari- ances = (Staes 2gs} (P1, P2), and (b) means as function of tactile image measurements diagonal covariance model.
0 {seac(=)) (=)} (P3). For all experiments,
Baselines: We compare against a learned sequence model such as LSTM, and black-box hyperpa- rameter search methods such as CMAES [11] and Nelder-Mead all of which optimize Loss 1. We also compare against a surrogate supervised learning method [4] which optimizes Loss 2.
Appendix D provide more details on the factor graph models, setup, and additional results.
5.2 End-to-end Tracking Errors
Fig. 4 shows final tracking results for navigation datasets. We initialize LEO with random such that the graph optimizer returns trajectories far from the ground truth. At the final iteration, LEO converges a that matches the optimized trajectories ground truth. We summarize quantitative performance against baselines minimizing final tracking loss (Loss 1). LEO consistently outper- forms baselines on final rmse tracking errors. The difference is more pronounced for datasets N3, N4 where the varying covariance model has higher dimensional parameter space.
Fig. 5 shows final tracking results for real-world planar pushing datasets. Similar navigation, show LEO converging to ground truth trajectory for two different objects, a rectangle and ellipse, with differing local contact patches. We report quantitative performance datasets Pl, P2. LEO outperforms different hand-tuned choices of covariances as used in prior work [4].
Datzeet NO
tranleunaa
LSTM
802
1009
NMool
1007
lacs
84%
LBO
Dataset
Dataset N2
Dataset
Dataset
Figure 4: Synthetic Navigation Datasets: Comparison of I.FOO against haselines. Each dataset I--N4 consists of 50 varying trajectories with a 30/20 train/est split. Translational and rotational trajectory tracking errors represent average RMSE over the test set, fovals is number of black-boo graph optimizer calls computed per data point during training.
7

---

Pure
Datustt
object
LEO Iten: 0
LEO Iecs. 2
LEO Iee:: 10
LEO Iter: 20
New Syp
LEO
I0E00S (2UA))
Killin dejeck
Figure 5: Real-world Planar Pushing Datasets: Fach dataset Pl, P2 consists of 15 varying trajectories with 1055 train/test split for rect,
ellipse object respectively. Translational rotational trajectory tracking errors represent average over the set.
LEO Loss
iter:
LEO Iter:
LEO iter.
LEO iter: 15
(a)
(b)
Figure 6: (a) LEO convergence for varying initial on dataset N1. (b) Evolution of graph optimizer trajectory sumples over LEO iterations.
12
ladid Joueciaa
Datawe: P3
cape losta [sual
S- MraclE)
case
Surrggate Eactor loes LDO loos
LEO initial iteration.
LEO final ineration
Time ax
(b)
Figure T: (a) Analysis showing that a surrogate loss, independent of the graph optimizer, may not minimize end to end tracking errors. (b)
Minimizing only surrogate loss results higher final tracking error than minimizing the loss.
5.3 Sample Efficiency and Convergence
Black-box hyper-paramede parameter search is sample inefficient and does not scale well with increasing dimensionality. LEO, on the other hand, is efficient it uses gradients. This can be seen in Fig. 4, where LEO consistently has lower graph optimizer calls (fevals). Moreover, LEO can also be used to learn models with large parameter spaces, e.g. Fig. 7 learns parameters for tactile factor neural network mapping image features to transformations.
Finally, Fig. 6 shows LEO consistently converging for varying initializations on navigation dataset NI. We further show how the LEO samples evolve over iterations. Initially, when the cost incorrect, the samples are more spread out. Over iterations, as the cost converges, the samples concentrate about the ground truth trajectory.
5.4 Surrogate Loss Analysis
We further analyze why surrogate loss (Loss 2), independent of the graph optimizer, may not minimize end-to-end tracking errors. We train a network on surrogate supervised learning loss to directly map pair of tactile image features to ground truth poses similar to prior work [4]. Fig. 7(b) shows the per time step surrogate loss on two trajectories - an initial trajectory that deviates from ground truth, and final converged trajectory. While the tracking emror of final is much less than initial, the surrogate loss does not strictly improve. This is further reflected in the table in Fig. 7(c) where the surrogate loss baseline has higher final tracking errors over LEO for dataset PI.
Conclusion
We presented an approach, LEO, for learning observation models end-to-end with graph optimizers that may be non-di -in-difeerntiialle. We compared LEO against baselines for different observation model types on two distinct tasks of navigation and real-world planar pushing. Overall, LEO is able to learn observation models with lower errors and fewer samples. While we investigated LEO with non-differentiable optimizers this paper, as future work, we would like to apply LEO to differ- entiable optimizers and compare against alternate solutions such unrolling (see Appendix A for a more detailed discussion). We would also like scale to more complex observation models with applications to 3D object tracking using tactile sensors.

---

Acknowledgements
This work was supported through the Facebook FRAIM program. We thank Frank Dellaert for insightful feedback and suggestions the paper.
References
F. Dellaert. Factor graphs: Exploiting structure in robotics. Annual Review Control, Robotics, and Autonomous Systems, 4, 2020.
F. Dellaert and M. Kaess. Factor graphs for robot perception. Foundations and Trends in Robotics.
[3] C. Cadena, L. Carlone, H. Carrillo, Y. Latif, D. Scaramuzza, Neira, L. Reid, and J. J. Leonard. Past, present, and future of simultaneous localization mapping: Toward the pabust-pereeption age. JEEE Trans, Robotics, 2016.
[4] P. Sodhi, Kaess, Mukadam, and Anderson. Learning lactile models for factor graph-based estimation. In IEEE Intl. Conf. on Robotics and Automation (ICRA), 2021.
[5] A. Baikovitz, P. Sodhi, M. Dille, and M. Kaess. Ground encoding: Learned factor graph- models for localizing ground penetrating radar. In IEEE/RSJ Intl. Conf. on Intelligent Robots Systems (IROS), 2021.
[6] B. Sundaralingam, A. S. Lambert, A. Handa, B. Boots, T. Hermans, S. Birchfield, N. Ratliff, and D. Fox. Robust learning of tactile force estimation through robot interaction. In IEEE Intl. Conf. on Robotics and Automation (ICRA), 2019.
[7] A. Lambert, A. Shaban, A. Raj, Z. Liu, and B. Boots. Deep forward and inverse perceptual models for tracking and prediction. In IEEE Intl. Conf. Robotics and Automation (ICRA), pages 675-682, 2018.
[8] B. Yi. M. Lee, A. Kloss, R. Martin-Marte and J. Bohg. Differentiable factor graph optimization for learning smoothers. arXiv preprint arXiv: 105.0207,, 2021.
[9] K. M. Jatavallabhula, G. Iyer, and L. Paull. GradSLAM: Dense SLAM meets automatic differentiation. In IEEE Intl. Conf. on Robotics and Automation (ICRA). 2020.
[10] M. Kaess, H. Johannsson, R. Roherts, lla, J. Leonard, and Dellaert. iSAM2: Incremental smoothing and mapping using the Bayes tree. Intl. J. of Robotics Research, 31(2):216-235 Feb. 2012.
[11] N. Hansen,
Akimoto, and Baudis. 101::00.588/zzeodoo 2559634, 2019. URL https:/s://dd h http///oooogg//o//oo/// /00 22222///zeooo2222
CMA-ES/pycma on Github. Zenodo,
[12] H. Hu and G. Kantor. Parametric covariance prediction for hetcroscedastic noise. IEEE/RSI Intl. Conf. on Intelligent Robots and Systems (IROS), 2015.
[13] Snoek, H. Larochelle, and R. P. Adams. Practical bayesian optimization of machine learning algorithms. arXiv preprint arXiv:1206.2944 2012.
[14] Y. LeCun, Chopra, R. Hadsell, M. Ranzato, and E. Huang. tutorial on energy-based learning. Pre- dicting structured data, 1(0), 2006.
[15] R. Kumar, S. Ozair, A. Goyal, A. Courville, and Bengio. Maximum entropy generators for energy- based models. arXiv preprint / 2019.
[16] D. Belanger, Yang, and A. McCallum. End-to-un end learning for structured prediction energy networks. In International Conference on Machine Learning (ICML), 2017.
[17] S. J. Julier and J. K. Uhlmann. Unscented filtering and nonlinear estimation. Proceedings of the IEEE, 92(3):401 4222,, 2004.
[18] S. Thrun, D. Fox, W. Burgard, and F. Dellaert. Artificial intelligence, 128(1-2)-9)) (2)19))999111, 2001.
Robust Monte Carlo localization for mobile robots.
[19] A. Kloss, G. Martius, and J. Bohg. How to train your diffcrentiable filter. arXiv preprint 20X2 2020.
[20] M. A. Lee, B. Yi. R. Martin-Martin, S. Savarese, and J. Bong. Multimodal sensor fusion with differen- tiable filters. In JEEE/RSJ Intl. Conf. Intelligent Robots and Systems (IROS), 2020.

---

[21] R. Jonschkowski, D. Rastogi, and O. Brock. Differentiable particle filters: End-to-end learning with algorithmic priors. arXiv preprini arXiv:/805.11122. 2018.
[22] W. Sun, A. Venkatraman, B. Boots, and A. Bagnell. Learning filter with predictive state inference machines. In International Conference on Machine Learning (ICML), 2016.
[23] T. Haarnoja, A. Ajay, Levine, and P. Abbeel. Backprop kf: Learning discriminative deterministic state estimators. In Advances in Neural Information Processing Systems 24 (NewrIPS), 2016.
[24] S. J. Julier and J. K. Uhlmann. A counter example to the theory of simultaneous localization and map building. In JEEE Intl. Conf. on Robotics and Automation (ICRA), volume 4, pages
2001.
[25] J. Ortiz, T. Evans, and A. J. Davison. A visual introduction to gaussian belief propagation. arXiv preprint vrXiv:2107.02300 2021.
[26] J. Czarnowski, T. Laidlow, R. Clark, and A. J. Davison. DeepFactors: Real-time probabilistic dense monocular SLAM. IEEE Robotics and Automation Letters, 2020.
[27] R. Hartley, M. Jadidi, L. Gan, J.-K. Huang. J. W. Grizzle, and R. M. Eustice. Hybrid contact preintegra- tion for Souul--nertial-conkee state estimation using factor graphs. In JEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS), pages 3783-3790, 2018.
[28] E. Westman and M. Kaess. Degeneracy-awwe aware imaging sonar simultaneous localization and mapping. Journal of Oceanic Engineering, 2019.
129] D. Fourie, J. Leonard, and M. Kacss. A nonparametric belief solution to the bayes tree. In JEEE/RSJ Intl. Conf. Intelligent Robots and Systems (IROS), 2016.
[30] R. Mur-Artal and J. D. Tardós. ORB-SLAM2: An open-source SLAM system for monocular, stereo, and RGB-D cameras. IEEE Trans. Robotics, 33(5):1255-12622 2017.
[31] J. Engel, T. Schöps, and D. Cremers. LSD-SLAM: Large-scale direct monocular SLAM. In Eur. Conf. on Computer Vision (ECCV), pages 834-849. Springer. 2014.
[32] A. Lambert, M. Mukadam, B. Sundaralingam, N. Ratliff, Boots, D. Fox. Joint inference kine- matic and force trajectories with visuo-tactile sensing. In IEEE Intl. Conf. on Robotics and Automation (ICRA), pages 1171, 2019.
[33] M. Bloesch, J. Czarnowski, R. Clark, S. Loutenegger, and A. Davison. CodeSLAM-lcamingg compact, optimisable representation for dense visual slam. In Proc. IEEE Int. Conf. Computer Vision and Pottern Recognition, 2018
[34] B. Wilder, F. Ewing, B. Dilkina, and M. Tambe. End end learning and optimization on graphs. Ad- vances in Neural Information Processing Systems, 32:4672 4683, 2019.
[35] S. Bechtle, A. Molchanov, Y. Chebotar, E. Grefenstette, L. Righetti, Sukhatme, and Meier. Meta learning via learned loss. In 2020 25th International Conference on Puttern Recognition (ICPR), 2021.
[36] Z. Lv, F. Dellaert, J. M. Rehg, and A. Geiger. Taking a deeper look at the inverse compositional algorithm. In Proc. IEEE Int. Conf. Computer Vision and Patiern Recognition, 2019.
[37] C. Tang and P. Tan. Ba-net: Dense bundle adjustment networks. In International Conference on Learning Representations (ICLR), 2018
[38] L. von Stumberg, P. Wenzel, Q. Khan, and D. Cremers. Gn-net: The geuss-newton loss for multi- relocalization. IEFE Robotics and Automation Letters, 5(2)8890897, 2020.
[39] M. Bhardwaj. B. Boots, and M. Mukadam. Differentiable Gaussian process motion planning. In IEEE Intl. Conf. on Robotics and Automation (ICRA), 2020.
[40] B. Amos and D. Yarats. The differentiable cross-entropy method. International Conference on Machine Learning, pages 291-302, PMLR, 2020.
[41] F. Dellaert. Factor graphs and GTSAM: A hands-on introduction. Technical report, Georgia Institute of Technology, 2012.
[42] P. E. Gill, W. Murray, and M. A. Saunders. SNOPT: An SQP algorithm for large-scale constrained optimization. SIAM review, 47(1):99-133 200..
10

---

[43] A. L. Dontchev and R. T. Rockafellar, Implicit functions and solution mappings, volume 543, Springer, 2009.
[44] B. Amos and J. Z. Kolter. Optnet: Differentiable optimization a layer in neural networks, In Interna- tional Conference Machine Learning (ICML). pages
2017.
[45] B. D. Ziebart, A. L. Maas, J. A. Bagnell, and A. K. Dey. Maximum entropy inverse reinforcement learning. In AAAI Conf. on Artificial Intelligence, volume pages 1433-1438, 2008.
[46] M. Wulfmeier, P. Ondruska, and I. Posner. Maximum entropy deep inverse reinforcement learning. arXiv preprint 1rXiv:1507.04885 2015.
[47] F. K. Gustafsson, M. Danelljan, R. Timofte, and T. B. Schön. How to train your energy-based model for regression. In British Machine Vision Conf. (BMVC), 2020.
[48] Y. Du, S. J. Tenenbaum, and I. Mordatch. Improved contrastive divergence training of energy based models. arXiv preprint arXiv:2012.01316, 2020.
[49] L.-C. Chen, A. Schwing. A. Yuille, and R. Urtasun. Learning deep structured models. In International Conference on Machine Learning (ICMI.), 2015.
[50] D. Belanger and A. McCallum. Structured prediction energy networks. In International Conference on Machine Learning (ICML), 2016.
[51] T. Ticleman. Training restricted boltzmann machines using approximations to the likelihood gradient. In International Conference on Machine Learning (ICML), pages 1064-1071, 2008.
[52] Y. Du and I. Mordatch. Implicit generation and modeling with energy based models. In Advances in Neural Information Processing Systems 24 (NeurIPS), 2019.
[53] E. Nijkamp, M. Hill, S.-C. Zhu, and Y. N. Wu. Learning non-convergent non-persistent short-run meme toward energy-based model. In Advances Neural Information Processing Systems 24 (NeurIPS), 2019.
[54] T. Kim and Y. Bengio. Deep directed generative models with energy-based probability estimation. arXiv preprint arXiv: 166. 600.0333,9 2016.
[55] Z. Dai, A. Almahairi, P. Bachman, E. Hovy, and A. Courville. Calibrating energy-based generative adversarial networks. arXiv preprint arXiv:1702.0 1702001691, 2017.
[56] M. J. Wainwright, T. S. Jaakkola, and A. S. Willsky. new class of upper bounds on the log partition function. IEEE Transactions on Information Theory, 51(7):2313-2233 2005.
[57] F. Dellaert and M. Kaess. Square root SAM: Simultaneous localization and mapping via square root information smoothing. Indl, of Robotics Research, 2006.
[58] M. Kaess, A. Ranganathan, and F. Dellacrt. iSAM: Incremental smoothing and mapping. IEEE Trans. Robotics, 1365) 1378, 2008
[59] P. Sodhi, S. Choudhury. J. G. Mangelson, and M. Kaess. ICS: Incremental constrained smoothing for state estimation. In IEEE Intl. Conf. on Robotics and Automation (ICRA), 2020.
[60] J. Sola, J. Deray, and D. Atchuthan. A miero lie theory for state estimation in roboties. arXiv preprint arXiv:1812.015:5 2018.
[61] A. Immer, M. Korzepa, and M. Bauer. Improving predictions of bayesian neural nets via local lineariza- tion. In International Conference on Artificial Intelligence and Statistics, pages 703-711. PMLR. 2021.
[62] A. Paszke, S. Gross, F. Massa, A. Lerer, J. Bradbury, G. Chanan, T. Killeen, Z. Lin, N. Ginelshein, L. Antiga, A. Desmaison, A. Kopf. E. Yang. Z. Detito, M. Raison, Tejani, S. Chilamkurthy, Steiner, L. Fang. J. Bai, and S. Chintala. PyTorch: An imperative style, high-performance deep learning library.
In Advances in Neural Information Processing Systems 24 (NeurIPS), pages 8024-8035 2019.
[63] M. Lambeta, W. Chou, S. Tian, B. Yang, B. Maloon, V. R. Most, D. Stroud, R. Santos, A. Byagowi, G. Kammerer, et al. DIGIT: A novel design for a low-cost compact high high-resolution tactile sensor with application to in- hand manipulation. IEEE Robotics and Automation Letters, 5(3):8838 -888883885440888 2020.


Reviews:
[{"review_id": "mQHKN1UpeZf", "reviewer": "Reviewer_krZP", "summary": "The paper proposes an algorithm for performing maximum-likelihood optimization of an energy-based model over states and observations that relies on Laplace + Gauss-Newton approximation to generate contrastive samples. The algorithm is evaluated by its tracking performance and sample efficiency on synthetic navigation and real-world planar pushing tasks.", "questions": "", "limitations": "", "rating": 4, "confidence": null, "soundness": null, "presentation": null, "contribution": null, "strengths": "", "weaknesses": "", "quality": null, "clarity": null, "significance": null, "originality": null, "strengths_and_weaknesses": "Strengths: The paper is well-written, clear, easy to follow. The approach is straightforward, interesting, and described in sufficient detail. The experiments are informative and highlight the properties of the algorithm. The video is well-done and provides a good overview of the approach.\n\nWeaknesses: Section 3. Problem Formulation could almost entirely be removed. One could directly start with Eq. (6) that gives the standard ML objective for energy-based learning.\n\nThe Gauss-Newton method for inference is a well-known technique for sampling from intractable models. In the related work section and in Sec. 4.3 this should be clarified, because otherwise one gets an impression that the authors are claiming that they invented it. A recent overview of Laplace approximation with Gauss-Newton method, including links to prior work, can be found in [1].\n\nThe title seems misleading. In general, references to \"graph\" in the paper tend to obfuscate matters. In the end, what is meant is a \"graphical model\" or a \"factor graph\". But this is more-or-less standard in any paper on probabilistic modeling. One could therefore misinterpret \"graph\" in the title as though it were referring to graph neural networks or some algorithms for optimization over graphs. Perhaps the easiest fix would be to say \"factor graph\" in the title.\n\n[1] Immer, A., Korzepa, M., & Bauer, M. (2021, March). Improving predictions of Bayesian neural nets via local linearization. In International Conference on Artificial Intelligence and Statistics (pp. 703-711). PMLR.", "was_revised": false, "final_justification": "", "rebuttal": "", "claims_and_evidence": "", "methods_and_evaluation_criteria": "", "theoretical_claims": "", "experimental_designs_or_analyses": "", "relation_to_broader_scientific_literature": "", "essential_references_not_discussed": "", "other_strengths_and_weaknesses": "", "other_comments_or_suggestions": "", "questions_for_authors": "", "summary_of_paper": "", "summary_of_recommendation": "The paper considers an interesting idea of framing the observation model as an energy-based model. An approach for training this model is proposed and evaluated. The results appear encouraging and there are clear directions for future work provided. This paper may inspire further investigation into energy-based models for filtering and other applications in estimation and control.", "technical_quality": "Very Good:  Convincing.", "clarity_of_presentation": "Very Good:  Well organized and clear.", "potential_impact": "", "robotics_focus": "", "quality_of_limitations": "", "questions_for_rebuttal": "", "reasons_to_accept": "", "reasons_to_reject": "", "questions_to_authors": "", "main_review": "", "summary_of_the_paper": "", "summary_of_the_review": "", "strength_and_weaknesses": "", "limitations_and_societal_impact": "", "review_text": "", "extra_scores": {"impact": "4: The work contains interesting new ideas that will potentially have major impact in robotics or machine learning.", "issues": "See weaknesses above", "reviewer_expertise": "Very good: Comprehensive knowledge of the area"}, "extra_text": {}}, {"review_id": "SByNwQWMgmu", "reviewer": "Reviewer_uKGu", "summary": "This paper applied energy-based method to solve a factor-graph optimization task (e.g., graphs that comprise factor nodes representing latent states which are connected to observation nodes). The proposed method has concrete robotic applications such as learning an observation model for a scenario concerning a robot hand manipulating an object with only tactile feedback.\n\nThe paper in particular focuses on two key points: (1) optimizing factor graph via implicit energy-based loss instead of explicit surrogate loss (i.e., using losses that parameterize the compatibility between latent states & observations instead of losses that characterizes the difference between ground-truth and explicit mapping from observations to latent states); and (2) treating factor graphs as random variables instead of fixed, unknown quantities, which relax the original discrete optimization task (over the discrete space of graphs) to a continuous optimization problem (over the continuous space of parameters defining the graph distribution).", "questions": "", "limitations": "", "rating": 2, "confidence": null, "soundness": null, "presentation": null, "contribution": null, "strengths": "", "weaknesses": "", "quality": null, "clarity": null, "significance": null, "originality": null, "strengths_and_weaknesses": "STRENGTH:\n\nThe paper is well-written &  the mathematical exposition is not difficult to follow. The key points of the paper are well-communicated.\n\nThe proposed method has also be extensively evaluated on several datasets with clear visual demonstrations, which are really helpful in highlighting the usefulness of the proposed method\n\nWEAKNESSES:\n\nWhile this is a nice application paper, it does not exactly provide any new algorithmic and/or theoretical insights. \n\nIn fact, both key points of (1) optimizing via energy-based implicit loss; and (2) optimizing distribution of graphs instead of graphs themselves to convert a discrete optimization task into a standard gradient-based optimization problem have been proposed and applied extensively in many fields of ML. \n\nIn addition, the comparison with optimizing surrogate explicit loss appears minimal and lacks specific detail. Other than the vague description in line 139, I couldn't find where else in the main text such explicit loss was detailed. Aren't there existing works on the specific applications used in the experiment that the authors should have compared with?\n\nLast, I'd suggest improving the differentiation between Loss 2 & Loss 3. Claiming that Loss 2 is surrogate & Loss 3 is energy-based (hence, implying it is not a surrogate) seems a bit misleading -- to me, the key difference is Loss 2 is based on an explicit mapping from observations to states while Loss 3 only characterize a fitness between a state configuration & the observations, which is implicit regarding the relationship between observations & latent states.", "was_revised": false, "final_justification": "", "rebuttal": "", "claims_and_evidence": "", "methods_and_evaluation_criteria": "", "theoretical_claims": "", "experimental_designs_or_analyses": "", "relation_to_broader_scientific_literature": "", "essential_references_not_discussed": "", "other_strengths_and_weaknesses": "", "other_comments_or_suggestions": "", "questions_for_authors": "", "summary_of_paper": "", "summary_of_recommendation": "I think the paper is technically correct, well-motivated for an application but the entire work appears very incremental and is unlikely to have much impact regarding algorithmic novelties (e.g., both key points of the paper have been investigated and applied heavily in the ML but perhaps with different context)\n\nI have elaborated my position in the \"weaknesses\" section above. That being said, I am not an expert in robotic so I am not entirely sure that this paper should be judged purely from a ML perspective -- if the other reviewers feel that the practical impact of this paper is sufficient in the robotic context, I would be happy to upgrade my rating.\n\nBut, regardless of the final outcome, I would still urge the authors to further elaborate & expand their comparison benchmark (especially against discrete graph optimizer approaches & existing works that employ explicit losses).\n\n", "technical_quality": "Good: Generally convincing, but some additional experiments or analyses needed.", "clarity_of_presentation": "Very Good:  Well organized and clear.", "potential_impact": "", "robotics_focus": "", "quality_of_limitations": "", "questions_for_rebuttal": "", "reasons_to_accept": "", "reasons_to_reject": "", "questions_to_authors": "", "main_review": "", "summary_of_the_paper": "", "summary_of_the_review": "", "strength_and_weaknesses": "", "limitations_and_societal_impact": "", "review_text": "", "extra_scores": {"impact": "3: The work is incremental and unlikely to have much impact, even though it may be technically correct and well executed.", "issues": "Please refer to my specific comments in the Weakness section above.", "reviewer_expertise": "Fair: Some knowledge of the area"}, "extra_text": {}}, {"review_id": "47nQrXomcX", "reviewer": "Reviewer_bQ4N", "summary": "The paper presents a method for learning observation models, i.e. probabilistic representations of the relationship between a latent state and sensor data, based on combining energy models with incremental Gauss-Newton solvers. The performance is evaluated on synthetic navigation and real-world object-pushing data sets.", "questions": "", "limitations": "", "rating": 3, "confidence": null, "soundness": null, "presentation": null, "contribution": null, "strengths": "", "weaknesses": "", "quality": null, "clarity": null, "significance": null, "originality": null, "strengths_and_weaknesses": "Strengths\n- The paper is well written and a pleasure to read.\n- The method seems sound, and computationally efficient enough to be useful.\n- The experimental results are encouraging.\n\nWeaknesses:\nI think it's a nice paper, but I have three significant concerns:\n1) As a reason for ruling out \"Loss 1\", the authors claim that the mapping from \\theta to \\hat x in (3) is not differentiable. I don't understand this statement. Equation (3) states that \\hat x is the minimizer of a smooth nonlinear least-squares problem. As such, it satisfies the first-order necessary conditions, and from this equation a simple application of the implicit function theorem gives the Jacobian of \\hat x with respect to \\theta. I think Loss 3 can still be justified on other grounds, but still this is strange.\n\n2) Section 4.1 and 4.2 introduce the main ideas leading to Equation (7) and the associated Algorithm 1, and they read as though this is a novelty of the paper. I'm not an expert in this field, but I thought this was all completely standard in maximum-likelihood modelling. E.g. essentially the same approach is described in detail in Ch 18 of the Goodfellow et al Deep Learning book, under names such as Contrastive Divergence, e.g. Algorithm 18.1 in that book.\n\nAs far as I can see, the main novelty is using the estimated mean and covariance from the smoothing algorithm to generate the samples. The standard approach would be to use some sort of Monte-Carlo method (e.g. Gibbs sampling) to generate samples. \n\nI think there is a nice contribution here, but this material could more clearly describe what is actually new in the approach and what is standard.\n\n3) Related to the above, I like the experimental results, but the choice of comparisons is a bit strange. To me, a more natural comparison would be a standard approach to Loss 3 based on MCMC. E.g. you could still use the proposed method as a proposal distribution, but then run a few iterations of a Gibbs sampler and see if that makes a difference.\n\n", "was_revised": false, "final_justification": "", "rebuttal": "", "claims_and_evidence": "", "methods_and_evaluation_criteria": "", "theoretical_claims": "", "experimental_designs_or_analyses": "", "relation_to_broader_scientific_literature": "", "essential_references_not_discussed": "", "other_strengths_and_weaknesses": "", "other_comments_or_suggestions": "", "questions_for_authors": "", "summary_of_paper": "", "summary_of_recommendation": "Overall, I found this an enjoyable paper to read and I think there are some useful ideas here. I have a few issues with the paper, as described above, but these should be possible to address in a revised version.\n\n", "technical_quality": "Very Good:  Convincing.", "clarity_of_presentation": "Very Good:  Well organized and clear.", "potential_impact": "", "robotics_focus": "", "quality_of_limitations": "", "questions_for_rebuttal": "", "reasons_to_accept": "", "reasons_to_reject": "", "questions_to_authors": "", "main_review": "", "summary_of_the_paper": "", "summary_of_the_review": "", "strength_and_weaknesses": "", "limitations_and_societal_impact": "", "review_text": "", "extra_scores": {"impact": "4: The work contains interesting new ideas that will potentially have major impact in robotics or machine learning.", "issues": "See above.", "reviewer_expertise": "Fair: Some knowledge of the area"}, "extra_text": {}}]

Author rebuttal:

Meta-review: This paper proposed an algorithm for performing maximum-likelihood optimization of an energy-based model. The approach appears to be standard by applying Laplace and Gauss-Newton approximations. The authors should justify what is new. Additionally, the authors are expected to elaborate further and expand their comparison benchmark.
Decision: Accept (Poster)