Algorithm 1 Monocular SLAM with DI-D feature initialization (vid, refpose).

vid sequential image video

refpose initial reference points

begin

k: = 0

(x̂0, P0): = Initialize (vid.FirstFrame(), refpose)

while true do

img: = vid.NextFrame()

(x̂k+1, Pk+1): = StatePrediction(x̂k, Pk)

(hk, ∇Hk): = MeasurementPrediction(x̂k+1, Pk+1)

(zk, Sk): = Matching&Validation (hi, ∇Hi, Pk+1, img)

(x̂k, Pk): = Update (x̂k+1 Pk+1, hk, zk, Sk)

(x̂k, Pk): = AddFeatures(x̂k, Pk, img)

k: = k + 1

end while

end