Updates for robust incremental graph optimization.

Hopefully, this will be utilized for an IROS 2019 submission. Due date: 24th-Feb.