Harris feature point extraction C++