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 and #450 (closed) proposing Sensor constructors taking
ParameterServer
as argument - #364 and #449 asking for checks on the user defined states
-
#381 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 proposing remove
ExtrinsicsSize
from theWOLF_SENSOR_CREATE
macro and inferring it from problem size.
The proposal to address all these issues partially or completely is the following:
- A new class
Prior
containing 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
ParameterServer
or takingPrior
s andParams
. The second ones to be used in tests. -
SensorBase
ParameterServer
constructor 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