Complete state vector new data structure?
At several point of the codebase, the complete state of a frame (or Landmark, or capture -> any object inheriting from HasStateBlock
) needs to be passed. Examples include Problem::getState(Timestamp ts)
where it represents the state estimation at a given timestamp, the FrameBase::FrameBase(...)
constructor where is represents an initial value etc. This state is passed as a Eigen::VectorXd
, assuming that the receiver "knows" to what structure it refers (PO, POV, POVCDL and not PVO for instance). This is more than enough for cases where this structure is obvious and we currently enforce it to be either PO or POV at the problem creation. I have struggled to adapt this model to the more complex case of multiple processorMotions acting on different states because in this case:
- processorMotion each return only a part of the estimated state
- their inner propagated frame is created by initializing only the corresponding stateblocks
I would argue that using a data structure describing by itself the structure of the state being passed whenever something else than a single stateblock value is passed would make our life easier. Building on the ideas of HasStateBlock
, I think that using a std::unordered_map<std::string, Eigen::VectorXd>
representing a dictionnary of {state_block_name, sb_value} would be a good candidate.
For instance, Eigen::VectorXd Problem::getCurrentState()
would become const std::unordered_map<std::string, Eigen::VectorXd>& Problem::getCurrentState()
Even in the more normal usual setting, we never (to my knowledge) do computation using this full state vector, rather retrieving segments of it to do calculations. It would also make things easier to handle for end users instead of having to write things like:
VectorXd full_state = problem->getCurrentState(); Vector3d angular_momentum = full_state.segment<3>(16);
we just write
Vector3d angular_momentum = problem->getCurrentState()["L"];