fix bug that caused the feature point of the wrong iteration to be stored
fix bug that caused the feature point of the wrong iteration to be stored

tudelft3d/masbcpp,jeffcoukell/masbcpp,KevinWiebe/masbcpp,jeffcoukell/masbcpp,KevinWiebe/masbcpp,tudelft3d/masbcpp