Merge pull request #1007 from VictorLamoine/ensenso_pr

Ensenso improvments
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