Covariance Estimation With Switchable Constraints

Switchable Constraints

From the tests ran yesterday on covariance estimation using M-estimators, it was verified that the M-estimators do not substantially modify the covariance of the state estimate. Continuing alone this line, today the ability of switchable constraints to correctly weight the covariance will be tested.

What are switchable constraints

Switch constraints were introduced by Niko Suenderhauf as a means to allow the graph to optimize it’s topology — the optimial topology is a graph consisting of only inliers — along with the desired states. This is depicted visually below, where the factor graph shown in Figure 1 contains one false loop closure constraint. This false constraint is removed using a switchable constraint in Figure 2 ( credit both images ) .

Fig 1 :: Inital Graph Containing False Constraint.

Fig 2 :: Graph with Switch Constraint Applied to False Loop

More formally, the initial graphs optimization function is

where $x_i$ is the state vector, $u_i$ is the control input, $u_{ij}$ is the loop closure displacement between $x_i$ and $x_j$, and $\Sigma$ and $\Lambda$ are covariance matrices. This optimization can be augmented with switchable constraints such that the new objective function becomes

where $s_{ij}$ is the switch variable being estimated, $\gamma_{ij}$ is the inital estimate of the switch variable ( generally this is set to 1 ), and $\psi()$ is a real-valued function s.t. $\psi(s_{ij}) = w_{ij} : \mathcal{R} \rightarrow [ 1,0 ]$ ( in Suenderhauf’s Thesis, a simple linear function is recommended )

How Switch Constraints Modify the Covariance Matrix

We can evaluate the affect that switchable constraints will have on the covariance estimate by analyzing the modified loop closure constraint

where $\delta_{ij} \triangleq \mathcal{f}(x_i,u_{ij} - x_j)$ and $w_{ij} \triangleq \psi(s_{ij})$

Then, by noting that $w_{ij}$ is a scalar, we can re-arrange the equation above to

This shows us that the estimated weight directly influences the information matrix, as shown below,

And the associated covariance matrix is

In this light, switchable constraints can be see as an adaptive M-estimator.

Compiling Switchable Constraints with GTSAM4 and Ubuntu 16.04

The initial switchable constraints library written by Niko was designed for GTSAM2. Currently we’re on GTSAM4, so several modification had to be made to his source code. You can pull down the updated code from my github repo using the line below.

$ git clone https://github.com/watsonryan/vertigo.git

Testing Switch Factors

To test switch factors, we use the same graph that all previous test were conducted with. For reference, the solution using the $L_2$ cost function is shown below. This plot shows the solution with and without a false constraint linking the fifth vertex to the second vertex. Note that the solution is substantially worse when the fault is present; however, the optimizers uncertainty in the solution is essentially unchanged.

Fig 3 :: Graph Optimization using $L_2$

Now, we add switchable constraints to the faulty graph to see how well it can handle erroneous data. As a comparison, the switch factor optimization on the faulty graph is compared to the solution obtained by $L_2$ optimization on the fault free graph. As can be seen in the Figure below, the positioning solutions from the two optimization routines are essentially identical; however, the switch factor optimiation conveys the additional uncertienty in the final three position nodes due to the false constraint.

Fig 4 :: Switch Factor Optimization of Faulty Graph Compared to $L_2$ Optimization of Clean Graph