How to avoid jumps just after new frames
Depending on the sensor setup and the keyframe generation policies and others, the estimation (Problem::getState()
) has jumps.
This is normal since normally (ideally) before adding a keyframe, the solver has converged to the optimal solution. But after adding a frame and some factors the factor graph changed and the solver will provide a new solution that in general doesn't have to be close to the previous frame + motion. This is a jump (discontinuity) in getState()
.
But there is an special case that may be more tricky. Since the solver has its own thread (is not called sequentially after processing captures), the solver may solve for example a factor graph that contains a new frame with a GNSS factor but without the odometry factor yet (it will be added after processing the next odom capture). This is unlikely but it happens, sometimes.
The jumps are typical in localization algorithms but they may be bad for navigation. I think this issue includes two different cases and it should have two different solutions:
- Jumps because of changes in factor graph - We may want to add an optional parameter in
PublisherTf
to make smoother the TF correction (map frame to odom frame). - Jumps because of incomplete factor graph - We should explore a mechanism to allow frames to be added to the factor graph. For example, a parameter in all processors
join_keyframe_mandatory
and a flag in new frames preventing to add it to the solver before all required processors have consider joining to it.
Thoughts?