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.

added To Do (release) enhancement labels

changed the description

changed title from Problem getStateWithCovariance() to New API Problem::getStateWithCovariance()

changed title from New API Problem::getStateWithCovariance() to New method Problem::getStateWithCovariance()?

I wonder if we can avoid making ProcessorMotion::statePlusDelta(...,Jacobians) non pure-virtual so that it is not mandatory for all processors deriving from it to implement it.

But maybe it's at all cases interesting to have this functionality. I am not sure. Since asking for covariances requires calls to Ceres::computeCovariance(), and this might be an expensive step, my concern is whether we implement it for everyone, or just for some of the processors for debugging and visualization purposes.

What are you opinions?

Otherwise, I completely agree with getStateWithCovariance().

I agree on implementing this new method, it seems very useful and relevant. Also, the analogy with getState logic has a lot of sense.

The problem here is that the current design does not guarantee that the frame covariance is stored in Problem.

The problem has a std::map in which has some covariance matrix blocks stored. But it only contains the covariance matrix blocks that have been computed by the solver. The blocks that are computed are set by the arguments given when calling SolverManager::computeCovariances() from the "outside" (for example the ROS node).

Now SolverManager contains a Problem pointer, but not the opposite. So, the processors are not able to ask for some covariance blocks to be computed.. Maybe, a different issue should be opened to discuss if we need a different implementation to allow some communication of which covariance matrix blocks are needed by the processors.

For now, it can be done knowing from the outside that the last key-frame covariance should be computed. It is ok for you?

mentioned in issue #197 (closed)

Regarding the ProcessorMotion::statePlusDelta(...,Jacobians) for all current processors, we can make getStateWithCovariance() a method of ProcessorMotion and make it non pure virtual so that it is only implemented in child classes that have a ProcessorMotion::statePlusDelta(...,Jacobians). It will lead to code repetition but eventually we will move the implementation to processor_motion.cpp.

Since we want to trigger covariance computation from processor motion or Problem, I think a pointer to ceres manager in problem would be mandatory, isn't it?

I think that a pointer to the SolverManager (whichever it is) would make multi-threading more complicated. The only information that is needed by the SolverManager is the list of desired covariances (they are known a priori), let's discuss this in a different issue (#199). The good think is that, for now, it can be temporarily solved by computing the corresponding covariances from the ros node (or the main).

Any new thoughts on this? @mederic_fourmy ? @joanvallve ? I am about to forget about this issue. Mederic, how did it end up going for you?

mentioned in issue #199

For the use that I needed it for (drawing ellipses around estimated Apriltag corners), the estimation was too slow to be used in real time so I dropped this line of work, but I think my implementation could be improved. I still think this is a relevant thing to have.

added Idea label and removed To Do (release) label