Added -save_pose option. Will save camera pose only if filename is given.