StateBlocks notifications

After a meeting with @jsola @asantamaria and @jcasals, and following the directives agreed with them, here you have the corresponding MR for handling of StateBlock (and Coinstraints) notifications to the Problem and the SolverManager.

Objectives

The new implementation has this objectives:

• Simplicity: to avoid bugs and facilitate debugging/future enhancements by other developers.
• As much as self-contained in StateBlock class: Try to make the most notifications automatically in order to reduce relying on the developer to remember to call a notification function after manipulating a state_block.
• New functionality: update local parameterization (removal or adding a new one).

Approach

We differentiate between 2 different nature of events in StateBlocks that have to be informed to the Problem so the SolverManager can be updated:

1. Creation or removal.
2. Update of the state, the status (fix/unfix) or the local parametrization.

In the new implementation, 1 is handled by notifications (ADD/REMOVE) and 2 by flags in the StateBlock. This made the notifications exactly analogous to the notifications of new/removal of Constraints.

The Problem&SolverManager does not know about the existence of a new StateBlock. On the contrary, Problem&SolveManager have a list of StateBlocks already added and are able to check if they have been updated without need of notification lists and so on. Also, it leads to a very simple notifications functionality (ADD/REMOVE) that simplified a lot the code.

The following is the description of the main changes, they concentrated in 3 classes (and some tests):

StateBlocks

There are the three corresponding update flags (atomic_bool to keep thread safe implementation) with the corresponding getters:

• state_update_: set to true in the StateBlock::setState(_state, const bool _notify = true) unless notify is false (this happens when the SolverManager copies back the optimization result).
• fix_update_: set to true in StateBlock::setFixed(bool _fixed) only if its status is actually changed.
• local_param_update_: set to true in setLocalParametrizationPtr(LocalParametrizationBasePtr _local_param) and removeLocalParametrization(). For each flag, the resetXXXUpdated() function sets to false. This approach allow us not to rely on the developer to inform about state/fix/local parametrization updates to the problem. They are handled automatically.

Since now, notifications are only ADD and REMOVE, they are stored and handled by Problem (see next point). The atributes notifications_ (and its mutex) and problem_ptr_ have been removed as well as the functions setProblem(), getProblem(), addNotification(), consumeNotifications(), hasNotifications() and getNotifications().

Problem

In Problem, only the ADD/REMOVE notifications are handled leaving the update flags to be handled directly by the SolverManager. The ADD/REMOVE implementation is analogous for both constraints and state blocks. When a StateBlock is created or removed, the Problem is notified and it will notify the SolverManager.

Since adding and then removing a state block and viceversa (removing a stateblock and adding it again) is equivalent to not doing anything, for the same state block (or constraint) only ADD or REMOVE notification can take place, not both. Because of this, a std::map is considered convenient since it also provides a simple and fast way to check for previous not processed notifications for the same state block/constraint (when removing, we rapidly see that there is an ADD notification so we can erase it).

So, StateBlocks and Constraints ADD/REMOVE notifications in Problem are stored in std::map<StateBlockPtr,Notification> state_block_notification_map_ and std::map<ConstraintBasePtr,Notification> constraint_notification_map_. The corresponding functions Problem::addYYY() and Problem::removeYYY() deal with these YYY_notification_map_. Also, the corresponding Problem::getYYYNotificationMap() returning a reference allows the SolverManager to process and erase the notifications.

The ADD/REMOVE notification API didn't entail any changes in any of the WOLF nodes (the registerNewStateBlocks() and removeStateBlocks() in CaptureBase,FrameBase,LandmarkBase and SensorBase remains the same).

SolverManager

A new pure virtual function SolverManager::updateStateBlockLocalParametrization(state_ptr) = 0 has been added to deal with the new functionality.

Only the function SolveManager::update() changed. Now it does the following:

1. Remove constraints
2. Add & remove state blocks
3. Add constraints
4. Update state blocks

(NOTE: Basically, the old implementation was 1, 2+4, 3. Separating 1 and 3 is due to ceres solver issues. Anyway, it is out of scope of the MR).

In 1, 2 and 3, we process the notifications stored in the problem problem_ptr_->getYYYNotificationMap() and erase them afterwards.

In 4, we iterate over the list of StateBlocks checking the update flags (state_ptr->getStateUpdate(), state_ptr->getFixUpdate(), state_ptr->getLocalParamUpdate()). When a flag is true, we perform the corresponding actions (update state, status, local_param) and reset the flag.

Comments

In the meeting, we decided to keep problem_ptr_ in order to automatically handle the ADD and REMOVE notifications (not relying on the developer asking to call problem->addStateBlock(sb) and problem->removeStateBlock(sb), respectively) at exchange of requiring to call setProblem() after creating the StateBlock. However, StateBlock does not have any knowledge of being removed, so it would also be required to call a function to notify the REMOVE. So anyway, each time a state block is created, a function should be called and each time a state block is removed, a function should be called... the problem_ptr is not helping to automatize anything. So I removed it.

All tests passed. However, I might don't have all possible libraries installed so maybe there are some tests that were not executed.