Position drift - Use map fix

[AlexReimann]

Instead of using the last frame, the points in the map should be used:

-Estimate the new position (or first just use the old one)
-Select points in the view of the new position of the map (or first just use the whole map)
-Use this to as oldObservation / depth image at t-1 to estimate the transformation