HasStateBlocks::addStateBlock() needs to register SB's

We use a ProblemPtr argument.

• if nullptr, do not register
• if valid, register

added To Do (release) label

Also, we need to remove the registration calls from the Node::link() places. And take care to update the places where Frames become Kfs and vice-versa.

created merge request !341 (merged) to address this issue

mentioned in merge request !341 (merged)

mentioned in issue #285 (closed)

How is this compatible with registerStateBlocks()? Could it happen that when calling it some of them are already registered? Is it an issue? We have to check if registering twice a stateblock is a problem.

Yes, registerStateBlocks() should check and not re-register SB's that are already registered, or pending for registration. I don't know how to do it, though. Is it possible with the current design?

Otherwise, maybe registerStateBlocks() could disappear completely... this might be even cleaner.

What about this?

Regarding notifications, the function problem->notifyStateBlock() already checks if a previous notification exists.

inline wolf::StateBlockPtr HasStateBlocks::addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem)
{
    assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
    state_block_map_.emplace(_sb_type, _sb);
    if (!isInStructure(_sb_type))
        appendToStructure(_sb_type);

    // conditionally register to problem
    Notification notification;
    if(_problem)
        _problem->notifyStateBlock(pair_key_sbp.second, ADD);

    return _sb;
}

I added a gtest for multiple registration and it works.

@joanvallve would you mind reviewing this MR ?

Here's a quick explanation of what's done:

1. Modify HasStateBlocks::addStateBlock(key, sb) --> (key, sb, problem). If problem is not nullptr, register the SB.

2. Keep registerStateBlocks() as is. It is still required.

3. In FrameBase, overload addStateBlock(key, sb).

   • If KF, then use getProblem() to pass to addStateBlock(key, sb, getProblem()). Else, pass nullptr.
   • Keep using registerStateBlocks() since sometimes (e.g. in the constructor) the SB are added but there is no Problem yet. Registration will be made as before at the time of emplacing / linking.

4. In SensorBase, overload addStateBlock(key, sb). This is related to the dynamic state blocks that may end up in Captures.

   • In the implementation, use getProblem() to pass to the base addStateBlock(key, sb, getProblem())

5. Write a couple of tests for registration issues:

   • Make a Frame and link it to Problem -> SBs not registered
   • Set it keyframe -> SBs registered
   • Make a new Frame and link it to Problem -> SBs not registered
   • Add a new SB --> not registered
   • Set KF --> all registered
   • Add a new SB --> registered

Just one thing to clarify, Problem doesn't keep a list of state blocks and factors registered to the solver (maybe it should), it just keeps the notifications that are pending to be consumed by the solver.

In other words: problem->notifyStateBlock(sb1, ADD) -> notifies the addition of sb1 (stored in notifications in Problem) problem->notifyStateBlock(sb1, ADD) -> duplicated addition handled by Problem since the solver hasn't been notified yet. solver->update() -> notifications from Problem are consumed, i.e. no notifications remains problem->notifyStateBlock(sb1, ADD) -> duplicated addition handled by Solver (I don't know if an error is thrown)

Two comments:

• I don't think registerStateBlocks() can be removed, this functionality is still required.
• Maybe a copy of the state blocks and factors registered in the solver should be also stored in problem, to properly check this duplicated addition or removal of not registered state block.

Can Problem ask SolverManager to ask Ceres to report the list of registered SB's and Factors?

Yes we do need and will keep resiterNewStateBlocks()

FYI: The notifications are kind of additive, as so:

- ADD + ADD = ADD
- REMOVE + ADD = notthing, no notification
- ADD + REMOVE = nothing, no notification
- REMOVE + REMOVE = REMOVE

This is implemented in problem->notifyStateBlock()

added ~209 label and removed To Do (release) label

@joanvallve do you think this can be merged regardless of the comments above, which would involve extra work?

All compiles and all ctests pass. Also, all plugins complie and pass ctests based on this branch in core.

@mederic_fourmy , once this MR is merged to core/devel, you will have to remove all calls to notifyStateBlock() in your plugin bodydynamics.

So far, I only find them in your gtests. Remove them and report any issue that you may have related to this.

Yes I removed most of the calls to notify already for other reasons, it should not change much the bodydynamics plugin.

@jsola can you add in the tests (if there is not already tested) what happens with 2 additions with an intermediate update of the solver?

OK

Done, pushed, and passing test. Have a look at the third test.

closed via merge request !341 (merged)

mentioned in commit 8c89dfb9