YAML specification of states / covariances

added To Do (release) label

depends on #293 (closed)

assigned to @jsola

This is my proposal.

1. State blocks are identified by a string. Currently, the strings are:

"P" position
"O" orientation
"V" velocity
"W" angular velocity
"I" intrinsic params

These strings will possibly evolve. We could establish a 3-letter code, case-insensitive, as follows

"pos", "Pos" and "POS"
"ori"
"vel"
"ang"
"itr"

Or, we can use full names, also case-insensitive

"Position"
"Orientation"
"Velocity"
"AngularRate"
"Intrinsics"

The problem with this last option is that it is easy to make typos and miss the point. For example, "Intrinsics" or "Intrinsic"? "AngularRate" or "AngularVelocity" ? etc.

2. Each state block vector has a vector. This is a regular YAML list of numbers.

  [1, 2, 3]

3. Each state block covariance has a matrix. This is as the matrices we have defined in YAML up to now,

  [[rows, cols] 1, 2, 3, 4, 5, 6, 7, 8, 9]

4. Finally, the specification is as follows (the example is for a 3D pose)

state:
  Pos: [1,2,3]
  Ori: [0,0,0,1]
covariance:
  Pos: 
    Pos: [[3,3] 1,2,3,4,5,6,7,8,9] # Cov_P_P
    Ori: [[3,3] 1,2,3,4,5,6,7,8,9] # Cov_P_O
  Ori:
    Pos: [[3,3] 1,2,3,4,5,6,7,8,9] # Cov_O_P 
    Ori: [[3,3] 1,2,3,4,5,6,7,8,9] # Cov_O_O

5. Very often, specified covariances are diagonal. In this case, it is better to specify a vector of standard deviations:

std:
  Pos: [1,2,3]
  Ori: [4,5,6]

6. Using only sigmas

state:
  Pos: [1,2,3]
  Ori: [0,0,0,1]
sigmas:
  Pos: [7,8,9] # Cov_P_P
  Ori: [7,8,9] # Cov_O_O 

I have been doing some testing with converter.h and now we could do something like this:

sparse_block_matrix:
    (1,2): [1,2,3,4,5,6,7,8,9]
    (3,1): [1,2,3,4,5,6,7,8,9]
    (2,2): [1,2,3,4,5,6,7,8,9]

Then we could say that sparse_block_matrix is a block matrix, say 5x5, where each block is 3x3. Each entry of this map is one of the non-null block matrices, and thus the missing entries are assumed to be the 0_5x5 matrices. Then, we could ask the param server in this way:

_server.getParam<map<WPair, Eigen::Matrix<3,3,double>>("sparse_block_matrix");
//The WPair is a bit of a hack that we can discuss, should this be useful

Don't know if this is useful, just throwing it out there.

closed