Robust Incremental Smoothing and Mapping (riSAM)