Problem when using multiple ProcessorMotion
Processors inheriting from ProcessorMotion
permits to preintegrated measurements from high rate sensors to define factors between distant Keyframes while producing a state estimate at sensor rate using debiased data (getCurrentState()
). This class was originally thought as a generalization of the IMU preintegration framework to other sensors sharing the same structure: ProcessorOdomXD
.
However, the current implementation only accounts for the possibility to use a single ProcessorMotion per problem. This implicit design choice, fostered by the common use case that we have encountered so far, is reflected by several features:
- In
Problem::installProcessor
, the problem classprocessor_motion_ptr_
attribute is set to the first processor motion to be installed or stays nullptr if no processor motion is used - In
Problem::getCurrentState
returns the state at the last data timestamp of thisprocessor_motion_ptr_
, not considering other processor motions that could have better or more recent estimates
In my case, the processorForceTorquePreint
that I am implementing gives information about a very different set of variables ("CDL" for COM, COM vel, Angular Momentum) than the ProcessorIMU
(POV) so it would be necessary to somehow acces both at high rate.
Another more problematic issue is the fact that the processor state, that is to say the stateblock vectors that are constrained by the motion factors and propagated by the processor (x_k = x_i + \Delta_ik) is suposed to be identical to the frame state block vector while they are not in general. For instance a problem using a ProcessorOdom3D
has structure PO while one using ProcessorIMU
has structure POV. This structure is defined when creating a KF using a string argument _frame_structure
or usin the Problem structure frame_structure_
.
However, if we were to use both processors in a single problem, the implementation of processorMotion would break since:
-
ProcessorMotion::getCurrentState()
retrieves the full state of the last KF and then applies the statePlusDelta which might not be adapted to the chosen state structure - When creating a new frame using
getProblem()->emplaceFrame(...)
, the processor state estimate might not be of the same size as the Keyframe state.
The first of the 2 item might be mitigated by implementing a new FrameBase::getState(std::string _frame_structure)
using a different std::string frame_structure_
attribute being stored in each processorMotion (ProcessorIMU: "POV", ProcessorOdom3D: "PO"). However, it seems that the second item is harder to deal with the current structure. A new frame could be initialized containing all the problem stateblocks but with only part of them being set to the current estimate, the other being at a default value.
Sorry for the long read, hard topic to summarize.