My fork of the Georgia Tech Smoothing And Mapping toolbox. Forked from version 2.3.1