New method Problem::getStateWithCovariance()?
We should implement a new Problem
method with signature
void Problem::getStateWithCovariance(const TimeStamp& _ts, Eigen::VectorXs& state, Eigen::MatrixXs& cov)
The function would have a logic akin to Problem::getState
:
- If no processor motion, return the last valid keyframe covariance (tested with
success = getFrameCovariance(..)
) - If processor motion, propagate the last valid covariance using the jacobians of
ProcessorMotion::statePlusDelta
The main change would come from the overloading of statePlusDelta
to include the jacobians which would impact all the derived processor motion.
Edited by Mederic Fourmy