Rewrite using C++ interface of openCV