Require: Frame I: Initialization Frame
Frame n: Subsequent Frame
: Feature points of Frame I
: Feature points of previous frames corresponding to
: Optimized rotation matrices of previous frames
: Optimized translation vectors of previous frames
: Initialized Map Points
Ensure: Map points and Initialization Evaluator .
Extract feature points of Frame n and match with to obtain matched feature points .
Optimize the Reprojection Error with Subsequent BA and thereby obtain optimized , , .
Calculate current evaluator value .
Stop iteration, and return , , as initialization results.
ifis too big then
Stop iteration, and report failure of initialization.
Algorithm 2: Error reduction with subsequent BA.