Finishing touch for issue 555.
Cleaned up the code a little bit for better readability.
An important fix when correction is needed (container offset).
Move out of the loop when the iterator is deleted in the loop.