M-estimator affect on covariance
Now that we have several robust optimization techniques implemented, which clearly have a benefit in the positioning domain when compared to $L_2$, we would like to see how these techniques affect the uncertainty in the estimated state. To evaluate this, a simple test case was constructed where only odometery measurements and loop-closure constraints were added to the pose graph. The test graph can be seen below.
Simple odometry test case
With this simple test case, both $L_2$ and Huber optimization were tested. First, the graph was optimized using the $L_2$ cost function. The optimized graph and covariance of each node can be seen in the figure below.
$L_2$ optimization with no false constraints
Next, the Huber robust noise model was utilized. Again, the optimized graph and associated covariance of each node can be seen in the figure below. As would be expected, there is no substantial difference between the optimized solutions.
Huber optimization with no false constraints
Now, a false loop-closure constraint is added between nodes two and four — the optimizer believes that these nodes should be zero meters apart with an uncertainty of 0.2 meters on position. The new initial pose graph is depicted below.
Odometry test case with one false loop-closure
Using the pose graph with a false loop-closure constraint as depicted above, $L_2$ optimization is performed again. When processing a graph that containts faults, it can clearly be seen in the figure below that the position solution is skewed by the false constraint; however, this does not correspond to a substantially larger uncertainty in the estimated state.
$L_2$ optimization with false constraint
Optimizing the same graph with the Huber noise model shows substancial imporvement with respect to positioning accuracy when compared to $L_2$ optimization. Additionally, the uncertainty in the state is increase, which is desired.
Huber optimization with false constraint