Fixed null iterator error

When calling erase on a set it invalidates all of the iterators pointing to the removed element. For compilers that strictly adhere to the c++ standard this will insert a random double into the set. 

This issue has been fixed by inserting elements before calling the erase function.