Node highlighting addition to previous pull request

[Nosille]

Sorry about the messy code in my previous PR. We have made multiple changes to Rtabmap over the last few years (often in a very hacky fashion), and I am trying to clean up and create pull requests for some of that code. However, as you noted, I inadvertently managed to grab a couple of pieces of code unrelated to the feature I was trying to share with you in the last PR. I appreciate your willingness to work with me anyway. I really like how it turned out, and I will try to be more careful in the future.

Along those lines, here is some code to properly highlight the graph poses that are currently selected.

[matlabbe]

Did some refactors and fixed this issue (highlighting not reset after changing node):

Thanks again!