Added option to save localization and ground truth poses to file.