Skip to content

SensorBase: extrinsic and intrinsic initialization and prior

We have to implement a generic way to specify priors in extrinsic and intrinsic in SensorBase (and the drift of intrinsics in case of dynamic ones). The thing is that it is overlapped with the current way of specifying the extrinsic (P and O together in the same vector "pose").

EDIT JS-28/4/22: todo list

  • Use the vector composite definition in all yaml-specified extrinsics
  • Use the vector composite definition in all yaml-specified priors
  • Use the vector composite definition in all yaml-specified landmark states
  • check quaternion norm for yaml-defined values.
    • Must be better than 1e-4.
    • Normalize if it is better than 1e-4.
    • Error if worse than 1e-4.
    • Remove quaternion normalization from the StateQuaternion constructor
  • Allow priors to be defined independently for all state blocks:
    • extrinsics and intrinsics,
    • either static and dynamic

I think that this should be implemented coherently with the prior to the problem (the first frame), which is:

prior:
  mode: "fix"   # can be "nothing", "initial_guess", "fix" or "factor"
  $state:       # Only required in case of "initial_guess", "fix" or "factor"
    P: [0,0]
    O: [0]
  $sigma:       # Only required in case of "factor"
    P: [0.1,0.1]
    O: [0.01]

The current YAML for sensors is

sensor:
  [...]
  extrinsic:
    pose: [0,0, 0]

And it follows to the API of the sensor constructors forced by the 2 macros WOLF_SENSOR_CREATE (one requires the extrinsics vector and the other its size). This implementation can be simplified if we design this new specification of extrinsic and intrinsic using VectorComposite. Also, we can add easily (and coherently) the prior to both extrinsic and intrinsic state blocs. Would be something like this:

sensor:
   [...]
   extrinsic:
      prior_mode: "factor"    # can be "initial_guess", "fix" or "factor" ("nothing" doesn't have sense..?)
      $state:                 # that would replace the current 'pose'
         P: [0, 0]
         O: [0]
      $sigma:                 # Only required in case of "factor"
         P: [0.1, 0.1]
         O: [0.01]
   intrinsic:
      prior_mode: "factor"    # can be "nothing", "initial_guess", "fix" or "factor" (I don't know if "nothing" has sense...)
      $state:                 # Only required in case of "initial_guess", "fix" or "factor"
         I: [0, 0]
         T: [0]
      $sigma:                 # Only required in case of "factor"
         I: [0.1, 0.1]
         T: [0.1]
      dynamic: true           # This only involves 'I'
      drift_sigma: [0.1, 0.1] # Only required in case of dynamic=true

This allows the sensor to have different intrinsic parameters. The dynamic and drift functionality would be only compatible with the I state block.

In the constructor of SensorBase, this information would be accessible in the ParamsSensorBase and the corresponding state blocks for extrinsics and intrinsics and the priors will be emplaced.

This would change several files.

Another issue that has to be addressed is how the keys and the sizes provided by the user will be checked (what if the user does not specify 'I'? or the sensor requires more intrinsics...?). To do so, the derived sensor should provide to the SensorBase constructor the keys required and its sizes. I think that a map of keys and sizes could be a typedef SizeComposite.

Edited by Joan Solà Ortega