define new tree based down init sequence and test

[dehann]

Parent clique:

• x34, x9, x54, x26, x46, x14, x4 with 5 children:
• x38 | x34, x4, x26, x54, x46, x14
• x30 | x26, x34
• x50 | x46, x54
• x12 | x14, x9
• x6 | x4, x9

Based on lower down information (priors) only x38 can initialize from below. All 4 other sibling cliques must cross initialize through the parent -- i.e. downward init.

By checking the number of intersections of separators between the siblings we find as follows:

• x38 --> x30 => 2, x50 => 2, x12 => 1, x6 => 1

as a triangular matrix:

* 2 2 1 1
* * 0 0 0
* * * 0 0
* * * * 1
* * * * *

Summing triangular matrix along columns gives (AA):

0 2 2 1 2

Summing symmetric version of matrix along columns gives (BB):

6 2 2 2 2

By summing the triangular matrix along rows we find (CC):

6
0
0
1
0

This (CC) implies special significance on cliques x38 and x12, while x30 and x50 are more 'independent'. Next we look at the best achievable status of each of the sibling cliques and find:

• x38, upsolved
• x30, needdown
• x50, needdown
• x12, needdown
• x6, needdown

Therefore x38 will be used for upward information that can be distributed to the siblings and that clique can therefore pass and CSM may proceed to the next stage of x38's solution.

The special case of clique x12 must now be considered. By eliminating x38 as upsolved the triangular matrix reduces to:

* * * * *
* * 0 0 0
* * * 0 0
* * * * 1
* * * * *

Summing along the triangular columns reveal (DD):

0 0 0 0 1

This also Indicates a special interaction between cliques x12 and x6 (row 4, column 5 is non-zero). Knowning that both :needdownmsg information, we must first determine what the interaction between these two cliques should be. Using CC and by looking at the opposite eliminated (x12) triangular matrix, we find:

* 2 2 * 1
* * 0 * 0
* * * * 0
* * * * *
* * * * *

Summing along the triangular columns reveal (EE):

0 2 2 0 1

• Cliques x30 and x50 can readily be initialized with downward information (non-zero columns EE).
• Notice how column for clique x6 in (EE) is also non-zero.
• Notice how column for clique x6 in (DD) is also non-zero.

Both cliques x12 and x6 are equally weighted with one shared variable in their separators -- variable x9 in this case (which doesn't appear in clique x38's separator). This process identifies that one clique may likely have to be initialized before the other and then provide additional downward initialization information for the last remaining branch. The task now is to determine if clique x12 or x6 should be initialized first?

The triangular column sum with x12 rows and columns eliminated shows that clique x6 can also be initialized, after which a second piece of downward initialization information will become available for use in clique x12's branch.

To determine okay for downinit on x12, we repeat the process by eliminating any further solved siblings from the triangular matrix:

Recap:

• assume CSM does not start in upsolved or needdownmsg states (default is null in current code).
• we look at (CC) where !:upsolved -- i.e. eliminate x38,
• to eliminate columns in the matrix and
• produce EE
• which okays downinit on x30, x50, and x6,
  • x12 must wait for two bits of down info based on (BB), from cliques x38 and x6,
  • The actual variable marginal messages are on [[x14]] and [[x9]] from the two cliques.
• x12 is blocked because of the associated non-zero in (CC) and because x6 (EE) is not yet solved.

When x12 returns upon downinit info availability status update via parent x34, EE is updated with all but x12 as upsolved. This implies no further down info will be available (since x6 has now been upsolved), and x12 can now proceed to retrieve down init messages and attempt upsolve. If the tree is correct, the upsolve should succeed.

Future:

• Better prioritize siblings since this is only a trivial case presented here.
  • Need mechanism to either let siblings downinit in parallel or block in sequence as more information becomes available.
  • Partial init states definitely require as much downward information as possible (#335 )

[dehann]

affects

• 339

• 335

[dehann]

[dehann]

• https://github.com/JuliaRobotics/Caesar.jl/blob/7a13c2e35109509d4311ed1bf2117baac13b8f2b/examples/marine/asv/kayaks/rangeOnlySLAM.jl#L1
• IIF 9f151c1a82d1504fc102e5387fda424fab4aed52
• RoME 37febd45121ecb0b249254c6b836df83d087edd0
• DFG 1778ab41222c943e77c154325ca6fd2ac0de6815
• FSM f9347e3db084165c6aef5fc54ea446be874b58b2

[dehann]

Hi @tonioteran , cc'ing in case you were interested in the work on how we solve the autoinit problem on the tree. I'm busy implementing this new fix as improvement over slightly hacky previous version. Variable initialization can be tricky...

[dehann]

implementation is getting a bit nasty, but getting to the final debugging stages now: https://github.com/JuliaRobotics/IncrementalInference.jl/blob/abb857f8e7098d5af9bc6e666fcf899b75f55304/src/CliqStateMachineUtils.jl#L316

[dehann]

was introduced in v0.7.3 but need more -- the inferdim dim work #353

[dehann]

xref #910 and #754