Landmark initialization
We need a better way to initialize a new landmark. This issue proposes the following algorithm
- Check for pre-conditions:
- feature is not associated to any lmk
- feature track is long enough
- feature track spans at least two keyframes
- Given 2 keyframes, first and last in track:
- Recover 2 cv::keypoints pi
- Compute 2 projection matrices Pi = [K 0] [Ri Ti ; 0 1]
- Compute lmk point x = cv::triangulatePoints(std::vector(pi), std::vector(Pi))
- Given N keyframes in track: for each i=1..N
- Compute errors ei = pi - pinhole(Ti, qi, x)
- Compute Jacobians Ji = dei/dx
- Compute hessian H = sum Ji.tr * Ji
- Compute rhs vector b = sum Ji.tr ei
- check numerical conditioning of H. If pass:
- Compute dx = - H.inv * b
- Update x = x + dx
- Initialize landmark at position x
Edited by Joan Solà Ortega