Dead Reckon Tether

Towards real-rime location prediction and model based target tracking. See brief description in this presentation.

Towards Real-Time Non-Gaussian SLAM from Dehann on Vimeo.

DRT Functions

Overview of related functions while this documentation is being expanded:

  • addVariable!(fg, :drt_0, ..., solvable=0)
  • drec1 = MutablePose2Pose2Gaussian(...)
  • addFactor!(dfg, [:x0; :drt_0], drec1, solvable=0, graphinit=false)
  • accumulateDiscreteLocalFrame!
  • accumulateFactorMeans
  • duplicateToStandardFactorVariable

DRT Construct

The idea is that the dead reckong tracking method is to update a single value based on high-rate sensor data. Perhaps 'particles' values can be propagated as a non-Gaussian prediction, depending on allowable compute resources, and for that see approxConvBelief. Some specialized plumbing has been built to facilitate rapid single value propagation using the factor graph.

Suppress w/ solvable

The construct uses regular addVariable! and addFactor! calls but with a few tweaks. The first is that some variables and factors should not be incorporated with the regular solveTree! call and can be achieved on a per node basis, e.g.:

fg = initfg()

# a regular variable and prior for solving in graph
addVariable!(fg, :x0, Pose2) # default solvable=1
addFactor!(fg, [:x0;], PriorPose2(MvNormal([0;0;0.0],diagm([0.1;0.1;0.01]))))

# now add a variable that will not be included in solves
addVariable!(fg, :drt0, Pose2, solvable=0)

A Mutable Factor

The next part is to add a factor that can be rapidly updated from sensor data, hence liberal use of the term 'Mutable':

drt0 = MutablePose2Pose2Gaussian(MvNormal([0;0;0.0],diagm([0.1;0.1;0.01])))
addFactor!(dfg, [:x0; :drt0], drt0, solvable=0, graphinit=false)

Notice that this factor is also set with solvable=0 to exclude it from the regular solving process. Also note the graphinit=false to prevent any immediate automated attempts to initialize the values to connected variables using this factor.

Sensor rate updates

The idea of a dead reckon tether is that the value in the factor can rapidly be updated without affecting any other regular part of the factor graph or simultaneous solving progress. Imagine new sensor data from wheel odometry or an IMU is available which is then used to 'mutate' the values in a DRT factor:

# continuous Gaussian process noise Q
Qc = 0.001*diagm(ones(3))

# accumulate a Pose2 delta odometry measurement segment onto existing value in drt0
accumulateDiscreteLocalFrame!(drt0,[0.1;0;0.05],Qc)

Dead Reckoned Prediction

Using the latest available inference result fg[:x0], the drt0 factor can be used to predict the single parameteric location of variable :drt0:

# can happen concurrently with most other operations on fg, including `solveTree!`
predictDRT0 = accumulateFactorMeans(fg, [:x0drt0f1;])

Note also a convenience function uses similar plumbing for integrating odometry as well as any other DRT operations. Imagine a robot is driving from pose position 0 to 1, then the final pose trigger value in factor drt0 is the same value required to instantiate a new factor graph Pose2Pose2, and hence:

# add new regular rigid transform (odometry) factor between pose variables 
duplicateToStandardFactorVariable(Pose2Pose2, drt0, fg, :x0, :x1)
Warning

(2021Q1) Some of these function names are likely to be better standardized in the future. Regular semver deprecation warnings will be used to simplify any potential updates that may occur. Please file issues at Caesar.jl if any problems arise.

Function Reference

RoME.duplicateToStandardFactorVariableFunction
duplicateToStandardFactorVariable(
    ,
    mpp,
    dfg,
    prevsym,
    newsym;
    solvable,
    graphinit,
    cov
)

Helper function to duplicate values from a special factor variable into standard factor and variable. Returns the name of the new factor.

Notes:

  • Developed for accumulating odometry in a MutablePosePose and then cloning out a standard PosePose and new variable.
  • Does not change the original MutablePosePose source factor or variable in any way.
  • Assumes timestampe from mpp object.

Related

addVariable!, addFactor!

source
RoME.accumulateDiscreteLocalFrame!Function
accumulateDiscreteLocalFrame!(mpp, DX, Qc; ...)
accumulateDiscreteLocalFrame!(mpp, DX, Qc, dt; Fk, Gk, Phik)

Advance an odometry factor as though integrating an ODE – i.e. $X_2 = X_1 ⊕ ΔX$. Accepts continuous domain process noise density Qc which is internally integrated to discrete process noise Qd. $DX$ is assumed to already be incrementally integrated before this function. See related accumulateContinuousLocalFrame! for fully continuous system propagation.

Notes

  • This update stays in the same reference frame but updates the local vector as though accumulating measurement values over time.
  • Kalman filter would have used for noise propagation: $Pk1 = F*Pk*F' + Qdk$
  • From Chirikjian, Vol.II, 2012, p.35: Jacobian SE(2), Jr = [cθ sθ 0; -sθ cθ 0; 0 0 1] – i.e. dSE2/dX' = SE2([0;0;-θ])
  • DX = dX/dt*Dt
  • assumed process noise for {}^b Qc = {}^b [x;y;yaw] = [fwd; sideways; rotation.rate]

Dev Notes

  • TODO many operations here can be done in-place.

Related

accumulateContinuousLocalFrame!, accumulateDiscreteReferenceFrame!, accumulateFactorMeans

source
IncrementalInference.accumulateFactorMeansFunction
accumulateFactorMeans(dfg, fctsyms; solveKey)

Accumulate chains of binary factors–-potentially starting from a prior–-as a parameteric mean value only.

Notes

  • Not used during tree inference.
  • Expected uses are for user analysis of factors and estimates.
  • real-time dead reckoning chain prediction.
  • Returns mean value as coordinates

DevNotes

Related:

approxConvBelief, solveFactorParametric, RoME.MutablePose2Pose2Gaussian

source
RoME.MutablePose2Pose2GaussianType
mutable struct MutablePose2Pose2Gaussian <: AbstractManifoldMinimize

Specialized Pose2Pose2 factor type (Gaussian), which allows for rapid accumulation of odometry information as a branch on the factor graph.

source

Additional Notes

This will be consolidated with text above:

  • regardless of slam solution going on in the background, you can then just call val = accumulateFactorMeans(fg, [:x0deadreckon_x0f1])

for a new dead reckon tether solution;

  • you can add as many tethers as you want.
  • So if you solving every 10 poses, you just add a new tether x0, x10, x20, x30...
  • as the solves complete on previous segments, then you can just get the latest accumulateFactorMean