[RST-2437] Ensure that all variables are updated

[svwilliams]

Ensure that all variables are updated when the update_variables flag is set. Previously, only variables involved in motion model segments that needed to be generated were being updated. Variables involved in previously generated segments were left untouched. This could result in incorrect initial state guesses being used for those variables.

As an example, imagine a sensor transaction that connects two adjacent states at T1 and T2, and a second transaction that connects T1 to T3. If both of these transactions are added to the graph at the same time:

• Transaction1 is processed. A motion model segment is generated from T1 to T2. Since states T1 and T2 are involved in the newly generated motion model segment, their states are updated. No other states are involved in this transaction, so all states have been updated.
• Transaction2 is processed. A motion model is generated from T2 to T3. Since a motion model was previously generated from T1 to T2, the motion model does not need to regenerate that segment. Since states T2 and T3 are involved in the newly generated motion model segment, their states are updated. However, the transaction also involves the state at T1. It was not involved in the generated motion model segment, so its state has not been updated. The original state from Transaction2 overwrites the updated state from Transaction1. This can lead to a poor initial guess being sent to the optimizer.

With this PR, if the update_variables flag is set, any variable from a previously generated motion model segment that is also involved in the current transaction is updated from the value stored in the motion model history.

[efernandez]

Maybe do auto last_stamp as you do with first_stamp

[efernandez]

In C++14 you can do:

[variable_uuid = variable->uuid()](const auto& input_variable)
{
  return input_variable.uuid() == variable_uuid;
}