Implementation of new nodes creation
This issue involves several different issues that can/should be implemented at once:
- #319 (closed) proposing more flexibility on the priors
-
#318 (closed) and #450 (closed) proposing Sensor constructors taking
ParameterServeras argument - #364 (closed) and #449 (closed) asking for checks on the user defined states
-
#381 (closed) proposing a new way of initialize the states of
SensorBase -
#452 (closed) proposing a new API for sensors,frames and landmarks to accept
VectorComposite -
#376 (closed) proposing remove
ExtrinsicsSizefrom theWOLF_SENSOR_CREATEmacro and inferring it from problem size.
The proposal to address all these issues partially or completely is the following:
- A new class
Priorcontaining all information needed for applying a prior in a state block:
class Prior
{
string prior_mode;
VectorXd state;
VectorXd sigma;
bool fixed;
bool dynamic;
VectorXd sigma_drift;
}
-
Sensors constructors will be of two types: taking the
ParameterServeror takingPriors andParams. The second ones to be used in tests. -
SensorBase
ParameterServerconstructor will be the responsible of extracting information to build thePrior, for that it will require the derived sensor keys (all the states that have to be asked to the param server). Here, some checks can be done (sizes and quaternion normalized). -
In the YAML file, the user will have to define the priors in the following way:
sensor:
[...]
states:
P:
prior_mode: "factor" # can be "initial_guess", "fix" or "factor" ("nothing" doesn't have sense..?)
state: [0, 0] # that would replace the current 'pose'
sigma: [0.1, 0.1] # Only required in case of "factor"
O:
prior_mode: "fix"
state: [0]
I:
prior_mode: "factor" # can be "nothing", "initial_guess", "fix" or "factor" (I don't know if "nothing" has sense...)
state: [0, 0] # Only required in case of "initial_guess", "fix" or "factor"
sigma: [0.1, 0.1] # Only required in case of "factor"
dynamic: true
drift_sigma: [0.1, 0.1] # Only required in case of dynamic=true
T:
prior_mode: "nothing"
dynamic: true
Edited by Joan Vallvé Navarro