Needed covariance blocks by processors

Now, 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).

However, some processors may need some covariance matrix blocks to be computed.

Now SolverManager contains a pointer to Problem, but not the opposite. So, from the wolf tree, we are not able to ask for some covariance blocks to be computed..

In my opinion, adding in Problem a pointer to the SolverManager is not desirable. It would make multi-threading quite more complicated. Also, it is dangerous allowing the threads of the processors to call SolverManager expensive functions like computeCovariance() or solve().

I think we should add some notification (analogous to the new/removed StateBlocks and Factors) or any other Problem attribute to be read and handled by the SolverManager when its thread calls solve().

added Idea label

mentioned in issue #196

I also consider undesirable to allow processors to call SolverManager::computeCovariances() directly. So the notification strategy is maybe a good compromise.

From the processor standpoint, one needs to consider that the behavior is not synchronous. You can ask for covariances to be computed via notifications, but you will have to wait until solve() is called by someone else, and your application will need also to call SolverManager::computeCovariances().

Also, a test for these covariances to exist will be required if we need them at the processor. Currently there is this bool success which I think can be used for this.

Perhaps, a control boolean in SolverManager could control whether we call computeCovariances() at the end of each call to solve(), or not.

The thing here is not about either calling or not computeCovariances() but about which covariance matrix blocks. The current implementation has 2 different functions:

• computeCovariances(StateBlockPtrList): computes the diagonal and off-diagonal blocks corresponding to all StateBlocks in the list combinations.
• computeCovariances(CovarianceBlocksToBeComputed): which is an enum that now can take this different values:
  • ALL: All marginals and all cross-covariances (the whole covariance matrix).
  • ALL_MARGINALS: All marginals (the block diagonal matrix).
  • ROBOT_LANDMARKS Marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks.

(When we say landmark or frame marginals, it means block diagonal and off-diagonal blocks of all its state blocks)

Correct me if I am wrong, but I think that instead of an specific frame or landmark, the processors will want the covariance of the last one, all of them, just the trajectory... this kind of semantic selection that can be known a priori by the solver and computed each time SolverManager::solve() is called. I think we need that kind of behavior instead of the not synchronous one.

I imagine that we should switch to a more "accumulable" wishlist to be filled by processors and/or user in an accumulation sense (i.e. a processor ask for the last frame to be computed, another processor ask for all landmarks and the last frame and the user ask for the trajectory marginals..). We could adapt CovarianceBlocksToBeComputed to this functionality and include it in Problem along with some machinery while installing a processor (this should add a new parameter in ParamsBase).

I think that the covariance matrix could be divided in the following "pieces":

• MARGINALS LAST KF
• MARGINALS TRAJECTORY
• MARGINALS LANDMARK
• CROSS LAST KF - LANDMARKS
• CROSS TRAJECTORY
• CROSS LANDMARKS
• CROSS TRAJECTORY LANDMARKS

Maybe, CovarianceBlocksToBeComputed should be converted to a struct of these booleans..?

In c, they use a binary mask to indicate options.

In c++ there should be something similar. But an enum is not great, unless we code it binary:

ROBOT MARGINAL = 1
LMK MARGINALS = 2
ROB - LMK CROSS = 4
...

then one can do things like ROB MARG || LMK MARG || ROB-LMK CROSS to indicate that we want these three options. There might be a type for this kind of things, I dont know.

This issue seems very much related to #196.

Both seem to be stall.

It is a neat feature to have. Although I feel its usage quite complicated, and also a safe implementation that guarantees that the covariances, when accessed, have meaningful contents (i.e. they have been computed short ago)...

mentioned in issue #360