Odometry disconnect at the end of long chains

[mc2922]

Odometry chains break towards the end of a longer chain?

Range-only factors, 190 Point2:
ex2-1.pdf
ex2-2.pdf

Range-only factors, 200 Point2:
ex1-1.pdf
ex1-2.pdf

See example script
here, with minimum number 171 point2s:
https://github.com/JuliaRobotics/Caesar.jl/blob/dynSAS/examples/marine/asv/kayaks/testRangeOnlyOdo.jl

Bayes Tree:
bt.pdf

[dehann]

Depiction of the Bayes tree that associated with this issue. The short branch has a prior on 'X', but that prior information is not properly distributed down the long sibling branch.

[dehann]

another anecdotal piece of information is that the problem also occurred on a single long branch.

[dehann]

Think this may be related to JuliaRobotics/IncrementalInference.jl#244 which has been postponed for the DFG and CSM upgrades.

[dehann]

From the photo above, the triangle is x169, square is x170. When looking at the clique low down on right hand branch (x169), the following subgraph is obtained. Note priors represent the downward belief messages during the final stages of the complete solve. This is where the disconnect in odo [x169 -- x170] seems to be occurring:

[dehann]

All these loose fragments should also be computed in parallel btw.

[dehann]

Ha, a little more progress. By looking closer I found that odometry factor x168--x169 is working the wrong way round for some reason,

and then found this. The order of fields x168 and x169 is important here, but clearly wrong:

@GearsAD , do you have any ideas here -- think we spoke a little while back on this but I cannot recall what we decided to do about fncargvID?

Values in factor seem fine:

Oh, and the factor graph here sfg31bd stands for sub factor graph inside cliq 31 before the down solve is performed.

[dehann]

this does not seem to be present in the original factor graph: