I'm working on Graph SLAM to estimate robot poses (x, y, z, roll, pitch, yaw). Now I want to integrate GPS measurement (x, y, z, of course no angles).
I implemented GPS as pose's prior. But I have a problem.
- Position(x, y, z) is perfectly corrected by graph optimization
- But orientaiton(roll, pitch, yaw) is very unpredictable(unstable) after optization.
i.e. It looks like position is fitted by the sacrifice of orientation.
I'm very confused about what's the right way of integrating GPS into graph SLAM. GPS should be handled as prior? or landmark? or one of pose-vertices?
...Thanks for your help in advance.
I use g2o as a graph-optimization library. In g2o, I implemented GPS measurement with EdgeSE3_Prior. GPS's quality is RTK so it's enough precise.