Compatible with gtsam or ISAM2

[YangSiri]

Hi, @dongjing3309

I found your work quite amazing. So I added it to my SLAM framework as a strong back-end. That is I build graph connecting poses by your GP factor, specifically gpslam::GaussianProcessPriorPose3, but the process gets stucked before optimizing. Though I've tested your demo offline, I wonder what my problem should be. And also it didn't work while using ISAM2. Could you please give me some advice on these? Thanks very much!