Fix addCorrespondences with std::vector
Fix number of lines drawn and color of the lines
Use sensor pose information when drawing correspondences
Handle correspondences not set
Simplify updateCorrespondences