Learning Observation Models with Incremental Non-Differentiable Graph
Optimizers in the Loop for Robotics State Estimation

Mohamad Qadri 1 Michael Kaess 1

Abstract

iSAM2 (Kaess et al., 2012) is an optimizer that leverages theBayes tree to solve incremental (i.e. as the number of factors
N grows continuously) inference problems formulated as afactor graph. In essence, it solves problems of the form:

We consider the problem of learning observation
models for robot state estimation with incremental
non-differentiable optimizers in the loop. Convergence to the correct belief over the robot stateis heavily dependent on a proper tuning of observation models which serve as input to the optimizer. We propose a gradient-based learning
method which converges much quicker to model
estimates that lead to solutions of much better
quality compared to an existing state-of-the-art
method as measured by the tracking accuracy overunseen robot test trajectories.

x∗ = arg min
x

N
X
1

2
i=1

||gi (xi ) − zi ||2θi(1)

or equivalently the MAP inference problem:x∗ = arg maxxNY

φi (xi )(2)

iwhere φi are the potentials assumed to take the form
φi ∝ exp − 21 ||gi (xi ) − zi )||2θi , x is the vector of unknown
states, xi a subset of x, and z = {zi } are the sensor measurements. Batch solvers such as Levenberg-Marquardt perform the typical linearize-solve loop which quickly becomesprohibitively expensive as the size of the problem grows.
iSAM2, on the other hand, leverages the Bayes tree anduses fluid relinarization and partial state updates making it
the de-facto optimizer for online smoothing-based state estimation problems in robotics. However, these features also
prevent iSAM2 from being differentiable (i.e. dependence
on relinearization thresholds, inherent non-differentiable
operations such as removal and re-insertion of tree nodes.)1. Introduction
Robot state estimation is the problem of inferring the stateof a robot (a set of geometric or physical quantities suchas position, orientation, contact forces etc.) given sensor
measurements. The problem is typically formulated as Maximum a Posteriori (MAP) inference over factor graphs where
each node (robot state) is connected to other states via soft
constraints or potentials (factors) which are distilled fromsensor measurements. Given a factor graph, a potential way
to perform the inference step is to convert it to a chordal
Bayes Net using an exact inference technique (i.e. variable
elimination) and then in turn, convert the Bayes Net to atree like structure (i.e. junction tree) where inference can bemade easier. However, this procedure can become ill-suitedfor real-time state estimation especially if the number ofvariables in the problem increases with time (i.e. as therobot navigates the environment). The Bayes tree (Kaesset al., 2011) was introduced to tackle this problem. It is atree structure where nodes are formed from the maximal
cliques of a chordal Bayes net. The Bayes tree is directed
and maintains the conditional independences described by
the original Bayes net. In addition, it allows for fast incremental inference which corresponds to simple tree editing.

On the other hand, the quality of the solution of eq.1 islargely dependent on a proper selection of the observation
models {θi } which parameterize the joint or conditional
distributions over states and measurements. However, due
the non-differentiability of iSAM2, current methods to tune
these parameters are generally sampling-based which areslow and may converge to poor optimas. In this work, we
propose a gradient-based optimization-based approach totune {θi } with iSAM2 in the loop using a direct tracking
error loss. We compare our method with a state-of-theart sampling-based method on a synthetic robot navigation
example and show that our procedure converges order-ofmagnitude faster to a better solution as measured by the
tracking accuracy over unseen robot test trajectories.

1

Robotics Institute, Department of Computer Science, Carnegie
Mellon University, Pittsburgh, United States. Correspondence to:Mohamad Qadri <mqadri@andrew.cmu.edu>.

2. Related Work

Published at the Differentiable Almost Everything Workshop of the
40 th International Conference on Machine Learning, Honolulu,
Hawaii, USA. July 2023. Copyright 2023 by the author(s).

Early state estimation techniques such as the family of
Kalman Filters (Julier & Uhlmann, 2004; Thrun et al., 2001)
1

Learning Observation Models with Incremental Non-Differentiable Graph Optimizers in the Loop for Robotics State Estimation

Figure 1. The parameter vector θ t serve as input to the least squares problem which is solved by iSAM2. Gradients of the solution withrespect to the parameters are computed and used to update θ t+1

state x∗ ∈ X after performing N update steps. Here, Mi
is a Lie Group (for example the special Euclidean groupni
SE(n)) and S++
is the set of ni × ni positive definite
matrices .

rely on the Markov assumption to enable real-time performance. Recent algorithms have been proposed to makethese filters differentiable (Kloss et al., 2021; Sun et al.,2016; Haarnoja et al., 2016). However, the inherent relianceon the Markov assumption and the inability to re-linearize
past states can lead to convergence to poor solutions. Hence,state-of-the-art robotic state estimation algorithms moved
to factor graph-based solutions which encode the inherent
temporal structure avoiding the need to marginalize paststates and providing methods to relinearize past estimates(Kaess et al., 2012; Qadri et al., 2022). However, observation models on factor graphs are either pre-specified andfixed (Lambert et al., 2019; Engel et al., 2014) or learned
using surrogate losses independent of the graph optimizer
or final tracking performance (Sodhi et al., 2021; Sundaralingam et al., 2019; Czarnowski et al., 2020). Methods
that differentiate through the arg min operator in eq. 1 byunrolling the optimizer can be used to learn these models(Yi et al., 2021; Jatavallabhula et al., 2020; Bechtle et al.,2021). However, these techniques are typically sensitive
to hyperparameters such as the number of unrolling steps(Amos & Yarats, 2020) in addition to suffering from vanishing as well as high bias and variance gradients. Recently,
a novel method LEO (Sodhi et al., 2022) took advantageof the probabilistic view offered by iSAM2 (as a solver ofeq.2) to provide a way to learn observation models, with
iSAM2 in the loop, by minimizing a novel tracking error. In
essence, at every training iteration LEO samples trajectories
from the posterior distribution (a joint Gaussian distribution
over the states) and the deviation with respect to the groundtruth trajectory is minimized using an energy-based loss. In
this work, we use LEO as our baseline.

Our goal is to learn the parameters {θi } using gradientbased methods from observed ground truth robots trajectories xGT . Specifically, We consider the following innerouter optimization procedure:
Inner Loop:x̂ = arg min
x

X1
i

2

||gi (x) − zi ||2θimin L(x̂(θ), xGT )Outer Loop:

θ

(3)
(4)

∂ x̂
Note that the gradient ∂L∂θ , requires an estimate of ∂θ . Although iSAM2 includes different non-differentiable operations, the gradient of the solution with respect to the parameter vector ∂∂θx̂ exists and can be computed by the implicit
function theorem (Dontchev et al., 2009) as done in existing work in convex optimization (Amos & Kolter, 2017;
Agrawal et al., 2019). Note that the original theorem consider functions operating on vector spaces. However, thetheorem can readily be extended to other manifolds by applying the appropriate group operations.

The Implicit Function Theorem:
Let x̂ := {x | f (x, θ) = 0} where x ∈ X and θ =n{θi | θi ∈ S++}. Let f be continuously differentiable in theneighborhood of (x̂, θ) namely ∇x f (x̂, θ) be nonsingular.
Then:
∂x̂
=−
∂θ



∂f (x̂(θ), θ)∂x

−1∂f (x̂(θ), θ)∂θ

(5)

3. Method
The gradient in eq. 5 can be derived and computed analytically. However, we note that since the size of parameter
vector θ is typically small (for example, each θi has a maximum of 6 free parameters when working with elements
in SE(2)), numerical differentiation proved to be efficient
especially when coupled with parallelization on CPU.

In this work, we view the incremental optimizer (iSAM2)
as a function f : X × θ → X which takes initial estimatesof the state x ∈ X : M1 × . . . × Mn at timestep t (thenumber of states increases with time), as well as parametersniθ = {θi | θi ∈ S++} and returns an estimate of the optimal
2

Dataset 2

Dataset 1

Learning Observation Models with Incremental Non-Differentiable Graph Optimizers in the Loop for Robotics State Estimation

Figure 2. Convergence as a function of training time measured in minutes. Both methods were run for 100 training iterations. Our method
converges after a few seconds while each iteration of LEO takes approximately 2.5 minutes. Additionally, our method converges toparameters that track the training trajectories more accurately.

Figure 3. Accuracy on the test set. Our method converges to parameters that generalize and track the test trajectories more accurately.

where Y ∈ N , τ is a small increment defined on Tϵ (N ).
The Exp operator map elements from a Lie Group to its algebra while the Log operator map elements from the algebra to
the group. ⊕, ⊖, and ◦ are the plus, minus, and composition
operators respectively (Sola et al., 2018) where:

3.1. Numerical Jacobians over Lie Groups
3.1.1. JACOBIANS ON V ECTOR S PACES
Given a multivariate function f : Rm → Rn , the Jacobian
is defined as the n × m matrix:
 ∂f1
∂f1 
. . . ∂x∂x1
m
∂f (x)  .
..
.. =  ..
(6)

.
.
∂x
∂fn
∂fn
. . . ∂xm
∂x1
h→0

2

∈ Rni

(11)

Hence, τ ∈ Rni and the operator ⊕ is the standard additionon vector space Rni .

We can similarly define the left Jacobian of functions acting
on manifolds f : N → M as the linear map from the Liealgebra Tϵ (N ) of N to Tϵ (M), the Lie algebra of M:
f (τ ⊕ Y) ⊖ f (Y)Df (X )
= lim
τ
→0
DY
τ
Log(f (Exp(τ ) ◦ Y) ◦ f (Y)−1 )
= lim
τ →0
τ

(10)

θi = diag−1 (Σi )

3.1.2. L EFT JACOBIANS ON L IE G ROUPSϵ(9)τ = Y1 ⊖ Y2 = Log(Y1 ◦ Y2−1 ); Y1 , Y2 ∈ N

n1nm
In this work, N = S++
and M = X . Addi× . . . × S++
tionally, we assume that each vector θi are the square root
elements of some corresponding diagonal positive definite
nimatrix Σi ∈ S++. i.e., we define the following map:∂fwhere ∂x
= lim f (x+hehi )−f (x) and ei is the ith standardibasis of Rm .

τ ⊕ Y = Exp(τ ) ◦ Y

3.2. Tracking Loss
Let a parameter estimate θ t ∈ Rm . The outer loss is theregularized mean squared error between the estimated trajectory x̂(θ t ) and the ground truth xGT . Let D be the training
set, T be the total number of states in xGT , D be the sum of

(7)
(8)
3

Learning Observation Models with Incremental Non-Differentiable Graph Optimizers in the Loop for Robotics State Estimation

the lie algebra dimensions of all states, and λ ∈ R:

At each training iteration, iSAM2 optimizes the inner loop
(eq. 3) objective for both methods:
1Xx̂ = arg min G(θ, x; z) = arg min||g gps (xi )−zigps ||2θgps
2
x
x
i

1
odom 2+ ||g odom (xi−1 , xi ) − zi−1,i||θodom
(16)
2

|D|

L(θ) =

1 X||vec(x̂j (θ t ) ⊖ xGT )||22 + λ||θ t ||22 (12)2|D| j=1

|D|
∂L
1 X
S(x̂j (θ t ))T · vec(x̂j (θ t ) ⊖ xGT ) +2λθ t={z}|∂θ |D| j=1
∈RT D

(13)

where vec is the vectorization operator and S(x̂(θ t )) ∈RT D×m is a matrix such that each row r is equal to:
 j
∂x̂
t
j
S(x̂ (θ ))r = vec∂θij

Our method then updates the parameters θ using eq.15 while
LEO minimizes the following energy-based loss:
Z
X
j
1
j
j
L(θ) =
E(θ, xgt ; z )+log e−E(θ,x;z ) dx (17)|D| jx(xgt ,zj )∈D

where (xjgt , zj ) is a ground truth sample from the training
set D, the energy E(θ, x; z) := G(θ, x; z), and the integral
is over the space of trajectories. Fig. 2 shows the training
time and root mean squared error (RMSE) at each iteration.
Fig. 3 shows the RMSE on the test set, and table I gives
a summary of the results. We note that our method converges to parameters that lead to better tracking accuracy on
all unseen test trajectories. In addition, while LEO needs
to generate samples from a high dimensional probability
distribution during training which is a time consuming process, our method generates gradients by directly comparing
deviation from the training trajectories leading to order-ofmagnitude faster training time. We additionally analyze theperformance of both methods as a function of the training
set size and provide the results in appendix A.2.

t

vec(Log(x̂j (θ̃ ) ◦ x̂j (θ t )−1 ))
= lim
(14)
τij →0τij
ttt+ τij and θ̃ = θ t otherwise. By the= θijwhere θ̃ij∂x̂
implicit function theorem, the gradient ∂θexists and isij
estimated in eq. 14 using finite differencing by perturbing
the parameter θij by τij . Parameters are then updated using
gradient descent with learning rate α:θ t+1 = θ t − α ·

∂L
∂θ

(15)

4. Results

5. Conclusion and Future Work

Table I. Comparison between LEO and Ours (final training runtime
and error, test error statistics).
Num training iterations
Parallelization enabled during training
Time per training iteration (min)Num of training trajectories
Length of training trajectories
Avg training RMS translation error (m)Avg training RMS rotation Avg error (rad)Num test trajectories
Length of test trajectories
Avg test RMS translation error (m)Avg test RMS rotation error (rad)LEO (D1 )
100
yes
2.863
5
300
0.385
0.087
20
300
0.384
0.067

Ours (D1 )
100
no
0.025
5
100
0.236
0.062
20
300
0.238
0.063

LEO (D2 )
100
yes
3.773
5
300
1.179
0.467
20
300
1.153
0.463

We presented a gradient-based learning algorithm which
estimates observation models with non-differentiable optimizers (iSAM2) in the loop for robotic state estimation.
While current state of the art algorithms require sampling
trajectories from the posterior distribution to bypass thenon-differentiability of the optimizer, our technique learns
parameters by formulating the problem as a bilevel optimization procedure where gradients are generated throughnumerical differentiation.

Ours (D2 )
100
no
0.022
5
100
0.730
0.051
20
300
0.733
0.050

We use two synthetic planar (i.e. in SE(2)) robotic navigation datasets D1 and D2 consisting of GPS and odometricmeasurements. Each dataset uses a different set of parameters {θGPS , θodom }Di to generate N trajectories (more details
in appendix A.1). We use 5 training ground truth trajectories
of length1 300 to train LEO2 and 5 training trajectories oflength 100 to train our method. We use a set of 20 unseen
trajectories (all of length 300) to test the tracking performance given our final learned parameters (the testing set isfixed for both methods). For LEO, we enable multithreaded
parallel trajectory sampling while do not use any form ofparallelization with our technique.
1
2

For future work, we want to extend our algorithm to learn
parameters {θi }, with iSAM2 in the loop, that are themselves functions of observations i.e. θi (zi , φi ) where φi can,for example, be the weights of a jointly trained neural network. Indeed, the outputs of the network can be perturbed∂x̂
to approximate ∂θas proposed in this work and then simii
ply chained with the gradient ∂θ∂ϕ (obtainable from existing
auto-differentiation packages such as PyTorch) to get thegradient of the optimized output trajectory with respect tonetwork weights. We plan to compare the sample efficiency,
training time, and generalization performance of our method
against different baselines. Finally, we plan to train our algorithm on real robotics data and deploy our state estimatoron real robotics platforms.

Length refers to the number of nodes in the graph optimizer
We use the official implementation of LEO by Paloma et al.

4

Learning Observation Models with Incremental Non-Differentiable Graph Optimizers in the Loop for Robotics State Estimation

References

Kaess, M., Johannsson, H., Roberts, R., Ila, V., Leonard,
J. J., and Dellaert, F. isam2: Incremental smoothing andmapping using the bayes tree. The International Journalof Robotics Research, 31(2):216–235, 2012.

Agrawal, A., Amos, B., Barratt, S., Boyd, S., Diamond, S.,
and Kolter, J. Z. Differentiable convex optimization layers. Advances in neural information processing systems,
32, 2019.

Kloss, A., Martius, G., and Bohg, J. How to train yourdifferentiable filter. Autonomous Robots, 45(4):561–578,
2021.

Amos, B. and Kolter, J. Z. Optnet: Differentiable optimization as a layer in neural networks. In International
Conference on Machine Learning, pp. 136–145. PMLR,
2017.

Lambert, A. S., Mukadam, M., Sundaralingam, B., Ratliff,N., Boots, B., and Fox, D. Joint inference of kinematicand force trajectories with visuo-tactile sensing. In 2019
International Conference on Robotics and Automation(ICRA), pp. 3165–3171. IEEE, 2019.

Amos, B. and Yarats, D. The differentiable cross-entropy
method. In International Conference on Machine Learning, pp. 291–302. PMLR, 2020.
Bechtle, S., Molchanov, A., Chebotar, Y., Grefenstette, E.,
Righetti, L., Sukhatme, G., and Meier, F. Meta learning
via learned loss. In 2020 25th International Conference
on Pattern Recognition (ICPR), pp. 4161–4168. IEEE,
2021.

Qadri, M., Sodhi, P., Mangelson, J. G., Dellaert, F., andKaess, M. Incopt: Incremental constrained optimization
using the bayes tree. In 2022 IEEE/RSJ International
Conference on Intelligent Robots and Systems (IROS),
pp. 6381–6388, 2022. doi: 10.1109/IROS47612.2022.
9982178.

Czarnowski, J., Laidlow, T., Clark, R., and Davison, A. J.
Deepfactors: Real-time probabilistic dense monocularslam. IEEE Robotics and Automation Letters, 5(2):721–
728, 2020.

Sodhi, P., Kaess, M., Mukadam, M., and Anderson, S.
Learning tactile models for factor graph-based estimation. In 2021 IEEE International Conference on Robotics
and Automation (ICRA), pp. 13686–13692. IEEE, 2021.

Dontchev, A. L., Rockafellar, R. T., and Rockafellar, R. T.
Implicit functions and solution mappings: A view fromvariational analysis, volume 11. Springer, 2009.

Sodhi, P., Dexheimer, E., Mukadam, M., Anderson, S., andKaess, M. Leo: Learning energy-based models in factor
graph optimization. In Conference on Robot Learning,pp. 234–244. PMLR, 2022.

Engel, J., Schöps, T., and Cremers, D. Lsd-slam: Largescale direct monocular slam. In Computer Vision–ECCV
2014: 13th European Conference, Zurich, Switzerland,
September 6-12, 2014, Proceedings, Part II 13, pp. 834–
849. Springer, 2014.

Sola, J., Deray, J., and Atchuthan, D. A micro lie theory for state estimation in robotics. arXiv preprint
arXiv:1812.01537, 2018.
Sun, W., Venkatraman, A., Boots, B., and Bagnell, J. A.
Learning to filter with predictive state inference machines.
In International conference on machine learning, pp.
1197–1205. PMLR, 2016.

Haarnoja, T., Ajay, A., Levine, S., and Abbeel, P. Backprop
kf: Learning discriminative deterministic state estimators.
Advances in neural information processing systems, 29,2016.

Sundaralingam, B., Lambert, A. S., Handa, A., Boots, B.,
Hermans, T., Birchfield, S., Ratliff, N., and Fox, D. Robust learning of tactile force estimation through robot interaction. In 2019 International Conference on Robotics
and Automation (ICRA), pp. 9035–9042. IEEE, 2019.

Jatavallabhula, K. M., Iyer, G., and Paull, L. slam: Dense
slam meets automatic differentiation. In 2020 IEEE International Conference on Robotics and Automation (ICRA),
pp. 2130–2137. IEEE, 2020.

Thrun, S., Fox, D., Burgard, W., and Dellaert, F. Robustmonte carlo localization for mobile robots. Artificial
intelligence, 128(1-2):99–141, 2001.

Julier, S. J. and Uhlmann, J. K. Unscented filtering andnonlinear estimation. Proceedings of the IEEE, 92(3):
401–422, 2004.

Yi, B., Lee, M. A., Kloss, A., Martı́n-Martı́n, R., and Bohg,
J. Differentiable factor graph optimization for learningsmoothers. In 2021 IEEE/RSJ International Conferenceon Intelligent Robots and Systems (IROS), pp. 1339–1345.
IEEE, 2021.

Kaess, M., Ila, V., Roberts, R., and Dellaert, F. The bayes
tree: An algorithmic foundation for probabilistic robot
mapping. In Algorithmic Foundations of Robotics IX: Selected Contributions of the Ninth International Workshop
on the Algorithmic Foundations of Robotics, pp. 157–173.
Springer, 2011.
5

Learning Observation Models with Incremental Non-Differentiable Graph Optimizers in the Loop for Robotics State Estimation

A. Appendix
A.1. Additional Details on Experimental Setup

Figure 4. The factor graph used to generate the synthetic robot navigation trajectories.

Figure 4 shows the structure of the factor graph used to generate the synthetic robot navigation trajectories. A unary GPS
factor g gps (xi ) is added to each pose xi while a binary odometry factor g gps (xi , xi−1 ) is specified between poses. To
simulate realistic robot navigation trajectories for each of datasets D1 and D2 , Gaussian noise ∼ N (0, θodom ) is injected to
ground truth relative odometry measurements while Gaussian noise ∼ N (0, θgps ) is added to absolute ground truth GPS
measurements.
A.2. Additional Experiments
We analyze our method and LEO as a function of the training set size. Tables II and III show the average training and testing
RMS errors on datasets D1 and D2 respectively. We note that our method possesses a much faster training time as well as
learns parameters that generalize better to unseen test trajectories regardless of the training set size.
Table II. Dataset D1 - Comparison between LEO and Ours (final training runtime and error, test error statistics).
Num trajectory in training set
1
5
10
20
30
Algorithm
LEO
Ours
LEO
Ours
LEO
Ours
LEO
Ours
LEO
Ours
Num training iterations
100
100
100
100
100
100
100
100
100
100
yes
no
yes
no
yes∗
no
yes∗
no
yes∗
no
Parallelization enabled during training
Time per training iteration (min)0.937 0.005 2.863 0.025 4.370 0.045 4.899 0.091
7.56 0.136
Average training RMS translation error (m)0.379 0.257
0.385 0.236 0.383 0.243 0.386 0.243 0.385 0.236
Average training RMS rotation Avg error (rad) 0.071 0.060
0.087 0.062 0.076 0.063 0.079 0.068 0.079 0.064
Num test trajectories
20
20
20
20
20
20
20
20
20
20
Length of test trajectories
300
300
300
300
300
300
300
300
300
300
0.385 0.237
0.384 0.238 0.385 0.238 0.384 0.240 0.384 0.235
Average test RMS translation error (m)Average test RMS rotation error (rad)0.067 0.059
0.067 0.063 0.067 0.063 0.067 0.069 0.067 0.066
Table III. Dataset D2 - Comparison between LEO and Ours (final training runtime and error, test error statistics).
Num trajectory in training set
Algorithm
Num training iterations
Parallelization enabled during training
Time per training iteration (min)Average training RMS translation error (m)Average training RMS rotation Avg error (rad)Num test trajectories
Length of test trajectories
Average test RMS translation error (m)Average test RMS rotation error (rad)

1
LEO
100
yes
0.934
1.129
0.443
20
300
1.132
0.447

5
Ours
100
no
0.004
0.715
0.056
20
300
0.728
0.051

LEO
100
yes
3.773
1.179
0.467
20
300
1.153
0.468

10
Ours
100
no
0.022
0.730
0.051
20
300
0.733
0.050

LEO
100
yes∗
4.403
1.090
0.421
20
300
1.583
0.419

20
Ours
100
no
0.038
0.731
0.055
20
300
0.735
0.055

LEO
100
yes∗
4.901
1.057
0.407
20
300
1.043
0.402

30
Ours
100
no
0.075
0.718
0.056
20
300
0.734
0.056

LEO
100
yes∗
6.96
1.104
0.429
20
300
1.093
0.427

Ours
100
no
0.113
0.761
0.045
20
300
0.771
0.045

For LEO, we faced compute memory problems on larger training sets (indicated by an asterix in tables II and III) which
required us to decrease the number of threads and samples (needed to compute the loss in eq. 17). The values of thehyperparameters used per training set size are specified in table IIII.
Table IIII. Hyperparameters used to train LEO per training set size
Num trajectory in training set
Number of threads
Number of samples generated per training trajectory per iteration

6

1
4
10

5
4
10

10
3
8

20
2
4

30
2
4