Evaluating Covariance Estimate from M-Estimators

After yesterday’s test, there was some doubt as to how the M-Estimator is scaling the covariance matrix. To evaluate if this is an actual issue in the code or just an issue with me not understanding how GTSAM applies the M-Estimators weight to the information matrix, I tested multiple M-Estimators to see if the trend from yesterday persists.

incorrectOdo photo odometryTestCorrect_zpsgtdn0vsr.png

Fig 1 :: Odometry test case with one false loop-closure


Using the same pose graph that was utilized yesterday, which is shown above, the $L_2$, Huber, DCS, and Geman-McClure M-Estimators were tested.

First, the case were no false constraints are added to the graph was tested. As expected, when no false constraints are added to the graph, all estimators provide the same covariance estimates. The final solution is shown in the figure below ( Note :: all four estimators overlap ).

l2WithFault photo l2WithFault_zpsza8q9fee.png

Fig 2 :: Optimized no fault pose-graph and associated covariance estimates for all estaimators


Next, we test the case that is shown in Figure 1 where one false constraint is added to the graph. First, we will test $L_2$ optimization, which is shown in the figure below. Obviously, this is not a good estimate with respect to node position or estimated uncertainty in the state.

l2WithFault photo l2WithFault_zpsza8q9fee.png

Fig 3 :: $L_2$ optimization with faultly pose-graph and associated covariance estimates


Now, we test the same graph with three M-Estimator: DCS, Huber, and Geman-McClure ( Because the kernel width equals 1 the DCS and Geman-McClure solutions should be exactly the same. ). The results for the M-Estimators are shown in Figure 3. Note, that I only ploted the DCS & Huber solutions becase the DCS and Geman-McClure solutions matched.

m_est photo m_estWFaultUpdate_zpsrebenk5r.png

Fig 4 :: Optimization of faultly pose-graph and associated covariance estimates for M-Estimators


Testing Sensitivity to Kernel Width

Next, the sensitivity of the Huber cost function to the user defined kernel width is tested. The sensivity is depicted in the image below. Obviously, the kernel width can have quite an affect on the solution. I’m going to start looking into adaptive M-Estimators that can adjust kernel width during optimization.

kernelWidth photo kernelWidth_zpsiu1z846r.png

Fig 5 :: Huber cost function sensitivity to kernel width




ISAM

Started playing with Michael Kaess’s stand alone ISAM library. Hopefully we will port the robust model that we select to this library so that we can utilize ISAM’s incremental optimization algorithm, which is used in real-time operation on many systems.



Installing ISAM on Ubuntu 16.04


$ svn co https://svn.csail.mit.edu/isam
$ sudo apt-get install cmake libsuitesparse-dev libeigen3-dev libsdl1.2-dev doxygen graphviz
$ cd ~/isam; mkdir build; cd bulid; cmake ../; make install



Inital Test

To run an initial example ( NOTE:: if libsdl1.2-dev did not install properly you will not be able to use the GUI. )


$ ./bin/isam data/victoriaPark.txt -G

The ouput should look similar to this