Fixed bug where new points were twice added

No real performance improvement, but it may influence the effectiveness
of matching between frames.